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

%Body 5 (Main body)


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   MODEL STRUCTURE:
%
%   Forces in E & State                -> Accelerations
%   State & CL/CD & AR % Aeras         -> Forces in E
%   Tan of the AoAs                    -> CL/CD
%   State                              -> Tan of the AoAs
%
%   Angles.m         ->    EE / Tan of AoAs
%   Aero_coeff.m     ->    Foils aeras / Aspect ratios / CLs-CDs
%   Forces.m         ->    Forces in E / Accelerations
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%% Inertias %%%%%%%%%%%%% 
I =     [Ix   0      0;
          0     Iy   0;
          0     0      Iz];
  

%%%%%%%%%%%%%%%%%%%%%%
%                    %
% COMPUTE LAGRANGIAN %
%                    % 
%%%%%%%%%%%%%%%%%%%%%%



%KINETIC ENERGY

%Rotation bringing e to E
%Re3 = [ cos(phi) sin(phi) 0;
%      -sin(phi) cos(phi) 0;
%            0          0     1];
        
Re3 = [cos(psi) sin(psi) 0;
      -sin(psi) cos(psi) 0;
            0          0     1];
        
Re2 = [cos(theta)      0  -sin(theta);
            0          1      0;
       sin(theta)      0   cos(theta)];
  
Re1 = [1      0          0;
      0   cos(phi)  sin(phi);
      0  -sin(phi)  cos(phi)];

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

e_phi = Phimat(:,1);
e_theta = sin(phi)*Phimat(:,3) + cos(phi)*Phimat(:,2);   
e_psi = [0 0 1]'; 

O = simple(dphi*e_phi + dtheta*e_theta + dpsi*e_psi);

V_E = simple(Phimat*[dx;dy;dz]);

dPhimat = simple(diff(Phimat,phi)*dphi + diff(Phimat,theta)*dtheta + diff(Phimat,psi)*dpsi);

%%%%%   ATTENTION : Check_O MUST BE NULL !!!! %%%%%%%%%%%%%%
Check_O = simple(dPhimat'*O)



V = [dx;dy;dz];
       

Ecin = simple(0.5*O'*I*O + 0.5*M*V'*V);
    

%POTENTIAL ENERGY


E_pot = M*g*z;

%Lagrangian
L = simplify(Ecin - E_pot);


%%%%%%%%%%%%%%%%%%%%%%%%%%
%                        %
% COMPUTE VIRTUAL FORCES %
%                        % 
%%%%%%%%%%%%%%%%%%%%%%%%%%

syms Force_G_E1 Force_G_E2 Force_G_E3 real
syms Torque_G_E1 Torque_G_E2 Torque_G_E3 real

Force_G_E = [Force_G_E1;Force_G_E2;Force_G_E3];
Torque_G_E = [Torque_G_E1;Torque_G_E2;Torque_G_E3];

Fx = (dot(Phimat'*Force_G_E,[1 0 0]'));
Fy = (dot(Phimat'*Force_G_E,[0 1 0]'));
Fz = (dot(Phimat'*Force_G_E,[0 0 1]')); 

Fphi   = (dot(Torque_G_E,e_phi));
Ftheta = (dot(Torque_G_E,e_theta));
Fpsi   = (dot(Torque_G_E,e_psi));
  
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
%MT are the command torques, i.e. torques compensated for the air friction
Fq = [Fx;Fy;Fz;Fphi;Ftheta;Fpsi];
%Equations
Ldq = jacobian(L,dq);
Lq = jacobian(L,q);
Ldqq = jacobian(Ldq,q);


%Equations: A*accelerations = B
A = simple(jacobian(Ldq,dq));
B = simple((Fq + Lq' - Ldqq*dq));
% 
ddq = simple(inv(A)*B);

%Creates Phimat file

List_of_variables = {'Phimat'};
File_name = 'Phimat_comp.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 Speeds file

List_of_variables = {'O','V_E'};
File_name = 'Speeds.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 Acceleration file

List_of_variables = {'ddq'};
File_name = 'Acceleration.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






tic
disp('Starting C++ code generation')

%Generate c++ files:
clear cfun;
clear hdecl;

%.cpp:
cfun{1}='#include "math.h" \n#include "mex.h" \n#include "string.h" \n\n#include "Datas.h" \n#include "dataFillings.h" \n#include "analytics_dyn.h" \n\n';
cfun{2}='#ifndef PI_DEF \n#define pi 3.141592653589793 \n#define PI 3.141592653589793 \n#endif \n\n'; 
cfun{3}=['// File generated on ' date ', Basile.Graf@epfl.ch \n\n\n'];

%.h:
hdecl{1}=['#ifndef ANALYTICS_DYN_H_DEF \n#define ANALYTICS_DYN_H_DEF \n \n'];
hdecl{2}=['// File generated on ' date ', Basile.Graf@epfl.ch \n\n\n'];


%Acceleration:
varName = {'ddq'};
expr =  {ddq};
[cfun, hdecl] = sym2c(varName, expr, 'acceleration_compute', cfun, hdecl);



hdecl{length(hdecl)+1}=['#endif \n'];


fid = fopen('analytics_dyn.cpp','wt');
for k = 1:length(cfun)
   fprintf(fid,cfun{k}); 
end
fclose(fid);

fid = fopen('analytics_dyn.h','wt');
for k = 1:length(hdecl)
   fprintf(fid,hdecl{k}); 
end
fclose(fid);

toc
disp('C++ code generation finished')