clear all
close all
clc

global a
a=0;

Ts=293;              %Temperatura ambiente durante la prova [K]
k=1.4;               % Rapporto calori specifici
Rg=287.053;          %Costante dell'aria [J/kgK]
Pa=0.101325*1e6;          %Pressione ambiente durante la prova
P0=Pa;
rho=Pa/(Rg*Ts);
cd_ISO=0.87;
b=0.46;
bS=1/0.4;
Ps=0.45*1e6+Pa;
R0=0.72e-3;
cG=60000/rho;
fs=1000;
k=1.4;
dt=1/fs;
VTubiA=0.52*pi*(0.002)^2;
VTubiB=0.4795*pi*(0.002)^2;
D=0.97*16e-3;        
d=6e-3;         
Aa=pi*D^2/4;
Ab=pi*(D^2-d^2)/4;
m_T=(0.75+0.13);  
coeff=0.67;

s=500e-3;
l1=0e-3;
l1=l1+60e-3;

Flc=3.2693;
F_LVDT=4.53; 

%COEFFICIENTI ATTRITO     
load Sinusoide_Carrello_Amp1_freq_1_exp_1
delta=Coeff1_1_1(1,1);
vs=Coeff1_1_1(1,2);
Fs=Coeff1_1_1(1,3);
Fc1=Coeff1_1_1(1,4);
c1=Coeff1_1_1(1,5);
Fc2=Coeff1_1_1(1,6);
c2=Coeff1_1_1(1,7);

toll_v=0.0003;

load 'Sine_1Hz_fs1kHz_49bar_A1_1.txt';
Sine_1Hz_fs1kHz_4bar_A1=Sine_1Hz_fs1kHz_49bar_A1_1;

PA=Sine_1Hz_fs1kHz_4bar_A1(:,1).*1e5+Pa;  
PB=Sine_1Hz_fs1kHz_4bar_A1(:,2).*1e5+Pa; 
QA=-Sine_1Hz_fs1kHz_4bar_A1(:,3);
QB=-Sine_1Hz_fs1kHz_4bar_A1(:,4);
Flc=Sine_1Hz_fs1kHz_4bar_A1(:,5);
X=Sine_1Hz_fs1kHz_4bar_A1(:,6)*1e-3; %Posizione, m
Xd=Sine_1Hz_fs1kHz_4bar_A1(:,7)*0.2601; %Velocità m/s
time=(0:size(PA,1)-1)*1/fs;

VstartA=Aa*X(1,1);
VstartB=Ab*(0.5-X(1,1));

V0A=VstartA*0+VTubiA;
V0B=VstartB*0+VTubiB*0.1;

Vel=diff(smooth(X,15))/dt;
tVel=time(1:end-1)+dt/2; 
tVel=[tVel tVel(end)+dt];
Vel=[Vel; Vel(end)];

Acc=diff(smooth(Xd,20))/dt;
Acc=[Acc; Acc(end)]; 
 V=5.0-sin(2*pi*1*time)+0.022;
V1=V(1:10000)+0.07*0;
time1=time(1:10000);
[t,x]=ode45(@(t,x)Function(x,Aa,Ab,V0A,V0B,Rg,Ts,F_LVDT,m_T,Pa,Ps,cd_ISO,V1,b,bS,time1,t,vs,c1,c2,Fc1,Fc2,delta,toll_v,Fs,s,l1,coeff),time1,[PA(1); PB(1); X(1);Xd(1)]);
%% Ricalcolo Risultati numerici
                              
U= interp1(time,V,t);
Vel_mod=x(:,4);
Acc_mod=diff(Vel_mod)/(t(2)-t(1));
Acc_mod=[Acc_mod; Acc_mod(end)];

for i=1:length(Vel_mod)
    
