clear all
close all
clc
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%ALTRI INPUT DA INSERIRE POI IN <EXCEL>
global pcav 
global Z
global w
global omega
global TC
global LC
global DR
global pa
global pD
global pm
global BM
global mu 
global BC


tic

Z = 12; %define number of teeth
w = 10; %define Tooth Width [mm]
TC = 0.0245; %define tooth tip clearance with casing [mm]
LC = 0.01; %define side tooth clearance with casing [mm]
BC = 0.0; %define bearing block clearance with casing sides [mm]
DR = 4.5; %define gear drain radius [mm]
AR = 0.95;  %define bearing aspect ratio [mm]
Lbear = AR*2*DR; %bearing length given radius and aspect radius [mm]
Clear = 0.0245; %define bearing internal clearance [mm]

RPM = 3350; % RPM
pa = 0e5; %[Pa]
pD = 0e5; %[Pa]
pm = 20e5; %[Pa]
BM = 1.4e9; %Bulk Modulus, [Pa]
pcav = 0e5; %Cavitation, [Pa]
rhocav = 818; %Cavitation, [kg/m3] 
mu = 14e-3; %[Pa*s]

omega = RPM * 2*pi/60; % [rad/sec]

%INPUT from .xlsx FILE 
filename = 'Gprofile.xlsx'; 
M = readmatrix(filename,'Sheet', 1, 'Range','A4:G4');
TH = M(1); TW_r = M(2); TW_t = M(3); GAP = M(4); VV = M(5); MR = M(6); MT = M(7);
M = readmatrix(filename,'Sheet', 2);
TR = M(:, 1); TW = M(:, 2);
filename = 'GPgeometry.xlsx'; 
M =  readmatrix(filename,'Sheet', 1, 'Range', 'A2');
TETA = M(:, 1); TV = M(:, 2); dVol = M(:, 3); UDA = M(:, 4); LDA = M(:, 5);
NoSV = M(:, 6); 
thcontact1 = M(:, 7); thcontact2 = M(:, 8); 
rcontact1 = M(:, 9); rcontact2 = M(:, 10);
thseal1 = M(:, 11); thseal2 = M(:, 12);
r2contact1 = M(:, 13); r2contact2 = M(:, 14);
dVol(isnan(dVol)) = [];  
rcontact2(end) = 10000; thcontact2(end) = 10000;  r2contact2(end) = 10000; 
% for ii = 2 : length(dVol)
%     if dVol(ii) == 0 && dVol(ii-1) ~= 0, dVol = dVol(1:ii-1); break, end
% end
dVol(isnan(dVol)) = [];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%ATTENZIONE STO SOVRASCRIVENDO VOLUME E SUA DERIVATA!!!!!!!!!!!!
va = VV/w; 
VV = 60.7; b = VV/va; fact = b/w; TR(end)=8.635; rprim = 6.9;

cdvol = polyfit( TETA(1 : length(dVol)), dVol, 1)  * fact;
cVol = polyfit( TETA(1 : length(dVol)), TV(1: length(dVol)), 2)  * fact;
dVdth = @(t) polyval(cdvol, t);
VolTr = @(t) polyval(cVol, t);

UDA = UDA(1 : length(dVol)); LDA = LDA(1 : length(dVol));
th = TETA( 1 : length(dVol));
Adel = @(t) makima(th, UDA, t); 
Asuc = @(t) makima(th, LDA, t);

% %%%%% !!!!!!ATTENZIONE !!!!!!! SOVRASCRIVO I DATI !!!!!
%%%% !!!!!!!!!!!!!!!!!!!! ATTENZIONE QUI !!!!!!!!!!!!!!!!!!!!!!!!!
UDA = [.45, .38, .32, .24, .19, .16, .13, .1, .07, .05, .026, .017, 0,0,0,0,0,0,0,0,0,0, 0,0 ]; 
LDA = [0,0,0,0,0,0,0,0,0,0, .024, .04, .06, .075, .1, .13, .16, .20, .24, .29, .35, .42, .5, .6];
th = 0:.5:11.5;
Adel = @(t) interp1(th, UDA, t);
Asuc = @(t) interp1(th, LDA, t); 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%
%TRAPPED VOLUME - PRESSURE EVOLUTION 

Qtin = @(t, p) (0.65*Adel(t).*(1e3*sqrt(2*abs(pm - p)./density(pm,BM,pcav,rhocav)))).*(pm>=p) + ...
               (-0.65*Adel(t).*(1e3*sqrt(2*abs(pm - p)./density(p,BM,pcav,rhocav)))).*(pm<p);
                                                   %[mm3/s] !!!!!!!!!!!!!!
