

x_foils = param(1);
tanCal_yaw = param(2);
z = param(3);
phi = param(4);
tanAngle_sail = param(5);
G_shift = param(6);
alpha_foils = param(7);
dx = param(8);%P.dx0;
Wind_angle = param(9);
z_foils = param(10);

t = 0;
Datas

Wave_amp = 0;

state = [0 0 z phi 0 0 dx 0 0 0 0 0];

%state = [ 453.7740  -91.8618    2.5027   -0.0183    0.0088   -0.5391   11.6111   -7.2397   -0.0212   -0.0015    0.0016   -0.0277];

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
Forces_all

Acceleration



ACC = ddq

GAs
GAsail
save stat_data

Display_stat


% COO_display_inter
% 
% x_grid_left = COO_display_inter(1,1);
% y_grid_left = COO_display_inter(2,1);
% z_foil_left = COO_display_inter(3,1)
% 
% z_sea_left = Wave_amp*sin(2*pi*(x_grid_left*Wave_x + y_grid_left*Wave_y + Wave_speed*t))
% EE_left
% 
% x_grid_right = COO_display_inter(1,2);
% y_grid_right = COO_display_inter(2,2);
% z_foil_right = COO_display_inter(3,2)
% 
% z_sea_right = Wave_amp*sin(2*pi*(x_grid_right*Wave_x + y_grid_right*Wave_y + Wave_speed*t))
% EE_right