V_mod=U(i);
if V_mod>5 
    A_eqmanS=abs((-4.929e-07)*(10-V_mod)+2.372e-06);
    cond=A_eqmanS<0;
    A_eqmanS(cond)=0;
    cond=x(1)/Pa<=bS; %subsonico
    gA(cond)=-cd_ISO*sqrt(293/Ts)*0.686/sqrt(Rg*Ts)*x(1)*A_eqmanS*sqrt(1-((x(1)/Pa-bS)/(1-bS))^2); 
    cond=x(1)/Pa>bS; %sonico
    gA(cond)=-cd_ISO*sqrt(293/Ts)*0.686/sqrt(Rg*Ts)*x(1)*A_eqmanS; 
    % 4 CARICO
    A_eqman=abs((3.869e-07)*V_mod-1.998e-06);
    cond=A_eqman<0;
    A_eqman(cond)=0;
    cond=x(2)/Ps>=b; %subsonico
    gB(cond)=cd_ISO*sqrt(293/Ts)*0.686/sqrt(Rg*Ts)*Ps*A_eqman*sqrt(1-((x(2)/Ps-b)/(1-b))^2);
    cond=x(2)/Ps<b; %sonico
    gB(cond)=cd_ISO*sqrt(293/Ts)*0.686/sqrt(Rg*Ts)*Ps*A_eqman;

 elseif V_mod==5
    gA=0;
    gB=0;   
    
else 
    A_eqman=abs((-3.953e-07)*V_mod+1.913e-06);
    cond=A_eqman<0;
    A_eqman(cond)=0;
    cond=x(1)/Ps>=b; %subsonico
    gA(cond)=cd_ISO*sqrt(293/Ts)*0.686/sqrt(Rg*Ts)*Ps*A_eqman*sqrt(1-((x(1)/Ps-b)/(1-b))^2);
    cond=x(1)/Ps<b; %sonico
    gA(cond)=cd_ISO*sqrt(293/Ts)*0.686/sqrt(Rg*Ts)*Ps*A_eqman;
    % 4 SCARICO
    A_eqmanS=abs((5.084e-07)*(10-V_mod)-2.637e-06);
    cond=A_eqmanS<0;
    A_eqmanS(cond)=0;
    cond=x(2)/Pa<=bS; %subsonico
    gB(cond)=-cd_ISO*sqrt(293/Ts)*0.686/sqrt(Rg*Ts)*x(2)*A_eqmanS*sqrt(1-((x(2)/Pa-bS)/(1-bS))^2);
    cond=x(2)/Pa>bS; %sonico
    gB(cond)=-cd_ISO*sqrt(293/Ts)*0.686/sqrt(Rg*Ts)*x(2)*A_eqmanS; 

end

GA(i)=gA*coeff;
GB(i)=gB*coeff;

Fp(i)=(x(i,1)-Pa)*Aa-(x(i,2)-Pa)*Ab;
v=Vel_mod(i);
%1)Caso di velocitÃ  nulla
if abs(v)-toll_v<=0  
cond=abs(Fp(i)-F_LVDT)<Fs;
Ff(cond)=Fp(i)-F_LVDT; 
cond=abs(Fp(i)-F_LVDT)>=Fs;
Ff(cond)=Fs*sign(Fp(i)-F_LVDT);

else
%2a) VelocitÃ  crescenti
cond=Acc_mod(i)>=0;    
Ff(cond)=c1*v+(Fc1+(Fs-Fc1)*exp(-(abs(v)./vs).^delta)).*sign(v);
%2a) VelocitÃ  decrescenti
cond=Acc_mod(i)<0;   
Ff(cond)=c2*v+Fc2*sign(v);

end

F_att(i)=(Ff);  
Fi(i)=(Fp(i)-F_att(i)-F_LVDT);

end

Fiexp=m_T*Acc;
Fpexp=(PA-Pa)*Aa-(PB-Pa)*Ab;
Ffexp=Fpexp-Fiexp-F_LVDT; 

figure
plot(Vel(1:1500),Ffexp(1:1500),'b','linewidth',4),hold on
plot(x(:,4),F_att,'r','linewidth',4)
xlabel('Velocity [$m/s$]','interpreter','latex'),ylabel('Friction Force [$N$]','interpreter','latex')
legend('Exp. Data','Num.')
grid on, zoom on,
set(gca,'FontName','Times New Roman'),set(gca,'FontSize',25) %set(gca,'FontSize',32)

