function ACC_out = Hydro_find_stat(param,P)

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

%param = [  0.5087   -0.0000    4.0355    0.0185    0.0043         0   45.5480]

%param = fmincon('Hydro_find_stat',param,[],[],[],[],[0 -0.1 0 0 0 0 10],[10 0.1 6 20*pi/180 0.042 10 80])


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



Datas

% Lin_mat_active
% 
% EA_max = max(real(eig(A(3:end,3:end))));

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

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;

GAs

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

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


%tanCal_sail

tanAngle_sail = (V_E(2) - V_E(1)*tanCal_sail)/(V_E(1) + tanCal_sail*V_E(2));     
     
%Gas

Angles



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

Acceleration

ACC = ddq



ACC_out =  0.5*ACC(1:3)'*ACC(1:3) + 0.5*1e2*ACC(4:6)'*ACC(4:6);

%return



%Display_stat
