% Run this script to extract severity parameters after completing 
% femoral parameterisation
% Requires the parameters to be copied into script unlike original which
% uses data from parameterisation script 

close all;
clear all;
clc;

% copy in femoral parameters here 
HR = 20.508418484820;									
centx = -0.128168763681;									
centy = -0.340325007660;									
centz = -0.724870263187;									
modelx = -81.178006219982;									
modely = -136.104635066815;									
modelz = -224.880151447739;								
NRa = [19.968063084501, 16.688099441351, 13.315966900318, 14.818502083520, ];									
NRb = [20.419091353823, 19.336136233632, 15.685560368155, 12.907422576536, ];									
NL = [-0.925863265991, -6.925863265991, -14.925863265991, -20.925863265991, ];									
NA = [-29.253766029535, -4.854180914282, -14.266218582709, 18.838611285856, ];									
NRx = [-0.036365401848, -0.070745431625, -0.687359903845, -0.566217707312, ];									
NRy = [0.303782064708, 1.425935876200, 0.982644669638, -0.062770442699, ];									


% Define central loft slices
camslices = (2:3);

% Initiate severity and angle vectors and define up vector (points anteriorly)
CAM_rad = [];
CAM_ang = [];
CAM_width = [];
vectU = [0 1];

for camslice = camslices
    
    % rotation matrices 
    rotmatneg = [cosd(NA(camslice)) sind(NA(camslice)); -sind(NA(camslice)) cosd(NA(camslice))];
    rotmat = [cosd(-NA(camslice)) sind(-NA(camslice)); -sind(-NA(camslice)) cosd(-NA(camslice))];

    % undo rotation
    elcent = rotmatneg * [NRx(camslice);NRy(camslice)];
    untiltx{camslice} = elcent(1);
    untilty{camslice} = elcent(2);
    
   % the ellipse 
    theta_r         = linspace(0,2*pi);
    ellipse_x_r     = untiltx{camslice} + NRa(camslice)*cos( theta_r );
    ellipse_y_r     = untilty{camslice} + NRb(camslice)*sin( theta_r );
    
    % circle representing head (assume centre is 0,0)
    circx = HR*cos( theta_r );
    circy = HR*sin( theta_r );
    
    rotated_ellipse = rotmat * [ellipse_x_r;ellipse_y_r];

    % copy ellipse 
    cam_ellipse = rotated_ellipse;

    % subtract the head centre (because it isn't quite (0,0)
    cam_ellipse(1,:) = cam_ellipse(1,:) - centx; 
    cam_ellipse(2,:) = cam_ellipse(2,:) - centy; 
    
    % Need campoint to be anterior only
    % So need condition of y < 0 as this is the posterior region 
    % where cam is not present
    camcond = rotated_ellipse(2,:) < 0;
    % make columns that satisfy the condition equal to 0
    ant_ellipse = cam_ellipse;
    ant_ellipse(:,camcond) = 0; 
    
    % find sqrt distances of cam ellipse points to centre
    squaredDistance = cam_ellipse(1,:).^2 + cam_ellipse(2,:).^2;
    
    % take sqrt and scale by radius (1/HR) so we express as % of HR 
    camsevSlices = (sqrt(squaredDistance) ) * 1/HR*100;
    
    % do same for anterior only to get campoint
    squaredDistanceA = ant_ellipse(1,:).^2 + ant_ellipse(2,:).^2;
    camsevSlicesA = (sqrt(squaredDistanceA) ) * 1/HR*100;
    
    % find biggest severity radius 
    [maxcamsev, indexOfMax] = max(camsevSlicesA);
    
    % define cam point - the point on the ellipse furthest from head centre 
    campoint = [ant_ellipse(1,indexOfMax) ant_ellipse(2,indexOfMax)];

    % append severity at cam point - to CAM_offset,
    % which stores these values for both slices
    campointsev = camsevSlicesA(indexOfMax);
    CAM_rad = [CAM_rad campointsev];
    
    % find points either side of campoint until radius is < tolerance
    % so width only counts cam region
    
    % first loop round adding 1 to the index
    highcampoints = [];    
    while (camsevSlices(indexOfMax) > 0.9*campointsev &  length(highcampoints) < 50) 
        highcam = [cam_ellipse(1,indexOfMax); cam_ellipse(2,indexOfMax)];
        highcampoints = [highcampoints highcam];
        indexOfMax = mod(indexOfMax-1 + 1,100)+1;
    end
    
    % the extra condition in while loops ensure we don't just loop forever
    % the mod ensure the loop continues from 100 to 1
    
    % record length
    camw1 = length(highcampoints);
    
    % reset index to campoint and work the other way
    [maxcamsev, indexOfMax] = max(camsevSlicesA); 
    while (camsevSlices(indexOfMax) > 0.9*campointsev &  length(highcampoints) < 100) 
        highcam = [cam_ellipse(1,indexOfMax); cam_ellipse(2,indexOfMax)];
        highcampoints = [highcampoints highcam];
        indexOfMax = mod(indexOfMax-1 - 1,100)+1;
    end
        
    camw2 = length(highcampoints);
        
    % record the width of the high severity part
    % this is a % of the neck circumference as it is out of 100
    % points on the ellipse
    CAM_width = [CAM_width length(highcampoints)];
        
    % plot cam_ellipse 
    figure
    plot(cam_ellipse(1,:), cam_ellipse(2,:), 'r',...
    highcampoints(1,:), highcampoints(2,:), 'b+',...
    campoint(1), campoint(2), 'g+',...   
    circx, circy, 'k',...
    'markers',15);

    %x{camslice},y{camslice}, 'ro', ...

    axis equal  
    title('Cam severity measurement')
    
    % angle between up vector and campoint
    % this defines the position of the cam
    camang = dot(vectU,campoint)/(norm(vectU)*norm(campoint));
    camang = radtodeg(acos(camang));
    CAM_ang = [CAM_ang camang];

end

Offset = mean(CAM_rad);
Width = mean(CAM_width);
Angle = mean(CAM_ang);

% final parameters
% CAM_ang: defines position of campoint (angle from anterior)  

% CAM_offset: offset of campoint from head as % of HR
% so small is bad, can be negative which means ellipse > HR at campoint

% CAM_width: % of neck slice circumference whose offset is < 20% of HR
% remember that y-value of the loft slices are also relevant 

% pass outputs into a Python file 
fid = fopen('opparams.py', 'w');

fprintf(fid,'Cam_ang = ['); 
for j = 1:length(CAM_ang)
    fprintf(fid,'%.12f, ',CAM_ang(j));
end
fprintf(fid,']\n'); 

fprintf(fid,'Cam_rad = ['); 
for j = 1:length(CAM_rad)
    fprintf(fid,'%.12f, ',CAM_rad(j));
end
fprintf(fid,']\n'); 

fprintf(fid,'Cam_width = ['); 
for j = 1:length(CAM_width)
    fprintf(fid,'%.12f, ',CAM_width(j));
end
fprintf(fid,']\n'); 

fprintf(fid,'CamRad = %0.12f\n',Offset);

fprintf(fid,'Angle = %0.12f\n',Angle);

fprintf(fid,'Width = %0.12f\n',Width);
   