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=6.47*pi*(0.002)^2+0.001; 
D=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=60e-3;

Flc=3.2693;
F_LVDT=4.53;

%Tolleranza sulla velocitÃ 
toll_v=0.0003;

%Parametri attrito
delta=1.5;
vs=0.06; 
Fs=6; 
Fc1=2.3; 
c1=10; 
Fc2=2.3; 
c2=10; 


V0A=VTubiA;
% V0B=VTubiB*0.01;
V0B=(s-l1)*Ab*0.2;

%%
load 'ps2_V05_dV1_CameraB_chiusa_3.txt'
ps1_V05_dV1_CameraB_chiusa_1=ps2_V05_dV1_CameraB_chiusa_3;
Ps=Pa+0.2*1e6;

PA=ps1_V05_dV1_CameraB_chiusa_1(:,1).*1e5+Pa; %Pa .*0.1    
PB=ps1_V05_dV1_CameraB_chiusa_1(:,2).*1e5+Pa; %Pa .*0.1    
QA=-ps1_V05_dV1_CameraB_chiusa_1(:,3);
QB=-ps1_V05_dV1_CameraB_chiusa_1(:,4);
Flc=ps1_V05_dV1_CameraB_chiusa_1(:,5);
X=ps1_V05_dV1_CameraB_chiusa_1(:,6)*1e-3; %Posizione, m
Xd=ps1_V05_dV1_CameraB_chiusa_1(:,7)*0.2601; %Velocità m/s
V=ps1_V05_dV1_CameraB_chiusa_1(:,8);
PB_clean=smooth(PB,20);

time=(0:size(PA,1)-1)*1/fs;

[t,x] = ode45(@(t,x)Function(t,x,Aa,Ab,V0B,l1,s,F_LVDT,m_T,vs,delta,toll_v,c1,Fc1,Fs,c2,Fc2,time,cd_ISO,b,bS,Rg,Ts,Pa,V,Ps,coeff,V0A),time,[PA(1);PB(1); X(1);Xd(1)]);
%%

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 
    gB=0;
    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; 
    
 elseif V_mod==5 
    gA=0;
    gB=0;   
    
else 
    gB=0;
    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;
    

end

GA(i)=gA*coeff;
GB(i)=gB*coeff*0;
 
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


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)];

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)

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(time(1:9:end),Xd(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:10:end),PB(1:10:end),'bo','LineWidth',1.5,'markersize',10),hold on
plot(t,x(:,2),'r','LineWidth',4)
ylabel('P$_{f}$ [$Pa$]','interpreter','latex'),xlabel('t [$s$]','interpreter','latex')
legend('P$_{f}$ Exp. Data','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:10:end),PA(1:10:end),'bo','LineWidth',1.5,'markersize',10),hold on
plot(t,x(:,1),'r','LineWidth',4)
ylabel('P$_{r}$ [$Pa$]','interpreter','latex'),xlabel('t [$s$]','interpreter','latex')
legend('P$_{r}$ Exp. Data','P$_{r}$ 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:20:end),Fiexp(1:20:end),'bo','LineWidth',1.5,'markersize',10),hold on
plot(time,Fi,'b','LineWidth',4),hold on
plot(time(1:20:end),Fpexp(1:20:end),'ro','LineWidth',1.5,'markersize',10),hold on
plot(time,Fp,'r','LineWidth',4),hold on
plot(time(1:20:end),Ffexp(1:20:end),'ko','LineWidth',1.5,'markersize',10),hold on
plot(time,F_att,'k','LineWidth',4)
ylabel('[$N$]','interpreter','latex'),xlabel('t [$s$]','interpreter','latex')
% xlim([time1(1) time1(end)])
legend('$F_{i}$ Exp. Data','$F_{i}$ Num.','$F_{p}$ Exp. Data','$F_{p}$ Num.','$F_{f}$ Exp. Data','$F_{f}$ Num.','FontSize',20,'interpreter','latex')
grid on,zoom on,  
set(gca,'FontName','Times New Roman'),set(gca,'FontSize',25)
