clear all

load stat_data

L_nose = 3;




Scale_forces = 1e-4;
Scale_forces_emp = 2e-4;
hullw = 1;hullh = 1;


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)];
              


%%%%%%%%%%%%%%%%%% 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]; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
              
              
COO_G = [x x;
         y y;
         z z-1];
              
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_bridge = Phimat'*(Mat_bridge_E) + [x;y;z]*ones(1,4);
COO_display_left = Phimat'*(Mat_left_E) + [x;y;z]*ones(1,4);
COO_display_right = Phimat'*(Mat_right_E) + [x;y;z]*ones(1,4);

                      
                             
COO_display_sides = Phimat'*Mat_sides_E + [x;y;z]*ones(1,6);
COO_display_rest = Phimat'*Mat_rest_E + [x;y;z]*ones(1,7);
COO_display_deck = Phimat'*Mat_deck_E + [x;y;z]*ones(1,5);

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

COO_display_pitch = Phimat'*(Mat_pitch_E) + [x;y;z]*ones(1,4);
COO_display_mast = Phimat'*(Mat_mast_E) + [x;y;z]*ones(1,2);
COO_display_sail = Phimat'*(Mat_sail_E) + [x;y;z]*ones(1,3);


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

             
%3D vision

%Forces_display(1,:),Forces_display(2,:),Forces_display(3,:),'r',
%plot3(COO_display(1,:),COO_display(2,:),COO_display(3,:),'b');grid;zoom on;axis equal;
ref_L = 3;


%plot3(X_forces_display,Y_forces_display,Z_forces_display,'r',COO_display_bridge(1,:),COO_display_bridge(2,:),COO_display_bridge(3,:),'b',COO_display_left(1,:),COO_display_left(2,:),COO_display_left(3,:),'b',COO_display_right(1,:),COO_display_right(2,:),COO_display_right(3,:),'b',COO_display2(1,:),COO_display2(2,:),COO_display2(3,:),'b',COO_display3(1,:),COO_display3(2,:),COO_display3(3,:),'b',COO_display4(1,:),COO_display4(2,:),COO_display4(3,:),'b',COO_display5(1,:),COO_display5(2,:),COO_display5(3,:),'b',COO_display6(1,:),COO_display6(2,:),COO_display6(3,:),'c',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');grid;%,[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 on;
%plot3(COO_display1(1,:),COO_display1(2,:),COO_display1(3,:),'b',COO_display2(1,:),COO_display2(2,:),COO_display2(3,:),'b',COO_display3(1,:),COO_display3(2,:),COO_display3(3,:),'b',COO_display4(1,:),COO_display4(2,:),COO_display4(3,:),'b',COO_display5(1,:),COO_display5(2,:),COO_display5(3,:),'b',COO_display6(1,:),COO_display6(2,:),COO_display6(3,:),'c',[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 on;axis equal

%plot3(X_forces_display,Y_forces_display,Z_forces_display,'r',COO_display5(1,:),COO_display5(2,:),COO_display5(3,:),'b',COO_display6(1,:),COO_display6(2,:),COO_display6(3,:),'c',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');%,[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 on;
%hold on

%hold on
%tic
%Plot_sea
%hold on
figure(1)
clf
Sampling = 0.2;
X_min = -20;
X_max = 20;
Y_min = -20;
Y_max = 20;

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);
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')


patch('Vertices',[COO_display_sides';COO_display_bridge';COO_display_left';COO_display_right';COO_display_yaw';COO_display_pitch'],'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_rest','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_deck','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_sail','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_mast(1,:),COO_display_mast(2,:),COO_display_mast(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 on;

% fill3(COO_display_bridge(1,:),COO_display_bridge(2,:),COO_display_bridge(3,:),'b',COO_display_left(1,:),COO_display_left(2,:),COO_display_left(3,:),'b',COO_display_right(1,:),COO_display_right(2,:),COO_display_right(3,:),'b',COO_display_deck(1,:),COO_display_deck(2,:),COO_display_deck(3,:),'b',COO_display_yaw(1,:),COO_display_yaw(2,:),COO_display_yaw(3,:),'b',COO_display_pitch(1,:),COO_display_pitch(2,:),COO_display_pitch(3,:),'b','SpecularStrength',0.8,'DiffuseStrength',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 on;
% plot3(COO_display5(1,:),COO_display5(2,:),COO_display5(3,:),'b','LineWidth',2)

set(gcf,'Color','k')
colormap('winter')
%camlight headlight 
%COO_display_inter(1,:),COO_display_inter(2,:),COO_display_inter(3,:),'c'

view(65,15)
axis equal
whitebg([.0 .0 0.0])

  %drawnow
  %return



camproj perspective 
camva(30)


Dist = 60;

HD = -20*pi/180;

AZ = 25*pi/180;

    
     
    Re3 = [cos(HD) sin(HD) 0;
      -sin(HD) cos(HD) 0;
            0          0     1];
        
    Re2 = [cos(AZ)      0  -sin(AZ);
            0          1      0;
           sin(AZ)      0   cos(AZ)];

    Cam_pos = Re3*Re2*[Dist;0;0];

    campos(Cam_pos);% + Phimat'*[-2 2 2]')
    camtarget([x y-G_shift z]');% + Phimat'*[x_foils y_foil_left/2 z_foils]')
    camup([0 0 1]');

    camproj perspective 

    set(gcf,'Color','k')

    drawnow
  