% Supplemental Item 3 for Spagna, J.C. et al., "PHYLOGENY, SCALING, AND THE GENERATION OF 
%								EXTREME FORCES IN TRAP-JAW ANTS"
% Code written by:  Antonis Vakis
% Tracing rotating appendages in MATLAB				
% Revision #3 - Comments edited by 	J. Spagna
% 11/22/2007

clear all;
close all;
clc;

% Recalling the filename and loading the calculated data
filename = input('Input the filename (case-sensitive): ','s');
load ([filename,'_data.mat']);
addpath(dir1,dir2);

% Resetting the coordinates to reflect the new origin (lower left)
coords = coords_new;
base_coords = base_coords_new;

% Building a vector of probable center of rotation (c.o.r.) locations
% Note: The approximate location of the mandible base will be used to
% find the centers of rotation of each mandible through iterations. This 
% crude distance will be more accurate when the mandibles are in their 
% closed position
Rcrude_left = sqrt(((coords(NEWframenum,1) - base_coords(1,1))^2) + ...
    ((coords(NEWframenum,2) - base_coords(1,2))^2));
Rcrude_right = sqrt(((coords(NEWframenum,3) - base_coords(1,1))^2) + ...
    ((coords(NEWframenum,4) - base_coords(1,2))^2));
Rcrude = (Rcrude_left + Rcrude_right)/2;

% Possible coordinates vector for the center of rotation
% Number of points to try out
numpts = 1600;                  % Make sure this number has a square root

% Note: the actual grid will have, for example, 144 instead of 121 points
cor_values_matrix = zeros(sqrt(numpts),sqrt(numpts),2);
cor_values = zeros(numpts + 1,2);

% Assuming that the actual center of rotation will be within a length of
% Rcrude of the mandible base location in the x-direction, and 
% Rcrude in the y-direction. Hence, a matrix of points in created 
% around the assumed position of the mandible base
cor_values_min_x = base_coords(1,1) - Rcrude;
cor_values_max_x = base_coords(1,1) + Rcrude;
interval_x = (cor_values_max_x - cor_values_min_x)/sqrt(numpts);

cor_values_min_y = base_coords(1,2) - Rcrude;
cor_values_max_y = base_coords(1,2) + Rcrude;
interval_y = (cor_values_max_y - cor_values_min_y)/sqrt(numpts);

cor_values_matrix(1,1,1) = cor_values_min_x;
cor_values_matrix(1,1,2) = cor_values_min_y;

for m = 1:sqrt(numpts)
    cor_values_matrix(m + 1,1,1) = cor_values_matrix(m,1,1) + interval_x;
    cor_values_matrix(m + 1,1,2) = cor_values_matrix(m,1,2) + interval_y;
    
    for n = 1:sqrt(numpts) 
        cor_values_matrix(:,n + 1,1) = cor_values_matrix(:,n,1);
        cor_values_matrix(:,n + 1,2) = cor_values_matrix(:,n,2);
    end
end

% The number of elements in the grid of (x0,y0) points
elemnum = (sqrt(numpts) + 1)^2;

cor_values_matrix_new(:,:,1) = cor_values_matrix(:,:,1)';
cor_values_x = reshape(cor_values_matrix_new(:,:,1),elemnum,1);
cor_values_y = reshape(cor_values_matrix(:,:,2),elemnum,1);
cor_values = [cor_values_x cor_values_y];

% The cor_values vector is a grid of points that will be tested to find the
% actual center of rotation for each mandible
figure(1)
plot(coords(:,1),coords(:,2),'bo')
hold on
plot(coords(:,3),coords(:,4),'ro')
hold on
plot(base_coords(1,1),base_coords(1,2),'ko')
hold on
plot(cor_values(:,1),cor_values(:,2),'g.')
title('Grid of possible locations of centers of rotation')
legend('Left mandible','Right mandible','Approximate mandible base',...
    'Possible locations of centers of rotation')
daspect([1 1 1])
saveas(gcf,[filename,'_grid.bmp'])

% We also need a list of possible R values, ranging from R_min to R_max as
% specified below
values = 200;
R_min = Rcrude - Rcrude/2;
R_max = Rcrude + Rcrude/2;
Rvalues = linspace(R_min,R_max,values);

% Therefore, now the first estimates of (x0,y0) have been calculated
% Calculating the RMS error
r_left = zeros(NEWframenum,elemnum);
r_right = r_left;
RMSleft = zeros(values,elemnum);
RMSright = RMSleft;

