clc
clear all
close all force

set(gca, 'color', [0 0.1 0.1])
set(gca, 'XColor', [0 0.5 0.5]);
set(gca, 'YColor', [0 0.5 0.5]);
axis equal;
grid on; hold on;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%USER INPUT (GEAR PROFILE)
Z = 10; %define number of teeth
PA = 20; %define Pressure angle [deg]
w = 33; %define Tooth Width [mm]
m = 4.69; %define gear module
lres = 0.01; % Best to keep it between 0.1 - 0.01 for accuracy and 
             % low running time
ACDFac = 2.15;  %STANDARD = 2
DCDFac = 2.2;  %STANDARD = 2
Tol_trap = m * 1e-02; %if trapped volume computation is aborted, 
                      %try a less strict tolerance 

%USER INPUT (CASING PROFILE)
Tup_case = 115; %define casing angular extension (delivery side) [deg]
Tdown = 107; %define casing angular extension (suction side) [deg]
%USER INPUT (GROOVE ANGULAR EXTENSION)
Tgroove = 0 %+115+12; %define groove extension starting from upper casing [deg]

Tup = Tup_case - Tgroove;

%USER INPUT (DISCHARGE PROFILE)
Dup = 6.0; %define upper discharge from centerline [mm]
Ddown = 5.0; %define lower discharge from centerline [mm]

%USER INPUT (ANGULAR STEP FOR GEOMETRY EVALUATION)
teta_step = 0.5; %angular step [deg], keep it small to improve accuracy
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% MAKE TWO IDENTICAL GEARS
[g1x, g1y, tcx, tcy, PCD1, ACD1, DCD1, TT] = MakeGear1(Z, PA, m, lres, ACDFac, DCDFac);
g2x=g1x; g2y=g1y; PCD2=PCD1; ACD2=ACD1; DCD2=DCD1;
figure(1)
plot( tcx, tcy, 'linewidth', 1.5) %plot single tooth profile

% EVALUATE GEOMETRIC DIMENSIONS
TH = .5*(ACD1-DCD1); %tooth height [mm]
TW = -2*tcx(1:length(tcx)/2); %tooth thickness along height [mm]
TR = tcy(1:length(tcy)/2); % radius coordinate [mm]
TW_r = TW(1); %tooth thickness @ root [mm]
TW_t = TW(end); %tooth thickness @ tip [mm]
GAP = (pi*DCD1 - Z*TW_r) / Z; %clearance @ root [mm]
VA = (pi*(ACD1/2)^2 - polyarea(g1x, g1y)) / Z; %vane area [mm^2]
VV = VA*w;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%WRITE OUTPUT .xlsx FILE 
filename = 'Gprofile.xlsx'; 
                            % please change file name if you want to create
                            % different geometries (or delete the existing
                            % .xslx file)
string = ["Tooth height" "Rooth thick" "Tip thick" "Root clearance" "Vane volume", "Pitch Radius", "Mean Thick"];
writematrix(string, filename, 'Sheet', 1, 'Range', 'A3')
writematrix([TH, TW_r, TW_t, GAP, VV, PCD1/2, TT], filename, 'Sheet', 1, 'Range', 'A4')
string = ["Radius" "Tooth thick"];
writematrix(string, filename, 'Sheet', 2, 'Range', 'A1')
writematrix([TR', TW'], filename, 'Sheet', 2, 'Range', 'A2')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%IDENTIFY LEFT AND RIGHT TOOTH TIP EDGES (X,Y) COORDINATES (CLOCKWISE) !!!
for ii=1:Z
  [xLTE(ii), yLTE(ii)] = rotateZ(tcx(length(tcx)/2), ...
                                 tcy(length(tcy)/2), -2*pi*(ii-1)/Z); 
  [xRTE(ii), yRTE(ii)] = rotateZ(tcx(length(tcx)/2 + 1), ...
                                 tcy(length(tcy)/2 + 1), -2*pi*(ii-1)/Z);
%   figure(2)
%   plot(xLTE(ii), yLTE(ii), '.r', xRTE(ii), yRTE(ii), '.b' )
%   hold on 
%   pause(.5)
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
warning('off', 'MATLAB:polyshape:repairedBySimplify')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%IDENTIFY RIGHT TOOTH SIDE COORDINATES (CLOCKWISE) !!!
is_RS = zeros(length(g1x) - 1, 1);
T = fix((length(g1x)-1) / Z) ;
for ii = 1 : Z
    tr = (ii-1)*T;
    is_RS( length(tcx)/2 + 1 + tr: length(tcx) + tr ) = 1;
end


WB = 1.00*(PCD1/2 + PCD2/2); %wheelbase [mm]  %USER INPUT
PA = acosd( 0.5*PCD1*cosd(PA) / (.5*WB) ); %pressure angle given wheelbase
O1 = - WB / 2; O2 = -O1; %gear axis x-axis coordinates

