clear all
%close all
load sim_data



load Param_save

% Display

% figure(2)
% subplot(2,3,1)
% plot(time_sim,state_sim(:,7));grid;zoom on;title('dx')
% subplot(2,3,2)
% plot(time_sim,state_sim(:,8));grid;zoom on;title('dy')
% subplot(2,3,3)
% plot(time_sim,state_sim(:,3));grid;zoom on;title('z')
% subplot(2,3,4)
% plot(time_sim,state_sim(:,4));grid;zoom on;title('phi')
% subplot(2,3,5)
% plot(time_sim,state_sim(:,5));grid;zoom on;title('theta')
% subplot(2,3,6)
% plot(time_sim,state_sim(:,6));grid;zoom on;title('psi')
% 

x_foils = param(1);
tanCal_yaw = param(2);
tanAngle_sail = param(5);
G_shift = param(6);
alpha_foils = param(7);
Wind_angle = param(9);
z_foils = param(10);

Datas

% Display flag : 0 -> x-fixed else -> x-moving
flag = 0;
time_scale = 1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%
tic;
time_disp = toc;

Scale_forces = 1e-4;
Scale_forces_emp = 2e-5;

Tfin = time_sim(end);
% 
L_nose = 3;
hullw = 1;hullh = 1;

%%%%%%%%%%%%%%%%%% Graphic matrices %%%%%%%%%%%%%%%

Mat_bridge_E = [x_foils              x_foils              x_foils-Chord_max    x_foils-Chord_max;
                y_foil_left          y_foil_right         y_foil_right         y_foil_left     ;
                z_foils              z_foils              z_foils              z_foils         ];
        
Mat_left_E = [x_foils                 x_foils                             x_foils-Chord_min                              x_foils-Chord_max;%;   x_foils+0.5*Chord];
	          y_foil_left             y_foil_left - cos(alpha_left)*E_foils     y_foil_left-cos(alpha_left)*E_foils                  y_foil_left;%         y_foil_left];
              z_foils                 z_foils-sin(alpha_left)*E_foils           z_foils-sin(alpha_left)*E_foils                      z_foils];%             z_foils];

Mat_right_E = [x_foils                 x_foils                             x_foils-Chord_min                              x_foils-Chord_max;%;   x_foils+0.5*Chord];
               y_foil_right            y_foil_right + cos(alpha_right)*E_foils    y_foil_right+cos(alpha_right)*E_foils   y_foil_right;%         y_foil_left];
               z_foils                 z_foils-sin(alpha_right)*E_foils           z_foils-sin(alpha_right)*E_foils        z_foils];%             z_foils];



Mat_yaw_E =    [x_yaw    x_yaw         x_yaw+Chord_yaw  x_yaw+Chord_yaw;
                y_yaw        y_yaw     y_yaw                y_yaw      ;
                z_foils-hullh  z_yaw-E_yaw   z_yaw-E_yaw        z_foils-hullh];
  
Mat_pitch_E = [x_pitch      x_pitch        x_pitch+Chord_pitch  x_yaw+Chord_pitch   ;
              y_pitch-0.5*Env_pitch  y_pitch+0.5*Env_pitch    y_pitch+0.5*Env_pitch         y_pitch-0.5*Env_pitch;
               z_pitch      z_pitch        z_pitch              z_pitch             ];

Mat_mast_E = [x_sail  x_sail;
              y_sail  y_sail;
              z_foils z_foils+L_mast];

              Den = sqrt(1 + param(5)^2);
              
Mat_sail_E = [x_sail    x_sail           x_sail - Baume/Den;
              y_sail    y_sail           y_sail - Baume*param(5)/Den;
              z_foils+2 z_foils+L_mast   z_foils + 2   ];             


Mat_sides_E =  [x_yaw         x_yaw           Long+x_yaw-L_nose    Long+x_yaw-L_nose    Long+x_yaw-L_nose   x_yaw;
                y_yaw+hullw   y_yaw           y_yaw                y_yaw + hullw        y_yaw-hullw         y_yaw-hullw;       
                z_foils       z_foils-hullh   z_foils-hullh        z_foils              z_foils             z_foils]; 