for p = 1:values
    for k = 1:elemnum
        SUM_left = 0;
        SUM_right = 0;
        
        for i = 1:NEWframenum
            r_left(i,k) = sqrt(((coords(i,1) - cor_values(k,1))^2) + ...
                ((coords(i,2) - cor_values(k,2))^2));
            r_right(i,k) = sqrt(((coords(i,3) - cor_values(k,1))^2) + ...
                ((coords(i,4) - cor_values(k,2))^2));
        end

        for j = 1:NEWframenum
            SUM_left = SUM_left + ((r_left(j,k) - Rvalues(p))^2);
            SUM_right = SUM_right + ((r_right(j,k) - Rvalues(p))^2);
        end
        
        RMSerror_left = sqrt(SUM_left/3);           % We have 3 free parameters
        RMSerror_right = sqrt(SUM_right/3);
        
        RMSleft(p,k) = RMSerror_left;
        RMSright(p,k) = RMSerror_right;
    end
end
RMSleft_vector = reshape(RMSleft,values*elemnum,1);
RMSright_vector = reshape(RMSright,values*elemnum,1);

% Minimum error values for the left and right mandible
RMSleft_min = min(RMSleft_vector);
RMSright_min = min(RMSright_vector);

% Creating a 3D mesh of the RMS error values
gridpos = 1:elemnum;
Rval = 1:values;

if (which == 0)
    % Left mandible
    figure(2)
    mesh(gridpos,Rval,RMSleft)
    title('Mesh of the RMS error for the left mandible')
    xlabel('x0,y0 iteration')
    ylabel('R iteration')
    zlabel('RMS error for the left mandible')
    saveas(gcf,[filename,'_mesh_left.bmp'])
    % Right mandible
    figure(3)
    mesh(gridpos,Rval,RMSright)
    title('Mesh of the RMS error for the right mandible')
    xlabel('x0,y0 iteration')
    ylabel('R iteration')
    zlabel('RMS error for the right mandible')
    saveas(gcf,[filename,'_mesh_right.bmp'])

elseif (which == 1)
    % Left mandible
    figure(2)
    mesh(gridpos,Rval,RMSleft)
    title('Mesh of the RMS error for the left mandible')
    xlabel('x0,y0 iteration')
    ylabel('R iteration')
    zlabel('RMS error for the left mandible')
    saveas(gcf,[filename,'_mesh_left.bmp'])

elseif (which == 2)
    % Right mandible
    figure(3)
    mesh(gridpos,Rval,RMSright)
    title('Mesh of the RMS error for the right mandible')
    xlabel('x0,y0 iteration')
    ylabel('R iteration')
    zlabel('RMS error for the right mandible')
    saveas(gcf,[filename,'_mesh_right.bmp'])
end

% To find the minimum x0,y0 and R for each case, we need to find the
% location of RMSleft_min and RMSright_min in the RMSleft and RMSright
% vectors respectively
count1_left = 0;
count1_right = 0;

for g = 1:(values*elemnum)
    count1_left = count1_left + 1;
    count1_right = count1_right + 1;
    if (RMSleft_min == RMSleft_vector(g,1))
        location1_left = count1_left;
    end
    if (RMSright_min == RMSright_vector(g,1))
        location1_right = count1_right;
    end
end

% Now that we know the location of the minimum values in RMSleft_vector and
% RMSright_vector, we need to translate it into x0,y0 and R values
colnum_left = floor(location1_left/values);
rownum_left = rem(location1_left,values);
colnum_right = floor(location1_right/values);
rownum_right = rem(location1_right,values);

% Therefore, for the left mandible:
x0y0left = cor_values(colnum_left,:);
Rleft = Rvalues(rownum_left);

% and for the right mandible:
x0y0right = cor_values(colnum_right,:);
Rright = Rvalues(rownum_right);

% Plotting left and right tip trajectories
% Plot the two circles as well
N = 200;                    % Number of points on the circle
t = (0:N)*2*pi/N;