%ROTATE IT FOR MESHING
phase = meshing_gears(tcx, tcy, O1, O2, PA, Z);
[g2x, g2y] = rotateZ(g2x, g2y, phase); 

%UPPER AND LOWER CONTACT POINTS
[UCx, UCy, LCx, LCy, condotta, teta0] = contact0(2*abs(O1), ACD1, PA, Z, m, tcx, tcy);
if condotta < 1, msgbox('CHECK YOUR INPUT PARAMETERS! GEAR MESHING IS NOT GARANTEED, \eps < 1','WARNING'), end
[g1x, g1y] = rotateZ(g1x, g1y, -teta0);
[g2x, g2y] = rotateZ(g2x, g2y, teta0);
[xLTE, yLTE] = rotateZ(xLTE, yLTE, -teta0);
[xRTE, yRTE] = rotateZ(xRTE, yRTE, -teta0);

%VISUAL PLOTTING OF CASING
[Xcase_s, Ycase_s, Xcase_d, Ycase_d] = casing(Tup_case, Tdown, O1 , .5*ACD1);
%VISUAL PLOTTING OF GROOVE
[Xgro_s, Ygro_s, Xgro_d, Ygro_d] = casing(Tup_case, -Tup, O1 , .5*ACD1*.9);

%VISUAL PLOTTING OF DISCHARGES
[Xdis_up, Ydis_up] = discharge_shape(Dup, abs(O1), abs(O1));
[Xdis_down, Ydis_down] = discharge_shape(-Ddown, -abs(O1), abs(O1));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
warning('off', 'MATLAB:polyshape:repairedBySimplify')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%COMPUTE GEAR MESHING AND RELEVANT GEOMETRY PROPERTIES
th1 = -deg2rad(teta_step);
for ii = 1 : ((360 / Z) / teta_step )
    TETA(ii) = (ii-1)*teta_step;
    [g1x, g1y] = rotateZ(g1x, g1y, th1);
    [g2x, g2y] = rotateZ(g2x, g2y, -th1);
    G1x = g1x + O1; G1y = g1y;
    G2x = g2x + O2; G2y = g2y;

    [xLTE, yLTE] = rotateZ(xLTE, yLTE, th1);
    [xRTE, yRTE] = rotateZ(xRTE, yRTE, th1);
    
    [Xtrap, Ytrap, Xc, Yc] = create_polygon( G1x, g1y, G2x, g2y, is_RS, PA, UCx, UCy, LCx, LCy, .1*length(tcx)/2, Tol_trap );
    [TA(ii) , TV(ii)]= evaluateArea( Xtrap, Ytrap, w );  %evaluate trapped area

    if length(Xc) == 1
         angle_contact(ii, 1) = rad2deg(mod(atan2(Yc, Xc-O1), 2*pi));
         angle_contact(ii, 2) = 1e4;
         r_contact(ii, 1) = sqrt(Yc^2 + (Xc-O1)^2);
         r_contact(ii, 2) = 1e4;
         vec1 = [-2*O1, 0]; vec2 = [Xc-O1, Yc];
         r_contact2(ii, 1) = norm(vec2 - vec1 , 2);
         r_contact2(ii, 2) = 1e4;
    else angle_contact(ii, 1) = rad2deg(mod(atan2(Yc(1), Xc(1)-O1), 2*pi));
         angle_contact(ii, 2) = rad2deg(mod(atan2(Yc(2), Xc(2)-O1), 2*pi));
         r_contact(ii, 1) = sqrt(Yc(1)^2 + (Xc(1)-O1)^2);
         r_contact(ii, 2) = sqrt(Yc(2)^2 + (Xc(2)-O1)^2);
         vec1 = [-2*O1, 0]; vec2 = [Xc(1)-O1, Yc(1)]; vec3 = [Xc(2)-O1, Yc(2)];
         r_contact2(ii, 1) = norm(vec2 - vec1 , 2);
         r_contact2(ii, 2) = norm(vec3 - vec1 , 2);
    end

    if ii>1 && TV(ii-1) == 0, TA(ii) = 0; TV(ii) = 0; UDA(ii) = 0; LDA(ii) = 0;
    else
        [polyup, polydown, UDA(ii), LDA(ii)] = discharge_Area( Xtrap, Ytrap, ...
                                   Xdis_up, Ydis_up,Xdis_down, Ydis_down );
           %evaluate upper discharge area and lower discharge area
    end

    [NoSV(ii), xxl, yyl, xxr, yyr] = sealedVolumes(Tup, Tdown, xLTE, yLTE, xRTE, yRTE, Z); 
                                              %number of sealed volumes
    angle_seal(ii, 1) = rad2deg(mod(atan2(yyl, xxl), 2*pi));
    angle_seal(ii, 2) = rad2deg(mod(atan2(yyr, xxr), 2*pi));
                                       
    figure(3)
    clf;
    set(gca, 'color', [0 0.1 0.1])
    set(gca, 'XColor', [0 0.5 0.5]);
    set(gca, 'YColor', [0 0.5 0.5]);
    axis equal;
    grid on; hold on;
    plot(G1x, g1y, 'w');
    plot(G2x, g2y, 'w');
    plot([PCD1, -PCD1]*tand(PA),  [-PCD1, PCD1], '--y', ...
          [UCx, LCx], [UCy, LCy], '-r')
    plot( Xcase_s, Ycase_s, '-w', Xcase_d, Ycase_d, '-w')
    plot(Xgro_s, Ygro_s, '-w', Xgro_d, Ygro_d, '-w')
    plot(Xc, Yc,  '.c', 'LineWidth', 1.25)
    plot(xxr +O1, yyr, 'om', xxl +O1, yyl, 'om', 'linewidth', 1.5)
    plot(Xdis_up, Ydis_up, '-b', Xdis_down, Ydis_down, '-b' )
    if length(Xtrap) > 1,  fill(Xtrap, Ytrap, 'g'), end
    if UDA(ii) > 0, fill(polyup.Vertices(:,1),polyup.Vertices(:,2), 'y') , end
    if LDA(ii) > 0, fill(polydown.Vertices(:,1),polydown.Vertices(:,2), 'y') , end
    title(['Number of Sealed Vane: ',num2str(NoSV(ii))]) 
    xlim([-1.5*WB, 1.5*WB])
    ylim([-PCD1, PCD1])
    xlabel('[mm]')
    ylabel('[mm]')
    pause(0.1)
