clc;
clear all;

global O;
global Oh;
global i;
global omega;

%%%%%%%%%% Bot parameters %%%%%
% length in metres, time in seconds
L = 37.476/1000;
nlength = 6;
lambda = L/nlength;
R = 2.6612/1000;
a = 0.6188/1000; % measure accurately
theta = atan(2*pi*R/lambda);
mu = 0.97;
b = 18.4/1000;
h = 20/1000; % total length = L+2*b+h = 95 mm

a_lambda = a/lambda;
contour_length = lambda/cos(theta)*nlength;
phirange = nlength*2*pi;
res = 7;
nres=floor(nlength*res+.5);
arc_length = contour_length/nres;

del = a*sqrt(exp(1))/2;
Metric = lambda/del;
disp('Lambda/delta'); disp(Metric);
disp('# discretizations in 1 wavelength'); disp(res);
if (res > Metric)
   disp('You have chosen wrong value of discretized points');
end

% INPUT parameter except geometry
omega_wave = -6.0*2*pi; % wave speed of flagellum (add up the mag of 'visual' helix rotation and head rotation rates)

Gh = zeros(6,6);
v = zeros(1,3*nres);

% Changes to be made in Gh calculation inside SBT_helical_Lighthill_1
%% function

%% coordinate system
% origin at head center, z axis points from head center towards the end of flagellar tail. 
% y axis is in the plan of view, and x axis perpendicular to it (right handed coordinate system)
% this means propulsion causes motion in -ive z. -ive velocity Vz will cause positive force exerted by fluid to oppose motion i.e. -ive
% force by flagella on fluid
% flagella, for us, is right handed and propels forward by rotation in clockwise direction when viewed from the end of flagella
% i.e. when viewed from the origin the rotation of each point is
% counter-clockwise (+ive omega, which causes -ive counter-torque from the fluid, thus positive torque exerted on the fluid

%% sign convention
% Fz (Tz) is the force (torque) exerted by only flagella on the surrounding fluid.
% in the older version after APS and before 14June2015, [F] were
% negative of the values output from the SBT code, I changed it to +ive
v(:) = 0;
v(1:nres) = 1;
[Fx, Fy, Fz, Tx, Ty, Tz, Fxp, Fyp, Fzp, Txp, Typ, Tzp] = SBT_helical_Lighthill_1(a_lambda, theta, nlength, res, v, R, b, h);
Fx = Fx*(4*pi*mu*arc_length); Fy = Fy*(4*pi*mu*arc_length); Fz = Fz*(4*pi*mu*arc_length);
Fxp = Fxp*(4*pi*mu*arc_length); Fyp = Fyp*(4*pi*mu*arc_length); Fzp = Fzp*(4*pi*mu*arc_length);
Tx = Tx*(4*pi*mu*arc_length); Ty = Ty*(4*pi*mu*arc_length); Tz = Tz*(4*pi*mu*arc_length);
Txp = Txp*(4*pi*mu*arc_length); Typ = Typ*(4*pi*mu*arc_length); Tzp = Tzp*(4*pi*mu*arc_length);
Gh(:,1) = [Fxp; Fyp; Fzp; Txp/L; Typ/L; Tzp/L];

v(:) = 0;
v(nres+1:2*nres) = 1;
[Fx, Fy, Fz, Tx, Ty, Tz, Fxp, Fyp, Fzp, Txp, Typ, Tzp] = SBT_helical_Lighthill_1(a_lambda, theta, nlength, res, v, R, b, h);
Fx = Fx*(4*pi*mu*arc_length); Fy = Fy*(4*pi*mu*arc_length); Fz = Fz*(4*pi*mu*arc_length);
Fxp = Fxp*(4*pi*mu*arc_length); Fyp = Fyp*(4*pi*mu*arc_length); Fzp = Fzp*(4*pi*mu*arc_length);
Tx = Tx*(4*pi*mu*arc_length); Ty = Ty*(4*pi*mu*arc_length); Tz = Tz*(4*pi*mu*arc_length);
Txp = Txp*(4*pi*mu*arc_length); Typ = Typ*(4*pi*mu*arc_length); Tzp = Tzp*(4*pi*mu*arc_length);
Gh(:,2) = [Fxp; Fyp; Fzp; Txp/L; Typ/L; Tzp/L];

v(:) = 0;
v(2*nres+1:3*nres) = 1;
[Fx, Fy, Fz, Tx, Ty, Tz, Fxp, Fyp, Fzp, Txp, Typ, Tzp] = SBT_helical_Lighthill_1(a_lambda, theta, nlength, res, v, R, b, h);
Fx = Fx*(4*pi*mu*arc_length); Fy = Fy*(4*pi*mu*arc_length); Fz = Fz*(4*pi*mu*arc_length);
Fxp = Fxp*(4*pi*mu*arc_length); Fyp = Fyp*(4*pi*mu*arc_length); Fzp = Fzp*(4*pi*mu*arc_length);
Tx = Tx*(4*pi*mu*arc_length); Ty = Ty*(4*pi*mu*arc_length); Tz = Tz*(4*pi*mu*arc_length);
Txp = Txp*(4*pi*mu*arc_length); Typ = Typ*(4*pi*mu*arc_length); Tzp = Tzp*(4*pi*mu*arc_length);
Gh(:,3) = [Fxp; Fyp; Fzp; Txp/L; Typ/L; Tzp/L];

omega_input = [1;0;0]/L;
[Fx, Fy, Fz, Tx, Ty, Tz, Fxp, Fyp, Fzp, Txp, Typ, Tzp, Q] = SBT_helical_Lighthill_2(a_lambda, theta, nlength, res, omega_input, R, b, h);
Fx = Fx*(4*pi*mu*arc_length); Fy = Fy*(4*pi*mu*arc_length); Fz = Fz*(4*pi*mu*arc_length);
Fxp = Fxp*(4*pi*mu*arc_length); Fyp = Fyp*(4*pi*mu*arc_length); Fzp = Fzp*(4*pi*mu*arc_length);
Tx = Tx*(4*pi*mu*arc_length); Ty = Ty*(4*pi*mu*arc_length); Tz = Tz*(4*pi*mu*arc_length);
Txp = Txp*(4*pi*mu*arc_length); Typ = Typ*(4*pi*mu*arc_length); Tzp = Tzp*(4*pi*mu*arc_length);
%     Gh(:,4) = [Fx; Fy; Fz; Tx/L; Ty/L; Tz/L];
Gh(:,4) = [Fxp; Fyp; Fzp; Txp/L; Typ/L; Tzp/L];

omega_input = [0;1;0]/L;
[Fx, Fy, Fz, Tx, Ty, Tz, Fxp, Fyp, Fzp, Txp, Typ, Tzp, Q] = SBT_helical_Lighthill_2(a_lambda, theta, nlength, res, omega_input, R, b, h);
Fx = Fx*(4*pi*mu*arc_length); Fy = Fy*(4*pi*mu*arc_length); Fz = Fz*(4*pi*mu*arc_length);
Fxp = Fxp*(4*pi*mu*arc_length); Fyp = Fyp*(4*pi*mu*arc_length); Fzp = Fzp*(4*pi*mu*arc_length);
Tx = Tx*(4*pi*mu*arc_length); Ty = Ty*(4*pi*mu*arc_length); Tz = Tz*(4*pi*mu*arc_length);
Txp = Txp*(4*pi*mu*arc_length); Typ = Typ*(4*pi*mu*arc_length); Tzp = Tzp*(4*pi*mu*arc_length);
Gh(:,5) = [Fxp; Fyp; Fzp; Txp/L; Typ/L; Tzp/L];

omega_input = [0;0;1]/L;
[Fx, Fy, Fz, Tx, Ty, Tz, Fxp, Fyp, Fzp, Txp, Typ, Tzp, Q] = SBT_helical_Lighthill_2(a_lambda, theta, nlength, res, omega_input, R, b, h);
Fx = Fx*(4*pi*mu*arc_length); Fy = Fy*(4*pi*mu*arc_length); Fz = Fz*(4*pi*mu*arc_length);
Fxp = Fxp*(4*pi*mu*arc_length); Fyp = Fyp*(4*pi*mu*arc_length); Fzp = Fzp*(4*pi*mu*arc_length);
Tx = Tx*(4*pi*mu*arc_length); Ty = Ty*(4*pi*mu*arc_length); Tz = Tz*(4*pi*mu*arc_length);
Txp = Txp*(4*pi*mu*arc_length); Typ = Typ*(4*pi*mu*arc_length); Tzp = Tzp*(4*pi*mu*arc_length);
Q = -Q*(4*pi*mu*arc_length)*omega_wave;
Gh(:,6) = [Fxp; Fyp; Fzp; Txp/L; Typ/L; Tzp/L];

Gh = Gh;

GH = zeros(6,6);

GH(1,1) = -6*pi*mu*b; GH(2,2) = -6*pi*mu*b; GH(3,3) = -6*pi*mu*b;
GH(4,4) = -8*pi*mu*b^3/L; GH(5,5) = -8*pi*mu*b^3/L; GH(6,6) = -8*pi*mu*b^3/L;
GH(4:6,:) = GH(4:6,:)/L;

K = -Gh + GH;
Q = [0;0;0;Q];

% helical wave speed (rad/s)
omega = omega_wave;

delta = sin(7.62*pi/180)*((L/2)+b+h);
delm = 0.904e-3;
G = [0;9.8;0];

% Time steps for integration for euler angle equations
tspan = 0:1/(omega/(2*pi))/100:200/(omega/(2*pi));
deltat = tspan(2) - tspan(1);
Time = length(tspan);

% y and A denotes the frame stuck to helix
% z and B is for the one stuck to head
% y/z contains euler angles, 1st one is phi, 2nd is theta, 3rd psi, A/B are
% transformation matrices
% Initial condition
y0 = [-9.3221; -1.5710; 568.1089];
z0 = [0; 0; 0];
yp = [0; 0; 0]; zp = [0; 0; 0];

A_inv = zeros(3,3,Time); B_inv = zeros(3,3,Time);
y = zeros(Time,3); z = zeros(Time,3);
y(1,:) = y0'; z(1,:) = z0';

Ro = zeros(3,Time+1);
w = zeros(3,Time); O = zeros(3,Time); Oh = zeros(3,Time);

options = odeset('RelTol',1e-10,'AbsTol',[1e-20 1e-20 1e-20]);

lp = [-delta;0;((L/2)+b+h)];

for i = 1:Time
    
    A_inv(:,:,i) = [cos(y(i,3))*cos(y(i,1)) - cos(y(i,2))*sin(y(i,1))*sin(y(i,3)), - sin(y(i,3))*cos(y(i,1)) - cos(y(i,2))*sin(y(i,1))*cos(y(i,3)), sin(y(i,2))*sin(y(i,1)); cos(y(i,3))*sin(y(i,1)) + cos(y(i,2))*cos(y(i,1))*sin(y(i,3)), - sin(y(i,3))*sin(y(i,1)) + cos(y(i,2))*cos(y(i,1))*cos(y(i,3)), - sin(y(i,2))*cos(y(i,1)); sin(y(i,2))*sin(y(i,3)), sin(y(i,2))*cos(y(i,3)), cos(y(i,2))];

    lpp = [-delta*cos(-omega*tspan(i)); -delta*sin(-omega*tspan(i)); ((L/2)+b+h)];
    
    P = zeros(6,1);
    C = cross(lpp,A_inv(:,:,i)'*G);
    P(4:6) = -C*delm/L;
    P(end) = P(end)-8*pi*mu*b^3/L*omega_wave;
    
    M = K\(P+Q);
    w(:,i) = [M(1); M(2); M(3)];
    O(:,i) = [M(4); M(5); M(6)]/L;
    Oh(:,i) = O(:,i) - [0;0;omega_wave];
        
    Ro(:,i+1) = Ro(:,i) + deltat*A_inv(:,:,i)*w(:,i);
    
    t = 0; q = 0; tH = 0; r = 0;
    
    
    if (i ~= Time)
        B = zeros(3,3);
        B(1,1) = sin(y(i,2))*sin(y(i,3)); B(1,2) = cos(y(i,3)); B(2,1) = sin(y(i,2))*cos(y(i,3)); B(2,2) = -sin(y(i,3)); B(3,1) = cos(y(i,2)); B(3,3) = 1;
        yp = B\O(:,i);

        
        [t,q]=ode15i(@EulerSolve_i,[tspan(i);tspan(i+1)],y(i,:)',yp,options);
        y(i+1,:) = q(end,:);
        

    end
    


end


figure;
plot3(Ro(1,:)*1000,Ro(2,:)*1000,Ro(3,:)*1000,'MarkerSize',30,'LineWidth',2);
% axis equal;
xlabel('\textbf{X (mm)}','Interpreter', 'Latex','Fontsize', 14); ylabel('\textbf{Y (mm)}','Interpreter', 'Latex','Fontsize', 14); zlabel('\textbf{Z (mm)}','Interpreter', 'Latex','Fontsize', 14);
title('\textbf{Tilt = $7.6^{\circ}$}','Interpreter', 'Latex','Fontsize', 14);