% Plotting traced points
if (which == 0)
    figure(4)
    plot(coords(:,1),coords(:,2),'b*')
    hold on
    plot(coords(:,3),coords(:,4),'r*')
    hold on
    % Plotting centers of rotation
    plot(x0y0left(1,1),x0y0left(1,2),'bo')
    hold on
    plot(x0y0right(1,1),x0y0right(1,2),'ro')
    hold on
    % Plotting mandible trajectories (as circles)
    plot((Rleft*cos(t) + x0y0left(1,1)),(Rleft*sin(t) + x0y0left(1,2)),'b-.');
    hold on
    plot((Rright*cos(t) + x0y0right(1,1)),(Rright*sin(t) + x0y0right(1,2)),'r-.');
    title('Coordinates of the mandible tips at each frame')
    legend('Left mandible','Right mandible','Left c.o.r.','Right c.o.r.',0)
    daspect([1 1 1])
    saveas(gcf,[filename,'_trajectories.bmp'])

elseif (which == 1)
    figure(4)
    plot(coords(:,1),coords(:,2),'b*')
    hold on
    % Plotting centers of rotation
    plot(x0y0left(1,1),x0y0left(1,2),'bo')
    hold on
    % Plotting mandible trajectories (as circles)
    plot((Rleft*cos(t) + x0y0left(1,1)),(Rleft*sin(t) + x0y0left(1,2)),'b-.');
    title('Coordinates of the mandible tips at each frame')
    legend('Left mandible','Left c.o.r.',0)
    daspect([1 1 1])
    saveas(gcf,[filename,'_trajectories.bmp'])

elseif (which == 2)
    figure(4)
    plot(coords(:,3),coords(:,4),'r*')
    hold on
    % Plotting centers of rotation
    plot(x0y0right(1,1),x0y0right(1,2),'ro')
    hold on
    % Plotting mandible trajectories (as circles)
    plot((Rright*cos(t) + x0y0right(1,1)),(Rright*sin(t) + x0y0right(1,2)),'r-.');
    title('Coordinates of the mandible tips at each frame')
    legend('Right mandible','Right c.o.r.',0)
    daspect([1 1 1])
    saveas(gcf,[filename,'_trajectories.bmp'])
end

% Calculating velocities and accelerations
% Slope and angle for each mandible position
for a = 1:NEWframenum
    mleft(a) = (coords(a,2) - x0y0left(1,2))/(coords(a,1) - x0y0left(1,1));
    mright(a) = (coords(a,4) - x0y0right(1,2))/(coords(a,3) - x0y0right(1,1));
    
    % Calculating absolute angle in radians
    % Left mandible
    if (mleft(a) > 0)           % i.e. in the 3rd quadrant
        theta_left(a) = atan(mleft(a)) + pi;
    elseif (mleft(a) < 0)       % i.e. in the 4th quadrant
        theta_left(a) = atan(mleft(a)) + 2*pi;
    end
    % Right mandible
    if (mright(a) > 0)          % i.e. in the 3rd quadrant
        theta_right(a) = atan(mright(a)) + pi;
    elseif (mright(a) < 0)      % i.e. in the 4th quadrant
        theta_right(a) = atan(mright(a)) + 2*pi;
    end
end

% Expressing the distances in terms of meters (m)
% i.e. for 10x zoom (X in m) = (X in px)*(1 in/ 96px)*(1/ 10)*(0.0254 m/in)
Rleft_m = (Rleft/dpi/zoom)*0.0254;
Rright_m = (Rright/dpi/zoom)*0.0254;

% Velocity calculations (velocity calculated at each interval)
velintervals = NEWframenum - 1;

for b = 1:velintervals
    % Angular velocities (in rad/sec)
    w_left(b) = (theta_left(b + 1) - theta_left(b))*fps;
    w_right(b) = (theta_right(b + 1) - theta_right(b))*fps;
    
    % Radial velocities (in m/sec)
    u_left(b) = w_left(b)*Rleft_m;
    u_right(b) = w_right(b)*Rright_m;
end

% Acceleration calculations
accintervals = velintervals - 1;

for c = 1:accintervals
    % Angular accelerations (in rad/sec/sec)
    alpha_left(c) = (w_left(c + 1) - w_left(c))*fps;
    alpha_right(c) = (w_right(c + 1) - w_right(c))*fps;
    
    % Radial accelerations (in px/sec/sec)
    a_left(c) = alpha_left(c)*Rleft_m;
    a_right(c) = alpha_right(c)*Rright_m;
end

% Maximum values
% Left mandible
max_ang_vel_left = max(abs(w_left));
max_rad_vel_left = max(abs(u_left));
max_ang_acc_left = max(abs(alpha_left));
max_rad_acc_left = max(abs(a_left));
% Right mandible
max_ang_vel_right = max(abs(w_right));
max_rad_vel_right = max(abs(u_right));
max_ang_acc_right = max(abs(alpha_right));
max_rad_acc_right = max(abs(a_right));