Mat_rest_E =   [ Long+x_yaw-L_nose  Long+x_yaw-L_nose  Long+x_yaw    Long+x_yaw-L_nose  x_yaw          x_yaw         x_yaw;
                 y_yaw+hullw        y_yaw              y_yaw         y_yaw- hullw       y_yaw+hullw    y_yaw-hullw   y_yaw;
                 z_foils            z_foils-hullh      z_foils       z_foils            z_foils        z_foils       z_foils-hullh]; 
           
               
Mat_deck_E =  [Long+x_yaw    Long+x_yaw-L_nose       x_yaw                x_yaw            Long+x_yaw-L_nose;
               y_yaw         y_yaw-hullw             y_yaw-hullw          y_yaw+hullw      y_yaw+hullw;
               z_foils       z_foils                 z_foils              z_foils          z_foils]; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%




tic
time_disp = toc;

while (time_disp < Tfin)    

    time_disp = toc;
   
    
    t = time_disp/time_scale;
    
    
     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Boat 1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    state = linint([time_sim state_sim],time_disp/time_scale);

    x = state(1);y = state(2); z = state(3); phi = state(4); theta = state(5) ; psi = state(6); 
    dx = state(7);dy = state(8); dz = state(9); dphi = state(10); dtheta = state(11) ; dpsi = state(12);

EE_flat

EE_left = EE_left_flat;
EE_right = EE_right_flat;
EE_yaw = EE_yaw_flat;

Angles

Fac_update = 1;
%Refine EEs
for k = 1:2
    Equ;
   

    EE_left = EE_left - Fac_update*Equ_left/dEqu_left;
    EE_right = EE_right - Fac_update*Equ_right/dEqu_right;
    EE_yaw = EE_yaw - Fac_update*Equ_yaw/dEqu_yaw;
   
end
%  
% % t = 0;
% 
% % Equ
% % 
% % Equ_left
% % Equ_right
% 
% 
if (EE_left+epsilon < 0)
    EE_left = epsilon;
end
if (EE_right+epsilon < 0)
    EE_right = epsilon;
end
if (EE_left > E_foils-epsilon)
    EE_left = E_foils-epsilon;
end
if (EE_right > E_foils-epsilon)
    EE_right = E_foils-epsilon;
end
if (EE_yaw > E_yaw-epsilon)
    EE_yaw = E_yaw-epsilon;
end
if (EE_yaw+epsilon < 0)
    EE_yaw = epsilon;
end

Aero_coeff_control


Forces


GAs
GAsail
%Display forces




Force_sail_disp = [GA_sail';
                   GA_sail' + Scale_forces*Force_sail']*Phimat;

Force_right_disp =  [GA_right';
                     GA_right' + Scale_forces*Force_right']*Phimat;
          
Force_left_disp =  [GA_left';
                    GA_left' + Scale_forces*Force_left']*Phimat;
          
Force_pitch_disp = [GA_pitch';
                    GA_pitch' + Scale_forces_emp*Force_pitch']*Phimat;
          
Force_yaw_disp = [GA_yaw';
                  GA_yaw' + Scale_forces_emp*Force_yaw']*Phimat;
          
              
X_forces = [Force_left_disp(:,1) Force_right_disp(:,1) Force_pitch_disp(:,1) Force_yaw_disp(:,1) Force_sail_disp(:,1)];
Y_forces = [Force_left_disp(:,2) Force_right_disp(:,2) Force_pitch_disp(:,2) Force_yaw_disp(:,2) Force_sail_disp(:,2)];
Z_forces = [Force_left_disp(:,3) Force_right_disp(:,3) Force_pitch_disp(:,3) Force_yaw_disp(:,3) Force_sail_disp(:,3)];


              
% Mat_inter_E = [x_foils                                   x_foils;
%                y_foil_left - cos(alpha_left)*EE_left     y_foil_right + cos(alpha_right)*EE_right;
%                z_foils - sin(alpha_left)*EE_left         z_foils - sin(alpha_right)*EE_right];
            
  
COO_G = [x x;
         y y;
         z z-1];

     x1 = x;y1 = y;z1 = z;
         
Scale_wind = 5e-1;
Mat_apparent_wind = [Phimat'*[x_sail y_sail L_mast]' Phimat'*[x_sail y_sail L_mast]'+Scale_wind*[-dx+Wind_x;-dy+Wind_y;0]]; 
Mat_wind =          [Phimat'*[x_sail y_sail L_mast]' Phimat'*[x_sail y_sail L_mast]'+Scale_wind*[Wind_x;Wind_y;0]]; 