figure
plot(time(1:7:end),PA(1:7:end),'bo','LineWidth',1.5,'markersize',10),hold on
plot(time(1:10:end),PB(1:10:end),'ro','LineWidth',1.5,'markersize',10),hold on
plot(t,x(:,1),'g','LineWidth',4),hold on
plot(t,x(:,2),'y','LineWidth',4)
ylabel('P [$Pa$]','interpreter','latex'),xlabel('t [$s$]','interpreter','latex')
legend('P$_{r}$ Exp. Data','P$_{f}$ Exp. Data','P$_{r}$ Num.','P$_{f}$ Num.','FontSize',20,'interpreter','latex')
xlim([time1(1) time1(end)])
grid on,zoom on,
set(gca,'FontName','Times New Roman'),set(gca,'FontSize',25) 

figure
plot(time(1:12:end),X(1:12:end),'bo','LineWidth',1.5,'markersize',10),hold on
plot(t,x(:,3),'r','LineWidth',4)
ylabel('x [$m$]','interpreter','latex'),xlabel('t [$s$]','interpreter','latex')
legend('Exp. Data','Num.')
xlim([time1(1) time1(end)])
grid on,zoom on,
set(gca,'FontName','Times New Roman'),set(gca,'FontSize',25)

figure
plot(t,GA*cG,'b','linewidth',4),hold on
plot(time,QA,'bo','linewidth',4),hold on
plot(t,GB*cG,'r','linewidth',4),hold on
plot(time,QB,'ro','linewidth',4),hold on
ylabel('Q [$l/min\ ANR\ 20^\circ \ C$]','interpreter','latex'),xlabel('t [$s$]','interpreter','latex')
xlim([time1(1) time1(end)])
grid on,zoom on,  
set(gca,'FontName','Times New Roman'),set(gca,'FontSize',25)
legend('$Q_{A}$ Num.','$Q_{A}$ Exp. Data','$Q_{B}$ Num.','$Q_{B}$ Exp. Data','FontSize',20,'interpreter','latex')


figure
plot(tVel(1:9:end),Vel(1:9:end),'bo','LineWidth',1.5,'markersize',10),hold on
plot(t,x(:,4),'r','linewidth',4)
ylabel('$\dot{x}$ [$m/s$]','interpreter','latex'),xlabel('t [$s$]','interpreter','latex')
xlim([time1(1) time1(end)])
grid on,zoom on
legend('Exp. Data','Num.')
set(gca,'FontName','Times New Roman'),set(gca,'FontSize',25)

figure
plot(time(1:5000),smooth(Acc(1:5000),1),'bo','linewidth',2), hold on
plot(t(1:5000),Acc_mod(1:5000),'r','linewidth',4)
ylabel('$\ddot{x}$ [$m/s^2$]','interpreter','latex'),xlabel('t [$s$]','interpreter','latex')
grid on,zoom on,
legend('Exp. Data','Num.')
set(gca,'FontName','Times New Roman'),set(gca,'FontSize',25)

figure
plot(time(1:5000),Fiexp(1:5000),'bo','linewidth',4), hold on
plot(t(1:5000),Fi(1:5000),'r','linewidth',4)
ylabel('$F_{i}$ [$N$]','interpreter','latex'),xlabel('t [$s$]','interpreter','latex')
grid on,zoom on,
legend('Exp. Data','Num','interpreter','latex')
set(gca,'FontName','Times New Roman'),set(gca,'FontSize',25)

figure
plot(time(1:5000),Fpexp(1:5000),'bo','linewidth',4),hold on
plot(t(1:5000),Fp(1:5000),'r','linewidth',4)
ylabel('$F_{p}$ [$N$]','interpreter','latex'),xlabel('t [$s$]','interpreter','latex')
grid on,zoom on,
legend('Exp. Data','Num','interpreter','latex')
set(gca,'FontName','Times New Roman'),set(gca,'FontSize',25)

figure
plot(time(1:6:10000),Ffexp(1:6:10000),'bo','LineWidth',1.5,'markersize',10),hold on
plot(t(1:end),F_att(1:end),'r','linewidth',4)
ylabel('$F_{friction}$ [$N$]','interpreter','latex'),xlabel('t [$s$]','interpreter','latex')
grid on,zoom on,
legend('Exp. Data','Num.','interpreter','latex')
set(gca,'FontName','Times New Roman'),set(gca,'FontSize',25)
