close all
clear all

g = 9.81;

% XF = 10;
% X0 = XF+4;
% 
% X0 = X0*.0254;
% XF = XF*.0254;

F0 = -8.54*g;
d = -.0214;
M0 = F0*d;

k_targ = [-.06 -0.09]; %N-m/deg
FreeLength = [9 10]*.0254;
InitLength = [13 14]*.0254;

l = 4.5*.0254;

for i = 1:length(k_targ)
k_target = k_targ(i);  %N-m/deg
X0 = InitLength(i);
XF = FreeLength(i);

k_target = k_target*180/pi;  %N-m/rad

[ kr kl ] = SpringConstants(k_target,X0,XF,M0,l);

alpha = -30:30;  %deg

F = kr*(X0-XF-l*alpha*pi/180)-kl*(X0-XF+l*alpha*pi/180);
M = F*l+M0;

figure
plot(alpha,M)
grid on
xlabel('alpha (deg)');
ylabel('M (N-m)');
legend(['kr = ' num2str(kr*.0254/4.45,'%3.2f') ' lb/in    kl = ' num2str(kl*.0254/4.45,'%3.2f') ' lb/in']);
title(['Kt = ' num2str(k_target*pi/180, '%3.2f') ' N-m/deg    XF = ' num2str(XF/.0254,'%2.0f') ' in    X0 = ' num2str(X0/.0254,'%2.0f') ' in    M0 = ' num2str(M0,'%3.1f') ' N-m']);  

% fprintf('Symetric Geometry:  X0 = %f in    XF =  %f in    l = %f in   k_targ =  %3.2f N-m/deg\n',X0/.0254,XF/.0254,l/.0254,k_target*pi/180);
% for M0 = 0:.1:M0
%   [ kr, kl ] = SpringConstants(k_target,X0,XF,M0,l);
%   kr = kr*.0254/4.45;  %Convert to in-lbs
%   kl = kl*.0254/4.45;  %Convert to in-lbs
%   SpringMin = X0-l*max(alpha)*pi/180;  
%   SpringMax = X0-l*min(alpha)*pi/180;
%   fprintf('M0 = %3.1f N-m    kr = %3.2f lb/in  kl = %3.2f lb/in   12*kr = %3.1f lb/in  12*kl = %3.1f lb/in    SpringMin = %3.1f in   SpringMax = %3.1f in  \n',M0,kr,kl,12*kr,12*kl,SpringMin/.0254,SpringMax/.0254);
% end
    
disp(' ');
fprintf('Offset Geometery  X0 = %f in    XF =  %f in    l = %f in    k_targ =  %3.2f N-m/deg \n',X0/.0254,XF/.0254,l/.0254,k_target*pi/180);
for M0 = 0:.1:M0
  [ k, delta_x ] = SpringConstantAndOffset(k_target,M0,l);
  k = k*.0254/4.45;  %Convert to in-lbs
  LeftSpringLength = X0-delta_x+l*alpha*pi/180;
  RightSpringLength = X0+delta_x-l*alpha*pi/180;
  SpringMin = min([RightSpringLength LeftSpringLength]);
  SpringMax = max([RightSpringLength LeftSpringLength]);
  fprintf('M0 = %3.1f N-m    k = %3.2f lb/in   12*k = %3.1f lb/in   delta_x = %3.2f in    SpringMin = %3.1f in   SpringMax = %3.1f in\n',M0,k,12*k,delta_x/.0254,SpringMin/.0254,SpringMax/.0254);
end

disp(' ');
disp(' ');

alpha = -30:30;  %deg
k = k*4.45/.0254;  %Convert to N-m
F = k*(X0+delta_x-l*alpha*pi/180-XF)-k*(X0-delta_x+l*alpha*pi/180-XF);
M = F*l+M0;

figure
plot(alpha,M)
grid on
xlabel('alpha (deg)');
ylabel('M (N-m)');
legend(['k = ' num2str(k*.0254/4.45,'%3.2f') ' lb/in    delta\_x = ' num2str(delta_x/.0254,'%3.2f') ' in']);
title(['Kt = ' num2str(k_target*pi/180, '%3.2f') ' N-m/deg    XF = ' num2str(XF/.0254,'%2.0f') ' in    X0 = ' num2str(X0/.0254,'%2.0f') ' in    M0 = ' num2str(M0,'%3.1f') ' N-m']);  

end