X_forces_display = X_forces + x*ones(size(X_forces));
Y_forces_display = Y_forces + y*ones(size(Y_forces));
Z_forces_display = Z_forces + z*ones(size(Z_forces));
              
COO_display_bridge1 = Phimat'*(Mat_bridge_E) + [x;y;z]*ones(1,4);
COO_display_left1 = Phimat'*(Mat_left_E) + [x;y;z]*ones(1,4);
COO_display_right1 = Phimat'*(Mat_right_E) + [x;y;z]*ones(1,4);

                             
COO_display_sides1 = Phimat'*Mat_sides_E + [x;y;z]*ones(1,6);
COO_display_rest1 = Phimat'*Mat_rest_E + [x;y;z]*ones(1,7);
COO_display_deck1 = Phimat'*Mat_deck_E + [x;y;z]*ones(1,5);

COO_display_yaw1 = Phimat'*(Mat_yaw_E) + [x;y;z]*ones(1,4);

COO_display_pitch1 = Phimat'*(Mat_pitch_E) + [x;y;z]*ones(1,4);
COO_display_mast1 = Phimat'*(Mat_mast_E) + [x;y;z]*ones(1,2);
%COO_display_mast = Phimat'*(Mat_mast_E) + [x;y;z]*ones(1,2);
COO_display_sail1 = Phimat'*(Mat_sail_E) + [x;y;z]*ones(1,3);


%COO_display_inter = Phimat'*(Mat_inter_E) + [x;y;z]*ones(1,2);

COO_display7 = Mat_apparent_wind + [x;y;z]*ones(1,2);
COO_display8 = Mat_wind + [x;y;z]*ones(1,2);

X_min = -20+x1;
X_max = 20+x1;
Y_min = -20+y1;
Y_max = 20+y1;
Sampling = 1;

% X_min = -20+x1;
% X_max = 20+x1;
% Y_min = -20+y1;
% Y_max = 20+y1;
% Sampling = 2;


X_grid = [X_min:Sampling:X_max];
Y_grid = [Y_min:Sampling:Y_max]-G_shift;


  Plot_sea


[faces,vertex,color] = surf2patch(X_grid,Y_grid,Z,Z);


%3D vision

ref_L = 3;

figure(1)
clf


patch('Vertices',vertex,'Faces',faces,'FaceVertexCData',color,'SpecularStrength',0.1,'DiffuseStrength',1,'AmbientStrength',0.8,'SpecularExponent',15,'SpecularColorReflectance',1,'FaceColor','flat','EdgeColor','none','MarkerFaceColor','flat','FaceLighting','gouraud');%,'FaceVertexCData',hsv(6),'FaceColor','flat')

