clear all
close all
%return
tic

syms x y z real
syms phi theta psi real

% dq

syms dx dy dz real
syms dphi dtheta dpsi real


q = [x y z phi theta psi]';
dq = [dx dy dz dphi dtheta dpsi]';

syms g real;       


%Masses
syms M real

%Others

syms Ix Iy Iz real

%syms FL_right FD_right real
syms E_foils real

syms x_foils y_foil_left y_foil_right z_foils real
syms rho real
syms rho_air real
syms Wind_x Wind_y real

Phimat_comp

Speeds

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                   %
% Compute the points of application %
%                                   % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

syms Chord_max Chord_min real



%%************** YAW **************%%

syms x_yaw z_yaw y_yaw real
syms E_yaw EE_yaw real
syms A_yaw real
syms CL_yaw CD_yaw real
syms tanCal_yaw real

GA_yaw = [x_yaw;y_yaw;z_yaw - 0.5*(E_yaw + EE_yaw)];
V_Eair = Phimat*[dx dy dz]' + cross(O,GA_yaw); 
Tan_AoA_yaw =  -V_Eair(2)/V_Eair(1) + tanCal_yaw;     

Force_yaw = rho*A_yaw*(CL_yaw*[0 -1 0;1 0 0;0 0 0] - CD_yaw*[1 0 0;0 1 0;0 0 0])*V_Eair*sqrt(V_Eair'*V_Eair);
Torque_yaw = (cross(GA_yaw,Force_yaw));



%%************** PITCH **************%%

syms tanCal_pitch real
syms x_pitch z_pitch y_pitch real
syms A_pitch real
syms CL_pitch CD_pitch tanCal_pitch real
  
GA_pitch = [x_pitch;y_pitch;z_pitch];
V_Eair = simplify(Phimat*[dx dy dz]') + cross(O,GA_pitch);
Tan_AoA_pitch = -V_Eair(3)/V_Eair(1) + tanCal_pitch;  

Force_pitch = rho*A_pitch*(CL_pitch*[0 0 -1;0 0 0;1 0 0] - CD_pitch*[1 0 0;0 0 0;0 0 1])*V_Eair*sqrt(V_Eair'*V_Eair);
Torque_pitch = (cross(GA_pitch,Force_pitch));



%%************** RIGHT **************%%
syms A_right real
syms CL_right CD_right real
syms alpha_right real
syms tanCal_right real
syms EE_right real

Phi_air = [1      0          0;
           0   cos(-alpha_right)  sin(-alpha_right);
           0  -sin(-alpha_right)  cos(-alpha_right)];


  
%   Phimat = [e1)_E e2)_E e3)_E]  - Transforms : V)_E = Phimat * V)_e
%  Phi_air = [E1)_Eair E2)_Eair E3)_Eair]  -  Transforms : V)_Eair = Phi_air * V)_E 

% Airfoils referential

  
GA_right = [x_foils;y_foil_right;z_foils] + simple(Phi_air'*[EE_right*(Chord_max-Chord_min)/(3*E_foils) - Chord_max/3;(E_foils + 2*EE_right)/3;0]);

V_Eair = Phi_air*V_E + Phi_air*cross(O,GA_right);

Tan_AoA_right = -V_Eair(3)/V_Eair(1) + tanCal_right;  

Force_right = Phi_air'*rho*A_right*(CL_right*[0 0 -1;0 0 0;1 0 0] - CD_right*[1 0 0;0 0 0;0 0 1])*V_Eair*sqrt(V_Eair'*V_Eair);
Torque_right = (cross(GA_right,Force_right));



%%************** LEFT **************%%
syms A_left real
syms CL_left CD_left real
syms alpha_left real
syms tanCal_left real
syms EE_left real

Phi_air = [1      0          0;
           0   cos(alpha_left)  sin(alpha_left);
           0  -sin(alpha_left)  cos(alpha_left)];
        
 
 
 %   Phimat = [e1)_E e2)_E e3)_E]  - Transforms : V)_E = Phimat * V)_e
 %  Phi_air = [E1)_Eair E2)_Eair E3)_Eair]  -  Transforms : V)_Eair = Phi_air * V)_E 
     
