clear all
close all

%param : x_foils, tanCal_yaw, z, phi, tanCal_sail, G_shift, alpha_foils(deg) dx wind_angle

%param = [  0    0    1    0    0.042   0   45   10  pi/2 1];
param = [ 0   0    2    0    0.175    2   45   25.0000    pi/2    0];

% Hydro_find_stat(param)
% return

LB = [  0       -0.1     0   -5*pi/180   0          -6        40        20          0        -1];
UB = [ 3        0.1     2   5*pi/180   0.5       6        60        25         pi         1];



P = [];

options = OPTIMSET;
options.MaxFunEvals = 1e3;

tic
%param = fmincon('Hydro_find_stat_acc',param,[],[],[],[],LB,UB,[],options,P);
%[Cons,E] = Non_lin_con(param);

% return
param = fmincon('Hydro_setpoint',param,[],[],[],[],LB,UB,'',options,P);
 




Optimization_time = toc


%param(10) = -3;

Opt_param.x_foils = param(1);
Opt_param.tanCal_yaw = param(2);
Opt_param.z = param(3);
Opt_param.phi = 180*param(4)/pi;
Opt_param.tanCal_sail = param(5);
Opt_param.G_shift = param(6);
Opt_param.alpha_foils = param(7);
Opt_param.Speed = param(8);
Opt_param.Wind_angle = 180*param(9)/pi;
Opt_param.z_foils = param(10);

Opt_param


z = param(3);phi = param(4);theta = 0;psi = 0;
dx = param(8);dy = 0;dz = 0;dphi = 0;dtheta = 0;dpsi = 0;

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

EE_flat

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


GAs

V_E = Phimat*[dx-Wind_x dy-Wind_y dz]';% + cross(O,GA_sail);

Tan_alpha = V_E(2)/V_E(1);   

tanAngle_sail = (Tan_alpha - tanCal_sail)/(1 + tanCal_sail*Tan_alpha);     

param(5) = tanAngle_sail;

save Param_save param

Angles
Tan_AoA_sail
Angle_sail = 180*atan(tanAngle_sail)/pi

Angle_app_wind = 180*atan(Tan_alpha)/pi
Check_stat
 
 drawnow

 %pause
 
P.x0 = 200;
P.y0 = -45;
P.z0 = 15;


pole_cam = 0.5;



P.alpha_cam = -pole_cam^2; 
P.beta_cam = -2*pole_cam;


P.x_rel = -40;
P.y_rel = -10;%10;
P.z_rel = 5;

P.Gain = 1;


X0 = [0 0 param(3) param(4) 0 0 param(8) 0 0 0 0 0 P.x0 P.y0 P.z0 0 0 0];

Tfin = 10;
P.time_stop = Tfin-10;

P.param = param;

intoptions.AbsTol = 1e-5;
intoptions.RelTol = 1e-3;
intoptions.MaxStep = Tfin/25;
intoptions.Events = 'off';

tic
[time_sim,state_sim]=ode45('Hydro_integ',[0 Tfin],X0,intoptions,P);
%[time_sim,state_sim]=ode45('HydroTest',[0 Tfin],X0,intoptions,P);

 for k = 1:length(time_sim)
     u_sim(:,k) = Hydro_integ(time_sim(k),state_sim(k,:),'output',P);
     %u_sim(:,k) = HydroTest(time_sim(k),state_sim(k,:),'output',P);
 end
Rel_Sim_time = toc/Tfin
 

%q = [x y z phi theta psi]';

save sim_data

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