% MATLAB script to generate femoral parameters in terms of desired
% severity parameters 
% ### Robert J. Cooper, University of Leeds, August 2017 ###

% to find (x,y) st cam-rad = 101 (25.25) and cam-ang 65
% ang = 65
% y = sqrt(25.25^2/(1+(tand(ang))^2));
% x = tand(ang)*y;
% easiest to set big rad of ellipse as cam-point
% although this is not required

close all;
clear all;
clc;

% MANUAL INPUT desired CAM-RAD (85-105) AND CAM-ANGLE (5-80)
dcrad = 92.5; % * VARY THIS *
dcradsc = 92.5; % leave this alone
dcang = 60; % * VARY THIS *

% parameters
HR = 25;
centx = 0;
centy = 0;
centz = 0;
modelx = 0;
modely = 0;
modelz = 0;

% defines ellipse centres 
% need to add half difference from 92.5
% based on first calculating HR*0.01*(dcradsc+3)
halfdif1 = 0.5*HR*0.01*(dcrad-dcradsc) 
dang = 60;
k0 = 0.5;
k1 = 0.8+0.5*halfdif1;
k2 = 1.35+0.5*halfdif1;
k3 = 0.75;

% The new centre (h,k) must lie on the same line as (x,y) to maintain angle
% ie x = tand(ang)*y hence the lines h1 = tand(dang+5)*k1
h0 = tand(dang+5)*k0;
h1 = tand(dang+5)*k1;
h2 = tand(dang-5)*k2;
h3 = tand(90-dang)*k3;

% chosen centres work for cam-ang = 60
% so rotate to correct if angle is changed
dang = dcang;
newk0 = [cosd(dang-60) sind(dang-60); -sind(dang-60) cosd(dang-60)]*[h0 k0]';
newk1 = [cosd(dang-60) sind(dang-60); -sind(dang-60) cosd(dang-60)]*[h1 k1]';
newk2 = [cosd(dang-60) sind(dang-60); -sind(dang-60) cosd(dang-60)]*[h2 k2]';
newk3 = [cosd(dang-60) sind(dang-60); -sind(dang-60) cosd(dang-60)]*[h3 k3]';
% h0 = newk0(1); % comment out to keep first ellipse same
% k0 = newk0(2);
h1 = newk1(1);
k1 = newk1(2);
h2 = newk2(1);
k2 = newk2(2);
h3 = newk3(1);
k3 = newk3(2);

% To maintain same cam-rad, now need to subtract from big radius (NRa)
% the magnitude of the new centre point i.e. sqrt(h^2+k^2) 
%%% here also need to add 1/2 difference from 92.5 on sub1 and 2
% WORKS for cam rad but nack also too far
sub0 = sqrt(k0^2+h0^2);
sub1 = sqrt(k1^2+h1^2)-2*halfdif1;
sub2 = sqrt(k2^2+h2^2)-2*halfdif1;
sub3 = sqrt(k3^2+h3^2);

% Scales using linear interpolation so rest of femur matches cam
c = 92.5;
a = 85;
b = 100;
y = 0.98;
z = 1.02;
fac1 = (c - a) * (z - y) / (b - a) + y;
y = 0.978;
z = 0.992;
fac2 = (c - a) * (z - y) / (b - a) + y;

% Define ellipse parameters
NRa = [fac1*HR-sub0, HR*0.01*(dcradsc+3)-sub1, HR*0.01*(dcradsc-3)-sub2, fac1*0.8*HR-sub3, fac1*0.958*HR, ]
NRb = [fac2*HR, 0.928*HR*0.01*(dcradsc+1), 0.8*HR*0.01*(dcradsc-1), fac2*0.66*HR, fac2*0.64*HR, ]
NA = [90-(dang+5), 90-(dang+5), 90-(dang-5), 90-dang, 10, ]
NRx = [h0, h1, h2, h3, 0, ]
NRy = [k0, k1, k2, k3, 0, ]
NL = [0.2644, -6.1599, -14.7256, -23.2912, -34.0224, ]

% Define central loft slices as usual to check cam parameters
camslices = (2:3);

% Rest of script is same as severity-cam except also outputs ellipse
% parameters

% 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,:), 'b',...
    highcampoints(1,:), highcampoints(2,:), 'r+',...
    campoint(1), campoint(2), 'c*',...   
    circx, circy, 'k',...
    0, 0, 'k.',...    
    'LineWidth',3,...
    'markers',15);
    axis equal  
    axis ([-30 30 -30 30]);  
    set(gca,'FontSize',24);

    figure
    plot(cam_ellipse(1,:), cam_ellipse(2,:), 'k',...
        highcampoints(1,:), highcampoints(2,:), 'r+',...
        campoint(1), campoint(2), 'c*',...   
         0, 0, 'k.',...       %k2, h2, 'k.',...   
    'LineWidth',3,...
    'markers',10);
    axis equal  
    axis ([-30 30 -30 30]);   
    set(gca,'FontSize',24);
 
    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,'NRa = ['); 
for j = 1:length(NRa)
    fprintf(fid,'%.12f, ',NRa(j));
end
fprintf(fid,']\n'); 

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

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

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

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

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

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,'Cam-Rad = %0.12f\n',Offset);

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

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