Qtout = @(t, p) (0.65*Asuc(t).*(1e3*sqrt(2*abs(p-pa)./density(p,BM,pcav,rhocav)))).*(p>=pa) + ... 
                (-0.65*Asuc(t).*(1e3*sqrt(2*abs(p-pa)./density(pa,BM,pcav,rhocav)))).*(p<pa);    
                                                    %[mm3/s] !!!!!!!!!!!!!
Qtdr = @(t, p) (.5*TR(1)*2*pi/Z)*LC.^3/(12*mu*(TR(1)-DR))*(p-pD);
                                                   %[mm3/s] !!!!!!!!!!!!!!
Qtlin = @(t, p) TH*LC.^3/(12*mu*MT)*(pm - p) + .5*(TH*LC*omega*MR);
                                                   %[mm3/s] !!!!!!!!!!!!!!
Qtlout = @(t, p)  TH*LC.^3/(12*mu*MT)*(p - pa) +.5*(TH*LC*omega*MR);
                                                   %[mm3/s] !!!!!!!!!!!!!!
funTrap = @(t, p) pi/180 * (BM./VolTr(t)).*((2*Qtin(t, p) - 2*Qtout(t, p) - ...
                            4*Qtdr(t, p) + 2*Qtlin(t, p) - 2*Qtlout(t, p))./omega - dVdth(t));

options = odeset('Events', @pressureEventsFcn);

figure()
   %[tt, pt, tend, pte, ie] = ode15s(funTrap, [0, th(end)], pm , options);
   [tt, pt] = ode15s(funTrap, [0, th(end)], pm );
   plot(tt, pt)
   xlabel('\theta [deg]')
   ylabel('p_{trap}/p_m')
   hold on
   
   if tt(end) < th(end)
         plot( [tt(end), th(end)], [pcav, pcav], '-r')
   end
   
   hold off

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%SEALED VOLUMES - PRESSURE EVOLUTION 
cont = 0; index = -ones(10, 1); nsv = -ones(10, 1);
for ii = 1 : length(NoSV) - 1
    if NoSV(ii) ~=  NoSV(ii+1)
        cont = cont + 1;
        index(cont) = ii + 1;
        nsv(cont) = NoSV(ii);
    end 
    if ii == length(NoSV) - 1
        cont = cont + 1;
        index(cont) = ii + 1;
        nsv(cont) = NoSV(ii);
    end
end 
index(index==-1) = []; nsv(nsv==-1) = [];

repeat = 5;  %repeat on inner seal volume pressure evolution loop !!!
tseal = []; pseal = [];

%INITIAL GUESS ON BEARING STATIC EQUILIBRIUM POSITION (SEP)
max_count = 20; %max iterations for predicting Static Equilibrium Position (SEP)
iter = 1;
rel_err_e = 10000; %relative error on eccentricity magnitude
rel_err_G = 10000; %relative error on eccentricity direction angle

%xbear = -0.001; ybear = -0.023;  %initial guess on SEP
xbear = 0.000; ybear = -0.8*TC;  %initial guess on SEP
BEAR = [xbear, ybear, sqrt(xbear^2+ybear^2), rad2deg(mod(atan2(ybear, xbear), 2*pi))];
filename = 'BearingCenterCoord.xlsx';
string = ["xbear" "ybear" "e" "GAMMA"]; 
writematrix(string, filename, 'Sheet', 1, 'Range', 'A1')
writematrix(BEAR, filename, 'Sheet', 1, 'Range', 'A2')

while iter <= max_count && (rel_err_e >= 1e-04 || rel_err_G >= 1e-04)