%axis equal
%return
patch('Vertices',[COO_display_sides1';COO_display_bridge1';COO_display_left1';COO_display_right1';COO_display_yaw1';COO_display_pitch1'],'Faces',[1 2 3 4;2 3 5 6;7 8 9 10;11 12 13 14;15 16 17 18;19 20 21 22;23 24 25 26],'FaceVertexCData',[0*ones(26,1) 0*ones(26,1) 1*ones(26,1)],'FaceColor','flat','EdgeColor','k','MarkerFaceColor','flat');%,'FaceVertexCData',hsv(6),'FaceColor','flat')
patch('Vertices',COO_display_rest1','Faces',[1 2 3;2 3 4;5 6 7],'FaceVertexCData',[0*ones(7,1) 0*ones(7,1) 1*ones(7,1)],'FaceColor','flat','EdgeColor','k','MarkerFaceColor','flat');%,'FaceVertexCData',hsv(6),'FaceColor','flat')
patch('Vertices',COO_display_deck1','Faces',[1 2 3 4 5],'FaceVertexCData',[0*ones(5,1) 0*ones(5,1) 1*ones(5,1)],'FaceColor','flat','EdgeColor','k','MarkerFaceColor','flat');%,'FaceVertexCData',hsv(6),'FaceColor','flat')
patch('Vertices',COO_display_sail1','Faces',[1 2 3],'FaceVertexCData',[0*ones(3,1) 0*ones(3,1) 1*ones(3,1)],'FaceColor',[1 1 1],'EdgeColor','none','MarkerFaceColor','flat');%,'FaceVertexCData',hsv(6),'FaceColor','flat')

hold on
 plot3(COO_display_mast1(1,:),COO_display_mast1(2,:),COO_display_mast1(3,:),'b','LineWidth',2)
 hold on
 plot3(X_forces_display,Y_forces_display,Z_forces_display,'r',COO_display7(1,:),COO_display7(2,:),COO_display7(3,:),'c',COO_display8(1,:),COO_display8(2,:),COO_display8(3,:),'--c',COO_G(1,:),COO_G(2,:),COO_G(3,:),'--r','LineWidth',2);%,[0 ref_L],[0 0],[0 0],'--g',[0 0],[0 ref_L],[0 0],'--g',[0 0],[0 0],[0 ref_L],'--g');grid;%zoom

 cmap = colormap('winter');
colormap([cmap(:,1) cmap(:,2) cmap(:,3)])

%camlight

%light('Position',[0 0 100])

camproj perspective 
camva(30)


Cam_pos = state(13:15);
Cam_up = [0 0 1]';
%view(-135,20) %135
campos(Cam_pos);%[x y z]'+ Cam_pos);% + Phimat'*[-2 2 2]')
camtarget([x y-G_shift z]');% + Phimat'*[x_foils y_foil_left/2 z_foils]')
camup(Cam_up);

axis equal
axis off

whitebg([.0 .0 0.0])
   set(gcf,'Color','k')
%%%%%

%%%%%%%%
drawnow

end


figure(4)
subplot(3,4,1)
plot(time_sim,state_sim(:,7),'r');grid;zoom on;title('dx');hold on
subplot(3,4,2)
plot(time_sim,state_sim(:,2),'r');grid;zoom on;title('y');hold on
subplot(3,4,3)
plot(time_sim,state_sim(:,3),'r');grid;zoom on;title('z');hold on
subplot(3,4,4)
plot(time_sim,state_sim(:,8),'r');grid;zoom on;title('dy');hold on
subplot(3,4,5)
plot(time_sim,state_sim(:,9),'r');grid;zoom on;title('dz');hold on
subplot(3,4,6)
plot(time_sim,180*state_sim(:,4)/pi,'r');grid;zoom on;title('phi');hold on
subplot(3,4,7)
plot(time_sim,180*state_sim(:,5)/pi,'r');grid;zoom on;title('theta');hold on
subplot(3,4,8)
plot(time_sim,180*state_sim(:,6)/pi,'r');grid;zoom on;title('psi');hold on
subplot(3,4,9)
plot(time_sim,state_sim(:,8),'r');grid;zoom on;title('dy');hold on
subplot(3,4,10)
plot(time_sim,180*state_sim(:,9)/pi,'r');grid;zoom on;title('dphi');hold on
subplot(3,4,11)
plot(time_sim,180*state_sim(:,10)/pi,'r');grid;zoom on;title('dtheta');hold on
subplot(3,4,12)
plot(time_sim,180*state_sim(:,11)/pi,'r');grid;zoom on;title('dpsi');hold on


figure(5)
plot(time_sim,sqrt(state_sim(:,7).^2 + state_sim(:,8).^2),'r');grid;zoom on;title('Speed') 

figure(6)
clf
subplot(2,2,1)
plot(time_sim,-state_sim(:,1) + state_sim(:,13),'b');grid;zoom on;title('Cam Delta x')
subplot(2,2,2)
plot(time_sim,-state_sim(:,2) + state_sim(:,14),'b');grid;zoom on;title('Cam y')
subplot(2,2,3)
plot(time_sim,-state_sim(:,3) + state_sim(:,15),'b');grid;zoom on;title('Cam z')

figure(7)
clf
subplot(2,2,1)
plot(time_sim,-state_sim(:,7) + state_sim(:,16),'b');grid;zoom on;title('Cam dx')
subplot(2,2,2)
plot(time_sim,-state_sim(:,8) + state_sim(:,17),'b');grid;zoom on;title('Cam dy')
subplot(2,2,3)
plot(time_sim,-state_sim(:,9) + state_sim(:,18),'b');grid;zoom on;title('Cam dz')

figure(8)
clf
plot(time_sim,180*atan(u_sim)/pi);grid;zoom on;title('AoA sail')