GA_left = [x_foils;y_foil_left;z_foils] + simple(Phi_air'*[EE_left*(Chord_max-Chord_min)/(3*E_foils) - Chord_max/3;-(E_foils + 2*EE_left)/3;0]);

V_Eair = Phi_air*V_E + Phi_air*cross(O,GA_left);

Tan_AoA_left =  -V_Eair(3)/V_Eair(1) + tanCal_left;

Force_left = Phi_air'*rho*A_left*(CL_left*[0 0 -1;0 0 0;1 0 0] - CD_left*[1 0 0;0 0 0;0 0 1])*V_Eair*sqrt(V_Eair'*V_Eair);
Torque_left = (cross(GA_left,Force_left));

%************** SAIL ******************

 syms x_sail y_sail z_sail real
 syms CL_sail CD_sail A_sail real
 syms tanAngle_sail real
 syms Baume real 
 
 Den = sqrt(1 + tanAngle_sail^2);
 
 GA_sail = [x_sail - Baume/Den/3;y_sail - Baume*tanAngle_sail/Den/3;z_sail];
 
 V_Eair = simplify(Phimat*[dx-Wind_x dy-Wind_y dz]') + cross(O,GA_sail);

 
 Tan_AoA_sail = (V_Eair(2) - V_Eair(1)*tanAngle_sail)/(V_Eair(1) + V_Eair(2)*tanAngle_sail);   

 Force_sail = rho_air*A_sail*(CL_sail*[0 1 0;-1 0 0;0 0 0] - CD_sail*[1 0 0;0 1 0;0 0 0])*V_Eair*sqrt(V_Eair'*V_Eair);
 Torque_sail = (cross(GA_sail,Force_sail));

 % ************** Structural drag *****************
 syms CD_structure A_structure real

 V_Eair = simplify(Phimat*[dx-Wind_x dy-Wind_y dz]');
 
 Force_drag = rho_air*A_structure*(- CD_structure*[1 0 0;0 1 0;0 0 0])*V_Eair*sqrt(V_Eair'*V_Eair);
   
  
 % *********** Summation of the forces ******************
 
 Force_G_E = (Force_drag + Force_sail + Force_left + Force_right + Force_pitch + Force_yaw);
 Torque_G_E = (Torque_sail + Torque_left + Torque_right + Torque_pitch + Torque_yaw);
 
 Force_G_E1 = Force_G_E(1);
 Force_G_E2 = Force_G_E(2);
 Force_G_E3 = Force_G_E(3);
 
 Torque_G_E1 = Torque_G_E(1);
 Torque_G_E2 = Torque_G_E(2);
 Torque_G_E3 = Torque_G_E(3);
 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 
%%%%%%%%%%  Intersection Hydrofoils with water %%%%%%%%%%

% x_foils, y_foil_left, y_foil_right z_foils give the attachement point of the
% foils in E

% alpha_left & alpha_right give the angle of the foils

% EE_left & EE_right give the length of the emerged parts

%Position of water : 

%=> Phimat = [e1)_E e2)_E e3)_E]  - Transforms : V)_E = Phimat * V)_e

% [Wave_x;Wave_y] = Lambda*W
% Wave_speed = V/Lambda

syms a b real
syms Wave_angle0 Wave_amp0 V_wave0 Lambda0 real
syms t real

%N = number of harmonics

N = 2;
syms Wave_angle1 Wave_amp1 V_wave1 Lambda1 real
syms Wave_angle2 Wave_amp2 V_wave2 Lambda2 real

Wave_angle = [Wave_angle1;Wave_angle2];
Wave_amp = [Wave_amp1;Wave_amp2];
Lambda = [Lambda1;Lambda2];

Wave_speed = [V_wave1/Lambda1 V_wave2/Lambda2];
Wave_speed0 = V_wave0/Lambda0;

Wave_x0 = cos(Wave_angle0)/Lambda0;
Wave_y0 = sin(Wave_angle0)/Lambda0;

for k = 1:N
    Wave_x(k) = cos(Wave_angle(k))/Lambda(k);
    Wave_y(k) = sin(Wave_angle(k))/Lambda(k);
end

%%%%%%%%%%%%%%%%%% Wave model %%%%%%%%%%%%%%%%%%%%%%%%

Wave_z = eval(Wave_amp0*sin(2*pi*(a*Wave_x0 + b*Wave_y0  + Wave_speed0*t))); 

%Harmonics
for k = 1:N
    Wave_z = Wave_z + (Wave_amp(k)*sin(2*pi*(a*Wave_x(k) + b*Wave_y(k) + Wave_speed(k)*t)));
end