% Plotting velocities and accelerations
if (which == 0)
    % Angular velocities
    figure(5)
    plot(w_left,'b-o')
    hold on
    plot(w_right,'r-o')
    title('Angular velocities during the strike')
    xlabel('Frames')
    ylabel('Velocity (rad/sec)')
    legend('Left mandible angular velocity','Right mandible angular velocity',0)
    saveas(gcf,[filename,'_angvel.bmp'])

    % Radial velocities
    figure(6)
    plot(u_left,'b-o')
    hold on
    plot(u_right,'r-o')
    title('Radial velocities during the strike')
    xlabel('Frames')
    ylabel('Velocity (m/sec)')
    legend('Left mandible radial velocity','Right mandible radial velocity',0)
    saveas(gcf,[filename,'_radvel.bmp'])

    % Absolute radial velocities
    figure(7)
    plot(abs(u_left),'b-o')
    hold on
    plot(abs(u_right),'r-o')
    hold on
    plot(linspace(1,NEWframenum - 1,100)...
        ,abs(max(max_rad_vel_left,max_rad_vel_right)),'g.')
    title('Absolute radial velocities during the strike')
    xlabel('Frames')
    ylabel('Absolute velocity (m/sec)')
    legend('Left mandible abs. rad. vel.','Right mandible abs. rad. vel.',...
        'Maximum value',0)
    saveas(gcf,[filename,'_radvelabs.bmp'])

    % Angular accelerations
    figure(8)
    plot(alpha_left,'b-o')
    hold on
    plot(alpha_right,'r-o')
    title('Angular accelerations during the strike')
    xlabel('Frames')
    ylabel('Acceleration (rad/sec/sec)')
    legend('Left mandible angular acceleration',...
        'Right mandible angular acceleration',0)
    saveas(gcf,[filename,'_angacc.bmp'])

    % Radial accelerations
    figure(9)
    plot(a_left,'b-o')
    hold on
    plot(a_right,'r-o')
    title('Radial accelerations during the strike')
    xlabel('Frames')
    ylabel('Acceleration (m/sec/sec)')
    legend('Left mandible radial acceleration',...
        'Right mandible radial acceleration',0)
    saveas(gcf,[filename,'_radacc.bmp'])

    % Absolute radial accelerations
    figure(10)
    plot(abs(a_left),'b-o')
    hold on
    plot(abs(a_right),'r-o')
    hold on
    plot(linspace(1,NEWframenum - 2,100)...
        ,abs(max(max_rad_acc_left,max_rad_acc_right)),'g.')
    title('Absolute radial accelerations during the strike')
    xlabel('Frames')
    ylabel('Absolute acceleration (m/sec/sec)')
    legend('Left mandible abs. rad. acc.','Right mandible abs. rad. acc.',...
        'Maximum value',0)
    saveas(gcf,[filename,'_radaccabs.bmp'])

elseif (which == 1)
    % Angular velocities
    figure(5)
    plot(w_left,'b-o')
    title('Angular velocities during the strike')
    xlabel('Frames')
    ylabel('Velocity (rad/sec)')
    legend('Left mandible angular velocity',0)
    saveas(gcf,[filename,'_angvel.bmp'])

    % Radial velocities
    figure(6)
    plot(u_left,'b-o')
    title('Radial velocities during the strike')
    xlabel('Frames')
    ylabel('Velocity (m/sec)')
    legend('Left mandible radial velocity',0)
    saveas(gcf,[filename,'_radvel.bmp'])

    % Absolute radial velocities
    figure(7)
    plot(abs(u_left),'b-o')
    hold on
    plot(linspace(1,NEWframenum - 1,100)...
        ,abs(max(max_rad_vel_left,max_rad_vel_right)),'g.')
    title('Absolute radial velocities during the strike')
    xlabel('Frames')
    ylabel('Absolute velocity (m/sec)')
    legend('Left mandible abs. rad. vel.','Maximum value',0)
    saveas(gcf,[filename,'_radvelabs.bmp'])

    % Angular accelerations
    figure(8)
    plot(alpha_left,'b-o')
    title('Angular accelerations during the strike')
    xlabel('Frames')
    ylabel('Acceleration (rad/sec/sec)')
    legend('Left mandible angular acceleration',0)
    saveas(gcf,[filename,'_angacc.bmp'])

    % Radial accelerations
    figure(9)
    plot(a_left,'b-o')
    title('Radial accelerations during the strike')
    xlabel('Frames')
    ylabel('Acceleration (m/sec/sec)')
    legend('Left mandible radial acceleration',0)
    saveas(gcf,[filename,'_radacc.bmp'])

    % Absolute radial accelerations
    figure(10)
    plot(abs(a_left),'b-o')
    hold on
    plot(linspace(1,NEWframenum - 2,100)...
        ,abs(max(max_rad_acc_left,max_rad_acc_right)),'g.')
    title('Absolute radial accelerations during the strike')
    xlabel('Frames')
    ylabel('Absolute acceleration (m/sec/sec)')
    legend('Left mandible abs. rad. acc.','Maximum value',0)
    saveas(gcf,[filename,'_radaccabs.bmp'])