%figure(200 + iter)
for jj = 1 : repeat 
    if nsv(1) ~= max(nsv)    % se schema N -> N+1 -> N  (es. 6 -> 7 -> 6 )
        for ii = 1 : length(index)
            if ii == 1 && jj == 1  %primo passo con guess iniziale
               thseal = [TETA(1), TETA(index(1))];
               CI =  pa * ones(nsv(ii), 1);    
               % CI =  yoshida(nsv(ii), pa);th1 = @(t) interp1(TETA, thseal1, t); 
               [ts, ps] = ode15s(@(t, p) odesealfcn(t,p, nsv(ii), index), [thseal(1), thseal(2)], CI);
               tseal = ts'; 
               pseal = [pa*ones(1, size(ps', 2)); ps'];
            elseif ii == 1 && jj > 1
               thseal = [TETA(1), TETA(index(1))];
               CI = pseal(1:end-1, end);
               [ts, ps] = ode15s(@(t, p) odesealfcn(t,p, nsv(ii), index), [thseal(1), thseal(2)], CI);
               tseal = ts';  
               pseal = [pa*ones(1, size(ps', 2)); ps'];  
            elseif ii == 2
               thseal = [TETA(index(ii-1)), TETA(index(ii))] ; 
               CI = pseal(:, end);
               [ts, ps] = ode15s(@(t, p) odesealfcn(t,p, nsv(ii), index), [thseal(1), thseal(2)], CI);
               tseal = [tseal, ts']; pseal = [pseal, ps'];
            elseif ii == 3
               thseal = [TETA(index(ii-1)), TETA(index(ii))] ;
               CI = pseal(1:end-1, end);
               [ts, ps] = ode15s(@(t, p) odesealfcn(t,p, nsv(ii), index), [thseal(1), thseal(2)], CI);
               PS = [ps'; pm*ones(1, size(ps', 2)) ];
               tseal = [tseal, ts']; pseal = [pseal, PS];
            end
        end

    else                 % se schema N -> N-1 -> N   (es. 7 -> 6 -> 7 )
       for ii = 1 : length(index)
            if ii == 1 && jj == 1  %primo passo con guess iniziale
               thseal = [TETA(1), TETA(index(1))];
               CI =  pa * ones(nsv(ii), 1);    %CI =  yoshida(nsv(ii), pa);
               [ts, ps] = ode15s(@(t, p) odesealfcn(t,p, nsv(ii), index), [thseal(1), thseal(2)], CI);
               tseal = ts'; 
               pseal = ps';
            elseif ii == 1 && jj > 1
               thseal = [TETA(1), TETA(index(1))];
               CI = pseal(:, end);
               [ts, ps] = ode15s(@(t, p) odesealfcn(t,p, nsv(ii), index), [thseal(1), thseal(2)], CI);
               tseal = ts';  
               pseal = ps';  
            elseif ii == 2
               thseal = [TETA(index(ii-1)), TETA(index(ii))] ; 
               CI = pseal(1:end-1, end);
               [ts, ps] = ode15s(@(t, p) odesealfcn(t,p, nsv(ii), index), [thseal(1), thseal(2)], CI);
               tseal = [tseal, ts']; 
               PS = [ps'; pm*ones(1, size(ps', 2))];
               pseal = [pseal, PS];
            elseif ii == 3
               thseal = [TETA(index(ii-1)), TETA(index(ii))] ;
               CI = [pa; pseal(1:end-1, end)];
               [ts, ps] = ode15s(@(t, p) odesealfcn(t,p, nsv(ii), index), [thseal(1), thseal(2)], CI);
               tseal = [tseal, ts']; 
               pseal = [pseal, ps'];
            end
        end  
    end
    
%     for kk = 1 : size(pseal, 1)
%         plot(tseal + (jj-1)*360/Z, pseal(kk, :), 'LineWidth', 1.5);
%         hold on
%     end
%     plot(TETA(index(1))*[1, 1] + (jj-1)*360/Z, [0, 1]*pm, '--k')
%     plot(TETA(index(2))*[1, 1] + (jj-1)*360/Z, [0, 1]*pm, '--k')
%     plot(TETA(index(3))*[1, 1] + (jj-1)*360/Z, [0, 1]*pm, '--k')
%     hold on
end

%hold off

%SAMPLING
SampleDeg = .5; % Angular sampling seal, keep same as geometry module sampling [deg]
t_trap_samp = 0:SampleDeg:th(end);
p_trap_samp = makima(tt, pt, t_trap_samp); %PTRAPPED IN SAMPLING ANGULAR POSITIONS 
%  p_trap_samp(p_trap_samp > pm ) = pm;  
%  p_trap_samp(p_trap_samp < pa) = pa;
t_seal_samp = 0:SampleDeg:tseal(end);
p_seal_samp = zeros(size(pseal, 1), length(t_seal_samp)); %PSEAL IN SAMPLING ANGULAR POSITIONS
  
for ii = 2 : length(tseal) - 2  %delete duplicate values at ode15s integration boundaries
    if tseal(ii) == tseal(ii - 1),  tseal(ii) = []; pseal(:, ii) = [];  end
end

for ii = 1 : size(pseal, 1)
    p_seal_samp(ii, :) = makima(tseal, pseal(ii, :), t_seal_samp);
end
    
% PRESSURE MEAN FORCE + CONTACT MEAN FORCE
for ii = 1 : length(TETA)

 thsealvec = (thseal2(ii)-thseal1(ii))/NoSV(ii) * ones(1, NoSV(ii)); 
         if TETA(ii) < TETA(index(1))
                 psealvec = p_seal_samp(end:-1:2, ii);
         elseif TETA(ii) >= TETA(index(1)) && TETA(ii) < TETA(index(2))
                 psealvec = p_seal_samp(end:-1:1, ii);
         else
                 psealvec = p_seal_samp(end-1:-1:1, ii);
         end

if thcontact2(ii) < 10000  && ii <= length(p_trap_samp)
    thfield = pi/180 * [thcontact1(ii), thseal1(ii)-thcontact1(ii),  thsealvec, ...
                        thcontact2(ii)-thseal2(ii), 360-thcontact2(ii)];   
    pfield = [p_trap_samp(ii), pm, psealvec', pa, p_trap_samp(ii)];
else 
   if thcontact1(ii) < 90  %contact is on delivery side
     thfield = pi/180 * [thcontact1(ii), thseal1(ii)-thcontact1(ii),  thsealvec, ...
                           360-thseal2(ii)]; 
     pfield = [pa, pm, psealvec', pa];

   else                   %contact is on suction side
     thfield = pi/180 * [thseal1(ii), thsealvec, ...
                           thcontact1(ii)-thseal2(ii), 360-thcontact1(ii)]; 
     pfield = [pm, psealvec', pa, pm];
   end
end

   [fxp(ii), fyp(ii)] = pressureForce(pfield, thfield, b, TR(end));
   [fxp1(ii), fyp1(ii)] = rotateZ(fxp(ii), fyp(ii), deg2rad(-27.7));
   if thcontact2(ii) < 10000
   Torque(ii) = meshingTorque(p_trap_samp(ii), r2contact1(ii), r2contact2(ii), TR(end), b);
   else,  Torque(ii) = meshingTorque(NaN, r2contact1(ii), r2contact2(ii), TR(end), b); end

end

fp = [mean(fxp), mean(fyp)]; %Pressure force [N]

fmesh= mean(Torque)/(0.001*rprim*cosd(27.7)); %NOTA: base circle radius = d_prim/2 * cos(alpha_lav)
fmg = fmesh*[-sind(27.7), cosd(27.7),];

fgear = fp + fmg; %Total force on gear[N]

load = .5*norm(fgear, 2);  %load on journal bearing
                           %(load is divided between two JBs)
load_angle = rad2deg(mod(atan2(fgear(2), fgear(1)), 2*pi)); %[deg]

e_old = sqrt(BEAR(iter, 1)^2 + BEAR(iter, 2)^2 );
G_old =  rad2deg(mod(atan2(BEAR(iter, 2), BEAR(iter, 1)), 2*pi));

[ecc,attitude_angle] = JBe(DR*0.001, Lbear*0.001, Clear*0.001, RPM/60, load, mu);
ecc = ecc * 1000; %[mm]
GAMMA = load_angle - attitude_angle;
xbear = ecc*cosd(GAMMA);
ybear = ecc*sind(GAMMA);

BEAR = [BEAR; xbear, ybear, ecc, GAMMA];
writematrix(BEAR, filename, 'Sheet', 1, 'Range', 'A2')

rel_err_e = abs(ecc - e_old)/abs(e_old);
rel_err_G = abs(GAMMA - G_old)/abs(G_old);

iter = iter + 1;

end

%% VANE PRESSURE HISTORY PLOT 
figure(300)
for ii = 1 : size(p_seal_samp, 1)
    plot(t_seal_samp + (ii-1)*360/Z - TETA(index(1)), p_seal_samp(ii, :),':k', 'linewidth', 1.5)
    xlim([0, 360])
    hold on
    if ii == size(p_seal_samp, 1) 
        tman = t_seal_samp(end)+(ii-1)*360/Z-TETA(index(1)):SampleDeg:(max(thseal2)-thcontact1(1));
        tasp = t_trap_samp(end)+tman(end):SampleDeg:360;
        plot(tman, pm*ones(length(tman), 1) , ':k', 'linewidth', 1.5)
        plot(t_trap_samp + tman(end), p_trap_samp, ':r', 'linewidth', 1.5)
        plot(tasp, [p_trap_samp(end); pa*ones(length(tasp)-1, 1)], ':k', 'linewidth', 1.5)
    end
end
  
figure(400)
plot(TETA, fxp, '-r', TETA, fyp, '-b')
ylabel('Force [N]')
xlabel('theta [deg]')
legend('fxp', 'fyp')
figure(401)
plot(TETA, fxp1, '-r', TETA, fyp1, '-b')
ylabel('Force [N]')
xlabel('theta [deg]')
legend('fxp1', 'fyp1')

figure(500)
plot(TETA, Torque)
ylabel('Torque [Nm]')
xlabel('theta [deg]')

toc
beep