clear all; clc; close all;

syms f1 f2 d1 d2 x1 x2 x;
% f1: load in the middle of the needle
% f2: load at the needle tip
% d1: location of the first load (f1)
% d2: length of the needle
% x1: location of the first sensor
% x2: location of the second sensor
% x : variable along the needle length

E=208e9; %Pa = kg/(m^1*s^2)
% E : Young's Modulus of the needle (208 GPa)
a=.67e-3; b=.45e-3; %m
density=8.44e-3/(1e-2)^3; %kg/m^3
volume=15e-2*pi*(a^2-b^2); %m^3
mass=density*volume; %kg
I=mass*(a^2+b^2)/12; %kg*m^2
% I : Moment of Inertia of the needle 
%E*I 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% True Model

%%% R = E*I/rho
r1 = (f1+f2)*x-(f1*d1+f2*d2);
r2 = f2*x-f2*d2;

%%% s = slope*E*I
s1 = int(r1);
x=d1;
c=simplify( eval(s1)-eval(int(r2)) );
s2 = int(r2)+c;

%%% y = deflection*E*I 
y1=int(s1);
x=d1;
c=simplify( eval(y1)-eval(int(s2)) );
y2=int(s2)+c;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Estimated Model from sensor signal

x = x1; rx1 = eval(r1);
x = x2; rx2 = eval(r2);

X = [x1^2 x1 1; x2^2 x2 1; d2^2 d2 1];
A = simplify( inv(X)*[rx1; rx2; 0] );

syms x;
r_est = simplify( A(1)*x^2 + A(2)*x +A(3) );
s_est = simplify( int(r_est) );
y_est = simplify( int(s_est) );

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Error Calculation

x=d2;
e=eval(y2)-eval(y_est);
f1=1e-3*9.81; f2=2e-3*9.81; d1=75; d2=150;
err=eval(e);
dx1=diff(err,x1);
dx2=diff(err,x2);

for x1=1:d1
    for x2=d1+1:d2
        Err(x2-d1, x1)=eval(err);
        DX1(x2-d1, x1)=eval(dx1);
        DX2(x2-d1, x1)=eval(dx2);
    end
end

figure;
x1=1:d1; x2=d1+1:d2;
surfc(x1, x2, (Err)/E/I/1000, 'EdgeColor','none'); colorbar;
% colormap hsv; shading interp;
xlabel('x1'); ylabel('x2'); axis([0 75 75 150 -15000 5000]);
zlabel('tip deflection error');
figure;
contourf(x1,x2, (Err)/E/I/1000, 'EdgeColor','none'); colorbar;
xlabel('x1');ylabel('x2');zlabel('tip deflection error');
colormap jet; grid on;

figure;
surfc(x1, x2, DX1, 'EdgeColor','none'); colorbar;
% colormap hsv; shading interp;
xlabel('x1');ylabel('x2');zlabel('d error / d x1');
axis([0 75 75 150 -300000 100000]);
figure;
contourf(x1, x2, DX1); xlabel('x1');ylabel('x2'); 
colorbar; grid on;

figure;
surfc(x1, x2, DX2, 'EdgeColor','none'); colorbar;
% colormap hsv; shading interp;
xlabel('x1');ylabel('x2');zlabel('d error / d x2');
axis([0 75 75 150 -80000 20000]);
figure;
contourf(x1, x2, DX2); xlabel('x1');ylabel('x2'); 
colorbar; grid on;

x1=25; x2=82;
figure; hold on; grid on;
for x=0:.05:75
    plot(x, eval(y1)/(E*I*1000)); plot(x, eval(y_est)/(E*I*1000), 'r');
    x=x+75; plot(x, eval(y2)/(E*I*1000)); plot(x, eval(y_est)/(E*I*1000), 'r');
end
xlabel('x'); ylabel('Deflection');
legend('True Model','Estimated Model');

figure; hold on; grid on;
for x=0:.05:75
    plot(x, eval(r1)/(E*I*1000)); plot(x, eval(r_est)/(E*I*1000), 'r');
    x=x+75; plot(x, eval(r2)/(E*I*1000)); plot(x, eval(r_est)/(E*I*1000), 'r');
end
xlabel('x'); ylabel('Curvature');
legend('True Model','Estimated Model');