clear all
% Define Global variables 
global mass C_damp Ksys u_friction C_d A massA t_exp y_exp rho_w error count paramsResults y_num AddedMass

% Experimental data
ExpDataFileName = 'C:\Users\romerolj\OneDrive - Oregon State University\Jorge\Research\H. Yeh\DataHWRL\LongSpring\Results_LS\h222\Trial09Comb_data.txt';
Data = readmatrix(ExpDataFileName,'Delimiter',',');
t_exp = Data(:,1);
y_exp = Data(:,2);

% % Numerical Data
% NumData = 'C:\Users\romerolj\OneDrive - Oregon State University\Jorge\Research\H. Yeh\DataHWRL\LongSpring\Results_LS\h257\Trial09NumericalIntegration_Data.txt'
% Data = readmatrix(NumData,'Delimiter',',')
% mass = Data(1,1);
% Ksys = Data(1,2);
% Ktrial = Data(1,3);
% zeta = Data(1,4);
% C_damp = Data(1,5);
% u_friction = Data(1,6);
% A = 0.97;
% rho_w = 1000;
% count = 0 

% Using data from dry trials anmd added mas from CH instant method and
% experimental acceration recordings
mass = 2637.4;
Ksys = 109592;
A = 0.97;
rho_w = 1000;
count = 0 




ResSummaryLS = [0.115606590887840	12796.4067116950	0.0117743622217990	399.894032663982	0.00723395351946868	110431.924071737];

% Find the values for damping and friction for each trial based on th
% einitial displacement 
zeta =     ResSummaryLS(3);
C_damp =  ResSummaryLS(4);
u_friction = ResSummaryLS(5);
AddedMass = 2413.77632692315;

MeanPeriod = 1.0458;
AddedMass = MeanPeriod^2*(1-zeta^2)*Ksys/(4*pi^2)-mass



% Global search
hold on 
% Syntax used                opt                       function         intial guess         lower bound     upper bound        
problem = createOptimProblem('fmincon', 'objective', @objectiveFunction, 'x0', [ 0.001, 1], 'lb', [0 0], 'ub', [3000 100]);
problem.options = optimoptions(@fmincon,'Algorithm','sqp','MaxIterations',100, 'OutputFcn', @myoutput);
gs = GlobalSearch;
[result, error] = run(gs, problem);

hold off 




% Retrieve the best solution
Fluid_damping = result(1);
dragCoefficient = result(2);

% disp(['Error: ', num2str(error)]);

% if error < 0.08
%     % Display the results
%     disp(['Added Mass: ', num2str(addedMass)]);
%     disp(['Drag Coefficient: ', num2str(dragCoefficient)]);
%     disp(['Error: ', num2str(error)]);
%     figure 
%     hold on 
%     plot(0:0.001:(size(ynum)-1)*0.001,ynum,DisplayName='Numerical',Color='k',LineStyle=':')
%     plot(0:0.001:(size(y_exp)-1)*0.001,y_exp,DisplayName='Experimental', Color='r');
%     legend()
%     hold off 
%     stop = true
% 
% end
% Runge-Kutta function

% figure()
% scatter(paramsResults(:,1),paramsResults(:,3), Marker=".")
% xlabel('Added Mass m_a')
% ylabel('Error MSE')
% set(gca,fontsize=16)
% 
figure()
scatter(paramsResults(:,1),paramsResults(:,2), Marker=".")
xlabel('Drag Coefficient C_d')
ylabel('Error MSE')
set(gca,fontsize=16)

% figure()
% scatter3(paramsResults(:,1),paramsResults(:,2),paramsResults(:,3), Marker=".")
% % surf(paramsResults(:,1),paramsResults(:,2),paramsResults(:,3))
% ylabel('Drag Coefficient C_d')
% xlabel('Added Mass m_a')
% zlabel('Error MSE')
set(gca,fontsize=16)

% AddedMass = paramsResults(1,1);
% DragCoefficient = paramsResults(1); 

AddedMass = 1992 ;
DragCoefficient = 31.2; 


 % DragCoefficient = 0;
y_num = rungeKuttaSolver(Ksys, y_exp, AddedMass, DragCoefficient);
y_num = y_num';
figure 
txt = sprintf(['m = %2.1f kg   k_{Sys} = %2.1f kN/m  m_a = %2.1f kg  C_d= %2.5f  '], ...
mass, Ksys/1000,AddedMass , DragCoefficient);
title(txt,'fontweight','normal')
hold on 
plot(0:0.001:(size(y_num)-1)*0.001,y_num,DisplayName='Numerical', Color='r',LineWidth=2);
plot(0:0.001:(size(y_exp)-1)*0.001,y_exp,DisplayName='Experimental',Color='k',LineStyle=':',LineWidth=2)
ylabel(['Displacement [m]'])
xlabel('Time [s]')
legend()
hold off 
box on 