%%%%%%%%%%%%%%%%%%% EE left %%%%%%%%%%%%%%%%%%%%%%%

Inter_equ_flat = [a;b;0] - ([x y z]' + Phimat'*([x_foils y_foil_left z_foils]' + EE_left*[0 -cos(alpha_left) -sin(alpha_left)]'));

Inter_equ = [a;b;Wave_z] - ([x y z]' + Phimat'*([x_foils y_foil_left z_foils]' + EE_left*[0 -cos(alpha_left) -sin(alpha_left)]'));

Sol_mat_ab = jacobian(Inter_equ(1:2),[a b]);
Remain_mat_ab = simplify(Inter_equ(1:2) - Sol_mat_ab*[a b]');
Check_ab = simplify(Inter_equ(1:2) - Sol_mat_ab*[a b]' - Remain_mat_ab);

Sol_ab = -simplify(inv(Sol_mat_ab)*Remain_mat_ab);

a = Sol_ab(1);
b = Sol_ab(2);

Check_Sol_ab = simplify(eval(Inter_equ(1:2)));

Inter_equ_1 = simplify(eval(Inter_equ(3)));

Equ_left = Inter_equ_1;
dEqu_left = simplify(jacobian(Inter_equ_1,EE_left));

syms a b real

Sol_mat_flat = jacobian(Inter_equ_flat,[a b EE_left]);

Remain_mat_flat = simplify(Inter_equ_flat - Sol_mat_flat*[a b EE_left]');
Sol_flat = -simplify(inv(Sol_mat_flat)*Remain_mat_flat);

Check_flat = simplify(Inter_equ_flat - Sol_mat_flat*[a b EE_left]' - Remain_mat_flat);
Check_Sol_flat = simplify(Sol_mat_flat*Sol_flat + Remain_mat_flat);

EE_left_flat = Sol_flat(3);


%%%%%%%%%%%%%%%%% EE right %%%%%%%%%%%%%%%%%%%%

Inter_equ_flat = [a;b;0] - ([x y z]' + Phimat'*([x_foils y_foil_right z_foils]' + EE_right*[0 cos(alpha_right) -sin(alpha_right)]'));

Inter_equ = [a;b;Wave_z] - ([x y z]' + Phimat'*([x_foils y_foil_right z_foils]' + EE_right*[0 cos(alpha_right) -sin(alpha_right)]'));

Sol_mat_ab = jacobian(Inter_equ(1:2),[a b]);
Remain_mat_ab = simplify(Inter_equ(1:2) - Sol_mat_ab*[a b]');
Check_ab = simplify(Inter_equ(1:2) - Sol_mat_ab*[a b]' - Remain_mat_ab);

Sol_ab = -simplify(inv(Sol_mat_ab)*Remain_mat_ab);

a = Sol_ab(1);
b = Sol_ab(2);

Check_Sol_ab = simplify(eval(Inter_equ(1:2)));

Inter_equ_1 = simplify(eval(Inter_equ(3)));

Equ_right = Inter_equ_1;
dEqu_right = simplify(jacobian(Inter_equ_1,EE_right));

syms a b real

Sol_mat_flat = jacobian(Inter_equ_flat,[a b EE_right]);

Remain_mat_flat = simplify(Inter_equ_flat - Sol_mat_flat*[a b EE_right]');
Sol_flat = -simplify(inv(Sol_mat_flat)*Remain_mat_flat);

Check_flat = simplify(Inter_equ_flat - Sol_mat_flat*[a b EE_right]' - Remain_mat_flat);
Check_Sol_flat = simplify(Sol_mat_flat*Sol_flat + Remain_mat_flat);

EE_right_flat = Sol_flat(3);


%%%%%%%%%%%%%%%%% EE yaw %%%%%%%%%%%%%%%%%%%%

Inter_equ_flat = [a;b;0] - ([x y z]' + Phimat'*([x_yaw y_yaw z_yaw]' + EE_yaw*[0 0 -1]'));

Inter_equ = [a;b;Wave_z] - ([x y z]' + Phimat'*([x_yaw y_yaw z_yaw]' + EE_yaw*[0 0 -1]'));

Sol_mat_ab = jacobian(Inter_equ(1:2),[a b]);
Remain_mat_ab = simplify(Inter_equ(1:2) - Sol_mat_ab*[a b]');
Check_ab = simplify(Inter_equ(1:2) - Sol_mat_ab*[a b]' - Remain_mat_ab);

Sol_ab = -simplify(inv(Sol_mat_ab)*Remain_mat_ab);

a = Sol_ab(1);
b = Sol_ab(2);

Check_Sol_ab = simplify(eval(Inter_equ(1:2)));

Inter_equ_1 = simplify(eval(Inter_equ(3)));

Equ_yaw = Inter_equ_1;
dEqu_yaw = simplify(jacobian(Inter_equ_1,EE_yaw));

syms a b real

Sol_mat_flat = jacobian(Inter_equ_flat,[a b EE_yaw]);

Remain_mat_flat = simplify(Inter_equ_flat - Sol_mat_flat*[a b EE_yaw]');
Sol_flat = -simplify(inv(Sol_mat_flat)*Remain_mat_flat);

Check_flat = simplify(Inter_equ_flat - Sol_mat_flat*[a b EE_yaw]' - Remain_mat_flat);
Check_Sol_flat = simplify(Sol_mat_flat*Sol_flat + Remain_mat_flat);

EE_yaw_flat = Sol_flat(3);




save SYMB

%Create Wave_z file

List_of_variables = {'Wave_z'};
File_name = 'Wave_z_compute.m';

Var_name = 'Wave_z';
Equation = strcat(Var_name,' = ',char(eval(Var_name)),';');
        
fid = fopen(File_name,'wt');
fprintf(fid,'%s \n',Equation);
fclose(fid);


%Creates Wave Parameters file (intersection foils with rough sea)

List_of_variables = {'Lambda', 'Wave_amp', 'V_wave','Wave_angle'};
File_name = 'Wave_param.m';

index_equ = 1;
for k = 1:length(List_of_variables) 
    Var_name = List_of_variables{k};
    for j = 1:N
        Var_name_j = strcat(Var_name,num2str(j));
        Equation = strcat(Var_name_j,' = ',Var_name,'(',num2str(j),')',';');
        string2eval = strcat('Equ',num2str(index_equ),' = Equation;');
        eval(string2eval);
        index_equ = index_equ + 1;
    
    end
end


fid = fopen(File_name,'wt');
for k = 1:index_equ-1
    eval(strcat('fprintf(fid,''%s \n'',Equ',num2str(k),');'));
end
fclose(fid);


%Creates Sea Equation file (intersection foils with rough sea)

List_of_variables = {'Equ_left','Equ_right','Equ_yaw','dEqu_left','dEqu_right','dEqu_yaw'};
File_name = 'Equ.m';

index_equ = 1;
for k = 1:length(List_of_variables) 
Var_name = List_of_variables{k};
size_var = size(eval(Var_name));
for i = 1:size_var(1)
    for j = 1:size_var(2)
        Var_name_ij = strcat(Var_name,'(',num2str(i),',',num2str(j),')');
        Equation = strcat(Var_name_ij,' = ',char(eval(Var_name_ij)),';');
        string2eval = strcat('Equ',num2str(index_equ),' = Equation;');
        eval(string2eval);
        index_equ = index_equ + 1;
    end
end
end
fid = fopen(File_name,'wt');
for k = 1:index_equ-1
    eval(strcat('fprintf(fid,''%s \n'',Equ',num2str(k),');'));
end
fclose(fid);

%Creates EE_flat file (intersection foils with flat sea)

List_of_variables = {'EE_right_flat','EE_left_flat','EE_yaw_flat'};
File_name = 'EE_flat.m';

index_equ = 1;
for k = 1:length(List_of_variables) 
Var_name = List_of_variables{k};
size_var = size(eval(Var_name));
for i = 1:size_var(1)
    for j = 1:size_var(2)
        Var_name_ij = strcat(Var_name,'(',num2str(i),',',num2str(j),')');
        Equation = strcat(Var_name_ij,' = ',char(eval(Var_name_ij)),';');
        string2eval = strcat('Equ',num2str(index_equ),' = Equation;');
        eval(string2eval);
        index_equ = index_equ + 1;
    end
end
end
fid = fopen(File_name,'wt');
for k = 1:index_equ-1
    eval(strcat('fprintf(fid,''%s \n'',Equ',num2str(k),');'));
end
fclose(fid);

%Creates Angle file

List_of_variables = {'Tan_AoA_left','Tan_AoA_right','Tan_AoA_pitch','Tan_AoA_yaw','Tan_AoA_sail'};
File_name = 'Angles.m';

index_equ = 1;
for k = 1:length(List_of_variables) 
Var_name = List_of_variables{k};
size_var = size(eval(Var_name));
for i = 1:size_var(1)
    for j = 1:size_var(2)
        Var_name_ij = strcat(Var_name,'(',num2str(i),',',num2str(j),')');
        Equation = strcat(Var_name_ij,' = ',char(eval(Var_name_ij)),';');
        string2eval = strcat('Equ',num2str(index_equ),' = Equation;');
        eval(string2eval);
        index_equ = index_equ + 1;
    end
end
end
fid = fopen(File_name,'wt');
for k = 1:index_equ-1
    eval(strcat('fprintf(fid,''%s \n'',Equ',num2str(k),');'));
end
fclose(fid);



%Creates Geometric file

List_of_variables = {'Phimat','GA_right','GA_left','GA_yaw','GA_pitch'};
File_name = 'GAs.m';

index_equ = 1;
for k = 1:length(List_of_variables) 
Var_name = List_of_variables{k};
size_var = size(eval(Var_name));
for i = 1:size_var(1)
    for j = 1:size_var(2)
        Var_name_ij = strcat(Var_name,'(',num2str(i),',',num2str(j),')');
        Equation = strcat(Var_name_ij,' = ',char(eval(Var_name_ij)),';');
        string2eval = strcat('Equ',num2str(index_equ),' = Equation;');
        eval(string2eval);
        index_equ = index_equ + 1;
    end
end
end
fid = fopen(File_name,'wt');
for k = 1:index_equ-1
    eval(strcat('fprintf(fid,''%s \n'',Equ',num2str(k),');'));
end
fclose(fid);



List_of_variables = {'GA_sail'};
File_name = 'GAsail.m';

index_equ = 1;
for k = 1:length(List_of_variables) 
Var_name = List_of_variables{k};
size_var = size(eval(Var_name));
for i = 1:size_var(1)
    for j = 1:size_var(2)
        Var_name_ij = strcat(Var_name,'(',num2str(i),',',num2str(j),')');
        Equation = strcat(Var_name_ij,' = ',char(eval(Var_name_ij)),';');
        string2eval = strcat('Equ',num2str(index_equ),' = Equation;');
        eval(string2eval);
        index_equ = index_equ + 1;
    end
end
end
fid = fopen(File_name,'wt');
for k = 1:index_equ-1
    eval(strcat('fprintf(fid,''%s \n'',Equ',num2str(k),');'));
end
fclose(fid);

%Creates Forces file

List_of_variables = {'Force_left','Force_right','Force_pitch','Force_yaw','Force_sail','Force_drag'};
File_name = 'Forces_all.m';

index_equ = 1;
for k = 1:length(List_of_variables) 
Var_name = List_of_variables{k};
size_var = size(eval(Var_name));
for i = 1:size_var(1)
    for j = 1:size_var(2)
        Var_name_ij = strcat(Var_name,'(',num2str(i),',',num2str(j),')');
        Equation = strcat(Var_name_ij,' = ',char(eval(Var_name_ij)),';');
        string2eval = strcat('Equ',num2str(index_equ),' = Equation;');
        eval(string2eval);
        index_equ = index_equ + 1;
    end
end
end
fid = fopen(File_name,'wt');
for k = 1:index_equ-1
    eval(strcat('fprintf(fid,''%s \n'',Equ',num2str(k),');'));
end
fclose(fid);

%Creates Forces file

List_of_variables = {'Force_G_E1','Force_G_E2','Force_G_E3','Torque_G_E1','Torque_G_E2','Torque_G_E3'};
File_name = 'Forces.m';

index_equ = 1;
for k = 1:length(List_of_variables) 
Var_name = List_of_variables{k};
size_var = size(eval(Var_name));
for i = 1:size_var(1)
    for j = 1:size_var(2)
        Var_name_ij = strcat(Var_name,'(',num2str(i),',',num2str(j),')');
        Equation = strcat(Var_name_ij,' = ',char(eval(Var_name_ij)),';');
        string2eval = strcat('Equ',num2str(index_equ),' = Equation;');
        eval(string2eval);
        index_equ = index_equ + 1;
    end
end
end
fid = fopen(File_name,'wt');
for k = 1:index_equ-1
    eval(strcat('fprintf(fid,''%s \n'',Equ',num2str(k),');'));
end
fclose(fid);


toc