elseif (which == 2)
    % Angular velocities
    figure(5)
    plot(w_right,'r-o')
    title('Angular velocities during the strike')
    xlabel('Frames')
    ylabel('Velocity (rad/sec)')
    legend('Right mandible angular velocity',0)
    saveas(gcf,[filename,'_angvel.bmp'])

    % Radial velocities
    figure(6)
    plot(u_right,'r-o')
    title('Radial velocities during the strike')
    xlabel('Frames')
    ylabel('Velocity (m/sec)')
    legend('Right mandible radial velocity',0)
    saveas(gcf,[filename,'_radvel.bmp'])

    % Absolute radial velocities
    figure(7)
    plot(abs(u_right),'r-o')
    hold on
    plot(linspace(1,NEWframenum - 1,100)...
        ,abs(max(max_rad_vel_left,max_rad_vel_right)),'g.')
    title('Absolute radial velocities during the strike')
    xlabel('Frames')
    ylabel('Absolute velocity (m/sec)')
    legend('Right mandible abs. rad. vel.','Maximum value',0)
    saveas(gcf,[filename,'_radvelabs.bmp'])

    % Angular accelerations
    figure(8)
    plot(alpha_right,'r-o')
    title('Angular accelerations during the strike')
    xlabel('Frames')
    ylabel('Acceleration (rad/sec/sec)')
    legend('Right mandible angular acceleration',0)
    saveas(gcf,[filename,'_angacc.bmp'])

    % Radial accelerations
    figure(9)
    plot(a_right,'r-o')
    title('Radial accelerations during the strike')
    xlabel('Frames')
    ylabel('Acceleration (m/sec/sec)')
    legend('Right mandible radial acceleration',0)
    saveas(gcf,[filename,'_radacc.bmp'])

    % Absolute radial accelerations
    figure(10)
    plot(abs(a_right),'r-o')
    hold on
    plot(linspace(1,NEWframenum - 2,100)...
        ,abs(max(max_rad_acc_left,max_rad_acc_right)),'g.')
    title('Absolute radial accelerations during the strike')
    xlabel('Frames')
    ylabel('Absolute acceleration (m/sec/sec)')
    legend('Right mandible abs. rad. acc.','Maximum value',0)
    saveas(gcf,[filename,'_radaccabs.bmp'])
end

% Outputting data into a text file
fid = fopen([filename,'_results.txt'],'wt');
fprintf(fid,'Image magnification factor = %g \n',zoom);
fprintf(fid,'First frame = %g \n',first);
fprintf(fid,'Last frame = %g \n \n',last);
% Left mandible
fprintf(fid,'Left mandible \n \n');
fprintf(fid,'Mandible length = %g m \n',Rleft_m);
fprintf(fid,'Maximum angular velocity = %g rad/s \n',...
    max_ang_vel_left);
fprintf(fid,'Maximum radial velocity = %g m/s \n',...
    max_rad_vel_left);
fprintf(fid,'Maximum angular acceleration = %g rad/s/s \n',...
    max_ang_acc_left);
fprintf(fid,'Maximum radial acceleration = %g m/s/s \n \n \n',...
    max_rad_acc_left);
% Right mandible
fprintf(fid,'Right mandible \n \n');
fprintf(fid,'Mandible length = %g m \n',Rright_m);
fprintf(fid,'Maximum angular velocity = %g rad/s \n',...
    max_ang_vel_right);
fprintf(fid,'Maximum radial velocity = %g m/s \n',...
    max_rad_vel_right);
fprintf(fid,'Maximum angular acceleration = %g rad/s/s \n',...
    max_ang_acc_right);
fprintf(fid,'Maximum radial acceleration = %g m/s/s \n',...
    max_rad_acc_right);
fclose(fid);