% MeanPeriod = 0.624;
AddedMass = MeanPeriod^2*(1-zeta^2)*Ksys/(4*pi^2)-mass
% AddedMass = paramsResults(1,1);
DragCoefficient = 0 
 % DragCoefficient = 19.5;
y_num = rungeKuttaSolver(Ksys, y_exp, AddedMass, DragCoefficient);
y_num = y_num';
figure 
txt = sprintf(['m = %2.1f kg   k_{Sys} = %2.1f kN/m  m_a = %2.1f kg  C_d= %2.5f  '], ...
mass, Ksys/1000,AddedMass , DragCoefficient);
title(txt,'fontweight','normal')
hold on 
plot(0:0.001:(size(y_num)-1)*0.001,y_num,DisplayName='Numerical', Color='r',LineWidth=2);
plot(0:0.001:(size(y_exp)-1)*0.001,y_exp,DisplayName='Experimental',Color='k',LineStyle=':',LineWidth=2)
ylabel(['Displacement [m]'])
xlabel('Time [s]')
% grid on 
legend()
hold off 
box on 




AddedMass = MeanPeriod^2*(1-zeta^2)*Ksys/(4*pi^2)-mass
% AddedMass = paramsResults(1,1);
DragCoefficient = 7.;

%Using analytical Soulition for decay 
M = (mass+AddedMass);
Wn = (Ksys/M)^0.5;
X0 = y_exp(1);
vol_Bridge = 1.098;
grav = 9.81;
TimeVector = 0.001:0.001:length(y_exp)*0.001;

X_amp_decay = (X0-(u_friction*(mass-vol_Bridge*rho_w)*grav/Ksys).*(exp(zeta*Wn.*TimeVector)-1)*coth(2*pi*zeta/(4*(1-zeta^2)^0.5))).*exp(-zeta*Wn*TimeVector)
zeta1 = 0.05
X_amp_decay2 = (X0-(u_friction*(mass-vol_Bridge*rho_w)*grav/Ksys).*(exp(zeta1*Wn.*TimeVector)-1)*coth(2*pi*zeta1/(4*(1-zeta1^2)^0.5))).*exp(-zeta1*Wn*TimeVector)
Aux1 = X_amp_decay2>0;
X_amp_decay2 = X_amp_decay2 .*Aux1;

[yupper,ylower] = envelope(y_exp,1000,'peak');

 % DragCoefficient = 19.5;
y_num = rungeKuttaSolver(Ksys, y_exp, AddedMass, DragCoefficient);
y_num = y_num'*Ksys;
figure 
txt = sprintf(['m = %2.1f kg   k_{Sys} = %2.1f kN/m  m_a = %2.1f kg  C_d= %2.5f  '], ...
mass, Ksys/1000,AddedMass , DragCoefficient);
title(txt,'fontweight','normal')
hold on 
plot(0:0.001:(size(y_num)-1)*0.001,(y_num)/1000,DisplayName='Numerical', Color='r',LineWidth=2);
plot(0:0.001:(size(y_exp)-1)*0.001,(y_exp*Ksys)/1000,DisplayName='Experimental',Color='k',LineStyle=':',LineWidth=2)
% plot(0:0.001:(size(y_exp)-1)*0.001,(y_exp*Ksys-y_num)/1000,DisplayName='Residual',Color='b',LineStyle='-.',LineWidth=2)
plot(TimeVector,X_amp_decay*Ksys/1000,DisplayName=['Envelope \zeta = ' num2str(round(zeta,4))],Color='c',LineStyle='-.',LineWidth=2)
plot(TimeVector,X_amp_decay2*Ksys/1000,DisplayName=['Env. Analytical Sol \zeta = ' num2str(zeta1)],Color='m',LineStyle='-.',LineWidth=2)

plot(TimeVector(3:end-1000),yupper(3:end-1000)*Ksys/1000,DisplayName=['Envelope Experimental'],Color=[.7 .7 .7],LineStyle='-.',LineWidth=2)
ax = gca; 
ax.FontSize = 11;
text(5,-12, [ '\zeta = ' num2str(zeta1)],Color='m')
ylabel(['Forces [kN]'])
xlabel('Time [s]')
legend()

hold off 




[yupper_Numerical,ylower_Numerical] = envelope(y_num/Ksys,1000,'peak');
yupper_Numerical_Normalized = yupper_Numerical./yupper;
yupper_Analytical_Cf_Normalized = X_amp_decay'./yupper;
yupper_Analytical_Cd_Normalized =  X_amp_decay2'./yupper;