end

%EVALUATE TRAPPED VOLUME AND TRAPPED VOLUME'S DERIVATIVES WITH RESPECT TO 
%ANGULAR POSITION (starting from first contact point)
VOL = zeros(1,  length(TV)); 
if  sum(TV~=0) ~= 0
    for ii = 1 : length(TV)
        if TV(ii) == 0, VOL = TV(1 : ii - 1); break, end
    end
end
dVol = zeros(1,  length(VOL));
dVol(1) = (VOL(2) - VOL(1)) / (-th1);
dVol(end) = (VOL(end) - VOL(end - 1)) / (-th1);
for ii = 2 : length(VOL) - 1
    dVol(ii) = (VOL(ii + 1) - VOL(ii - 1)) / (-2*th1);
end

figure(4)
plot(TETA(1 : length(dVol)), dVol, TETA, TV)
legend('dV_{trap}/d\theta [mm^3/rad]', 'V_{trap} [mm^3]')
xlabel('\theta [deg]')

figure(5)
plot(TETA(1 : length(dVol)), UDA(1 : length(dVol)), ...
     TETA(1 : length(dVol)), LDA(1 : length(dVol)), ...
     TETA(1 : length(dVol)), TA(1 : length(dVol)) )
legend('Delivery connection Area [mm^2]', 'Suction connection Area [mm^2]', 'Trapped Area [mm^2]')
xlabel('\theta [deg]')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%WRITE OUTPUT .xlsx FILE 
filename = 'GPgeometry.xlsx';
                              % please change file name if you want to
                              % create different geometries (or delete the
                              % existing  .xslx file)
string = ["THETA" "TrapVol" "dVOL/dTH" "DelivArea" "SuctArea"...
          "SealedVanes" "ThContact1" "ThContact2" "rContact1" "rContact2" "ThSeal1" "ThSeal2" "r2Contact1" "r2Contact2"];
writematrix(string, filename, 'Sheet', 1, 'Range', 'A1:N1')
writematrix(TETA',filename,'Sheet',1,'Range','A2')
writematrix(TV',filename,'Sheet',1,'Range','B2')
writematrix(dVol',filename,'Sheet',1,'Range','C2')
writematrix(UDA(1:length(dVol))',filename,'Sheet',1,'Range','D2')
writematrix(LDA(1:length(dVol))',filename,'Sheet',1,'Range','E2')
writematrix(NoSV',filename,'Sheet',1,'Range','F2')
writematrix([angle_contact, r_contact, angle_seal, r_contact2],filename,'Sheet',1,'Range','G2')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%scrivere data files per amesim
   [g2dx, g2dy] = rotateZ(g1x, g1y, 1.1*phase);

  % Creating file to be written to
    fileName1 = fopen('driving.dat','w');
    fileName2 = fopen('driven.dat','w');
    for ii = 1:length(g1x)
    % Writing data to file
    fprintf(fileName1, '%f %f\n', [g1x(ii), g1y(ii)]);
    fprintf(fileName2, '%f %f\n', [g2dx(ii)-WB, g2dy(ii)]);
    end
    fclose(fileName1)
    fclose(fileName2)

    drven=importdata('driven.dat');
    drvng=importdata('driving.dat');

plot(drvng(:,1), drvng(:, 2), '-r', drven(:,1), drven(:, 2), '-b');
axis equal
legend('driving', 'driven')