limit  = 3000;
figure 
hold on
% plot(TimeVector,X_amp_decay*Ksys/1000,DisplayName=['Envelope \zeta = ' num2str(round(zeta,4))],Color='c',LineStyle='-.',LineWidth=2)
plot(TimeVector(3:end-limit),yupper_Analytical_Cd_Normalized(3:end-limit),DisplayName=['Envelope Numerical C_f'],Color='m',LineStyle='-.',LineWidth=2)
% plot(TimeVector(3:end-1500),yupper_Analytical_Cf_Normalized(3:end-1500),DisplayName=['Envelope Numerical C_f'],Color=[.7 .7 .7],LineStyle='-.',LineWidth=2)
plot(TimeVector(3:end-limit),yupper_Numerical_Normalized(3:end-limit),DisplayName=['Envelope Numerical C_d'],Color=[.5 .25 .5],LineStyle='-.',LineWidth=2)
yline(1,'k-','Relative  / Experimental =1 ','HandleVisibility','off')
ax = gca; 
ax.FontSize = 11;
text(5,-12, [ '\zeta = ' num2str(zeta1)],Color='m')
ylabel(['Numerical Envelope / Experimental Envelope [-]'])
xlabel('Time [s]')
ylim([0.5 1.5])
legend()
hold off 

Using Added Mass from the mean period of the motion in the first four cycles

% Using damping C from the longest initial displacement from dry tests and
% optimizing the solution for C_d and C_f where C_f comes from the linear
% viscous damping term in the EOM
AddedMass = MeanPeriod^2*(1-zeta^2)*Ksys/(4*pi^2)-mass

% Global search
hold on 
% Syntax used                opt                       function         intial guess         lower bound     upper bound        
problem = createOptimProblem('fmincon', 'objective', @objectiveFunction, 'x0', [1, 1], 'lb', [0 0], 'ub', [8000 100]);
problem.options = optimoptions(@fmincon,'Algorithm','sqp','MaxIterations',100, 'OutputFcn', @myoutput);
gs = GlobalSearch;
[result, error] = run(gs, problem);

hold off 




% Retrieve the best solution
Fluid_damping = result(1);
dragCoefficient = result(2);


Damp_f = paramsResults(1,1);
DragCoefficient = paramsResults(1,2) ;



 % DragCoefficient = 0;
y_num = rungeKuttaSolver(Ksys, y_exp, Damp_f, DragCoefficient);
y_num = y_num';
figure 
txt = sprintf(['m = %2.1f kg   k_{Sys} = %2.1f kN/m  C_f = %2.1f [N/s]  C_d= %2.5f  '], ...
mass, Ksys/1000,Damp_f , DragCoefficient);
title(txt,'fontweight','normal')
hold on 
plot(0:0.001:(size(y_num)-1)*0.001,y_num,DisplayName='Numerical', Color='r',LineWidth=2);
plot((0:0.001:(size(y_exp)-1)*0.001)+0.036,y_exp,DisplayName='Experimental',Color='k',LineStyle=':',LineWidth=2)
ylabel(['Displacement [m]'])
xlabel('Time [s]')
legend()
hold off 

% Sort rows of paramsResults based on the 3rd column
paramsResults = sortrows(paramsResults, 3);

Damp_f = paramsResults(1,1);
DragCoefficient = paramsResults(1,2) ;


 % DragCoefficient = 0;
y_num = rungeKuttaSolver(Ksys, y_exp, Damp_f, DragCoefficient);
y_num = y_num';
figure 
txt = sprintf(['m = %2.1f kg   k_{Sys} = %2.1f kN/m  m_a = %2.1f kg  C_d= %2.3f  C_f = %2.1f N/s'], ...
mass, Ksys/1000,AddedMass , DragCoefficient, Damp_f);
title(txt,'fontweight','normal')
timeVectorNum = 0:0.001:(size(y_num)-1)*0.001
timeVectorExp = 0:0.001:(size(y_exp)-1)*0.001
hold on 
plot(timeVectorNum',y_num,DisplayName='Numerical', Color='r',LineWidth=2);
plot(timeVectorExp',y_exp,DisplayName='Experimental',Color='k',LineStyle=':',LineWidth=2)
ylabel(['Displacement [m]'])
xlabel('Time [s]')
legend()
hold off 
Matrix2Write = [timeVectorNum',y_num];
writematrix(Matrix2Write,'NumericalH222LS.txt','Delimiter',',')

Matrix2Write = [timeVectorExp',y_exp];
writematrix(Matrix2Write,'ExperimentalH222LS.txt','Delimiter',',')

function NI = rungeKuttaSolver(Ksys, y_exp, C_fluid, DragCoefficient)
    global mass C_damp Ksys u_friction t_exp y_exp AddedMass

    % Numerical Integration Script
     
    % Initial Diplacement
    Initial_disp = (y_exp(1));
    
    % >>>  Number of grid points 
    N = 8000;
    
    % >>>  t variable
    tMin = 0;
    tMax = 8;
    
    % >>>  Initial value for x and y variable
    xx = zeros(1,N); y = zeros(1,N); 
    xx(1) = 0;
    yy(1) = Initial_disp;
    
    
    
    % SETUP  ==============================================================  
    % t interval and step size
    t = linspace(tMin,tMax,N);
    h = t(2) - t(1);
    % xDot & yDot variables as a function of t
    %      at x(nX) & y(nY) (calls function XDOT & YDOT)   
    nX = 1; nY = 1;
    xDot = XDOT(t,xx(nX), yy(nY),C_fluid,DragCoefficient);
    yDot = YDOT(t,xx(nX),yy(nY));
    
    
    % Compute y and yDdot values as a function of time =====================
    for n = 1 : N-1
       kx1 = h*XDOT(t(n), xx(n), yy(n),C_fluid,DragCoefficient);
       ky1 = h*YDOT(t(n), xx(n), yy(n));
       
       kx2 = h*XDOT(t(n) + h/2, xx(n) + kx1/2, yy(n) + ky1/2,C_fluid,DragCoefficient);
       ky2 = h*YDOT(t(n) + h/2, xx(n) + kx1/2, yy(n) + ky1/2);
       
       kx3 = h*XDOT(t(n) + h/2, xx(n) + kx2/2, yy(n) + ky2/2,C_fluid,DragCoefficient);
       ky3 = h*YDOT(t(n) + h/2, xx(n) + kx2/2, yy(n) + ky2/2);
       
       kx4 = h*XDOT(t(n) + h,   xx(n) + kx3,   yy(n) + ky3,C_fluid,DragCoefficient);
       ky4 = h*YDOT(t(n) + h,   xx(n) + kx3,   yy(n) + ky3);

       xx(n+1) = xx(n) + (kx1+2*kx2+2*kx3+kx4)/6;
       yy(n+1) = yy(n) + (ky1+2*ky2+2*ky3+ky4)/6;
    end
    
    
    
    
    % xDOT & yDot variables (calls functions XDOT YDOT) 
    xDot = XDOT(t,xx(1),yy(1),C_fluid,DragCoefficient);
    yDot = YDOT(t,xx(1),yy(1));
    
    
    % [2D] plots  =========================================================   
    Y = linspace(1,3,N);
    [tt, My] = meshgrid(t,Y);
    YYDot = YDOT(tt,My);

    % Return the numerical solution
    NI = yy;
    % figure 
    % hold on 
    % plot(t,NI)
    % plot(t,y_exp)
end



function f = XDOT(t,x,y,C_fluid,DragCoefficient)
  global mass C_damp Ksys u_friction t_exp y_exp AddedMass
  grav = 9.81;
  rho_w = 1000;
  vol_Bridge = 1.098;
  A = 0.97;
  f = (1/(mass+AddedMass))*(-(Ksys).*y - (C_damp+C_fluid).*x - sign(x).*u_friction*(mass*grav-vol_Bridge*rho_w*grav)-0.5*DragCoefficient*rho_w*A*x*abs(x));
 
end


function g = YDOT(t,x,y)
    g = x;
end




function plotfunction()
global y_exp y_num
    figure 
    hold on 
    plot(0:0.001:(size(y_num)-1)*0.001,y_num,DisplayName='Numerical',Color='k',LineStyle=':')
    plot(0:0.001:(size(y_exp)-1)*0.001,y_exp,DisplayName='Experimental', Color='r');
    legend()
    hold off 
end 


% Objective function
function [error] = objectiveFunction(params)
    global mass damp Ksys u_friction A t_exp y_exp y_num  count paramsResults error y_num AddedMass
    count = count + 1
    % Parameters
    Damp_f = params(1);
    drag_coefficient = params(2);
    

    % Time span
    tspan = [0, max(t_exp)];

    % Initial conditions
    y0 = [0; 0];

    % Numerical solution (mass, stiffness, friction, damping, tspan, y0)
    y_num = rungeKuttaSolver(Ksys, y_exp,Damp_f, drag_coefficient)';

    % Calculate the error
    error = sum((y_exp(1:5000) - y_num(1:5000)).^2);
    
    paramsResults(count,1) = Damp_f;
    paramsResults(count,2) = drag_coefficient;
    paramsResults(count,3) = error;

    if error < 0.015
        figure 
        hold on 
        plot(0:0.001:(size(y_num)-1)*0.001,y_num,DisplayName='Numerical',Color='k',LineStyle=':')
        plot(0:0.001:(size(y_exp)-1)*0.001,y_exp,DisplayName='Experimental', Color='r');
        legend()
        hold off 
        stop = true
    end


end


% Output function
function stop = myoutput
      global error
      stop = false;
      % Check if objective function is less than 0.09
      if error < 0.01
          error
          stop = true;
      end
      disp('manzana')
end
