include "MMG.h"

include "math.h"

define PI 3.14159265

MMG::MMG()
{

p = 1025;
L = 3.898;
B = 0.42;
Beam = 2.01;
d = 0.19;
m = 194.3;
CB = 0.624;
Izz = 214.221;
L = 3.898;
ShipScale[0] = 3.898; ShipScale[1] = 0.42; ShipScale[2] = 0.19;
ShipScale[3] = 0.624; ShipScale[4] = 0.19; ShipScale[5] = 1.9431;
mx = 8.7966; my = 2.323994; Jzz = 204.136;
Xvv = -0.08; Xrr = 0.006; Xvr = -0.0045;
Yv = -0.2437; Yr = 0.0363; Yvv = -1.0242; Yvr = -0.1647; Yrr = -0.0742;
Yvvr = -0.2261; Yvrr = -1.0102;
Nv = -0.0975; Nr = -0.0439; Nvv = 0.1039; Nrr = -0.0536;
Nvrr = -0.0342; Nvvr = -0.6425;
WindParam[0] = 0;       //风的参数？？？
WindParam[1] = 0;
WaveParam[0] = 0;
WaveParam[1] = 0;
ECFI_Origin.lat = 45.794608;     //初始纬度
 ECFI_Origin.lon = 126.526928;    //初始经度
ECFI_Pos = ECFI_Origin;          //当前经纬度

}

Force MMG::HydroDynamics()
{

double Angle;
double Equ, Eqv, Eqr;
Angle = WaveParam[1] - ShipPos.N;
Equ = ShipPos.u - WaveParam[0] * cos(Angle);
Eqv = ShipPos.v - WaveParam[0] * sin(Angle);
Eqr = ShipPos.r;
//片体进速
double l_v, r_v;
l_v = Equ - Eqr * Beam / 2;
r_v = Equ + Eqr * Beam / 2;
// 左片体水动力
double V_l, Rn, Cf;
double k, Cr, Ct, S, Xu;
V_l = sqrt(pow(l_v, 2) + pow(Eqv, 2));
Rn = V_l * ShipScale[0] / (1.103 * 10E-6);
Cf = 0.075 / pow((log10(Rn) - 2), 2);
k = 0.12;
Cr = k * l_v / pow((9.8 * ShipScale[0]), 0.5);
Ct = Cf + Cr + 0.004;
S = 5.143;
Xu = -0.5 * Ct * p * S * pow(l_v, 2);
double vw, rw;
if (V_l == 0)
{
    vw = 0;
    rw = 0;
}
else
{
    vw = Eqv / V_l;
    rw = Eqr * ShipScale[0] / V_l;
}
double XH_L, YH_L, NH_L;
XH_L = Xu + (Xvv * pow(vw, 2) + Xvr * vw * rw + Xrr * pow(rw, 2)) * (0.5 * p * pow(V_l, 2) * ShipScale[0] * ShipScale[4]);
YH_L = (Yv * vw + Yr * rw + Yvv * fabs(vw) * vw + Yrr * fabs(rw) * rw + Yvr * fabs(vw) * rw + Yvvr * pow(vw, 2) * rw + Yvrr * vw * pow(rw, 2)) * (0.5 * p * pow(V_l, 2) * ShipScale[0] * ShipScale[4]);
NH_L = (Nv * vw + Nr * rw + Nvv * fabs(vw) * vw + Nvvr * pow(vw, 2) * rw + Nvrr * vw * pow(rw, 2) + Nrr * fabs(rw) * rw) * (0.5 * p * pow(V_l, 2) * pow(ShipScale[0], 2) * ShipScale[4]);
//右片体水动力
double V_r;
V_r = sqrt(pow(r_v, 2) + pow(Eqv, 2));
Rn = V_r * ShipScale[0] / (1.103 * 10E-6);
Cf = 0.075 / pow((log10(Rn) - 2), 2);
k = 0.12;
Cr = k * r_v / (9.8 * pow(ShipScale[0], 0.5));
Ct = Cf + Cr + 0.004;
S = 5.143;
Xu = -0.5 * Ct * p * S * pow(r_v, 2);
if (V_r == 0)
{
    vw = 0;
    rw = 0;
}
else
{
    vw = Eqv / V_r;
    rw = Eqr * ShipScale[0] / V_r;
}
double XH_R, YH_R, NH_R;
XH_R = Xu + (Xvv * pow(vw, 2) + Xvr * vw * rw + Xrr * pow(rw, 2)) * (0.5 * p * pow(V_r, 2) * ShipScale[0] * ShipScale[4]);
YH_R = (Yv * vw + Yr * rw + Yvv * fabs(vw) * vw + Yrr * fabs(rw) * rw + Yvr * fabs(vw) * rw + Yvvr * pow(vw, 2) * rw + Yvrr * vw * pow(rw, 2)) * (0.5 * p * pow(V_r, 2) * ShipScale[0] * ShipScale[4]);
NH_R = (Nv * vw + Nr * rw + Nvv * fabs(vw) * vw + Nvvr * pow(vw, 2) * rw + Nvrr * vw * pow(rw, 2) + Nrr * fabs(rw) * rw) * (0.5 * p * pow(V_r, 2) * pow(ShipScale[0], 2) * ShipScale[4]);
// 总水动力
double XH, YH, NH;
XH = XH_L + XH_R;
YH = YH_L + YH_R;
NH = NH_L + NH_R + (XH_L - XH_R) * Beam / 2;
HrdroForce.X = XH;
HrdroForce.Y = YH;
HrdroForce.N = NH;
return HrdroForce;

}

Force MMG::Wind()
{

double CDL_Af0, CDL_AfPI;
double Afw, Alw, CDt, sigma, A_param, Density_air; // Afw Alw 迎风投影面积  Density air 空气密度
CDL_Af0 = 0.55;
CDL_AfPI = 0.65;
Afw = 0.8;
Alw = 1.5776;
CDt = 0.85;
sigma = 0.6;
A_param = Afw / Alw;
Density_air = 1.204;
// 初始化
double Wind_V, Wind_Direc;
Wind_V = WindParam[0];
Wind_Direc = WindParam[1];
// 基本计算-攻角
double Relative_Angle, temp_gain;
Relative_Angle = Wind_Direc - ShipPos.Yaw;
temp_gain = floor(Relative_Angle / (2 * PI));
Relative_Angle = Relative_Angle - temp_gain * 2 * PI;
double Attack_Angle;
if ((Relative_Angle  >= 0) && (Relative_Angle <= PI))
    Attack_Angle = PI - Relative_Angle;
else if ((Relative_Angle > PI) && (Relative_Angle <= 2 * PI))
    Attack_Angle = Relative_Angle - PI;
else
    Attack_Angle = 0;
// 相对速度
double R_u, R_v, Attack_V;
R_u = ShipPos.u - Wind_V * cos(Relative_Angle);
R_v = ShipPos.v - Wind_V * sin(Relative_Angle);
Attack_V = sqrt(R_u * R_u + R_v * R_v);
// 算风压系数
double CDL, CX, CY, CN;
CDL = ((CDL_AfPI - CDL_Af0) * (Attack_Angle / PI) + CDL_Af0 *
    0) * A_param;
CX = -CDL / A_param * cos(Attack_Angle) / (1 - sigma * (1 - CDL / CDt) * pow(sin(2 * Attack_Angle), 2) / 2);
if (Relative_Angle < PI)
    CY = CDt * sin(Attack_Angle) / (1 - sigma * (1 - CDL / CDt) * pow(sin(2 * Attack_Angle), 2) / 2);
else
    CY = -CDt * sin(Attack_Angle) / (1 - sigma * (1 - CDL / CDt) * pow(sin(2 * Attack_Angle), 2) / 2);
CN = (0.13 - 0.18 * (Attack_Angle - 0.5 * PI)) * CY;
// 算风力
double FWX, FWY, FWN;
FWX = 0.5 * Density_air * CX * pow(Attack_V, 2) * Afw;
FWY = 0.5 * Density_air * CY * pow(Attack_V, 2) * Alw;
FWN = 0.5 * Density_air * CN * pow(Attack_V, 2) * 4.65;
WindForce.X = FWX;
WindForce.Y = FWY;
WindForce.N = FWN;
return WindForce;

}

Force MMG::Propusion(double Xp, double Yp, double Np)
{

double tar_v, tar_ang;
double vel_err, yaw_err;
double kp_v, kp_y;
double delta_Xp, delta_Np;
kp_v = 2; kp_y = 1; 
tar_v = 3.0; tar_ang = 30.0 * PI / 180;
vel_err = tar_v - ShipPos.u;
yaw_err = tar_ang = ShipPos.Yaw;
delta_Xp = kp_v * vel_err;
delta_Np = kp_y * yaw_err;
PropusionForce.X = PropusionForce.X + delta_Xp;
PropusionForce.N = delta_Np;
if (PropusionForce.X > 5000)
    PropusionForce.X = 5000;
Xp = tar_v * (194.3 + 8.7966) - (194.3 + 2.323994) * ShipPos.v * ShipPos.r - HrdroForce.X - WindForce.X;
Yp = 20;
Np = tar_ang;
PropusionForce.X = Xp;
PropusionForce.Y = Yp;
PropusionForce.N = Np;
return PropusionForce;

}

State MMG::DerivateUpdate(State recState)
{

// 流和水动力
HydroDynamics();
Wind();
Propusion(PropusionForce.X, PropusionForce.Y, PropusionForce.N);
State OUT;
OUT.u = ((HrdroForce.X + PropusionForce.X + WindForce.X) + (m + my) * ShipPos.v * ShipPos.r) / (m + mx);
OUT.v = ((HrdroForce.Y + PropusionForce.Y + WindForce.Y) - (m + mx) * ShipPos.u * ShipPos.r) / (m + my);
OUT.r = (HrdroForce.N + PropusionForce.N + WindForce.N) / (Izz + Jzz);
OUT.N = ShipPos.u * cos(ShipPos.Yaw) - ShipPos.v * sin(ShipPos.Yaw); 
OUT.E = ShipPos.u * sin(ShipPos.Yaw) + ShipPos.v * cos(ShipPos.Yaw);
OUT.Yaw = ShipPos.r;
return OUT;

}

State MMG::RungeKutta4(State recState, double h)
{

State K1, K2, K3, K4;
State Temp;
Temp = recState;
K1 = DerivateUpdate(Temp);
Temp.u = Temp.u + h * K1.u / 2;     
Temp.v = Temp.v + h * K1.v / 2;
Temp.r = Temp.r + h * K1.r / 2;
Temp.E += h * K1.E / 2;
Temp.N += h * K1.N / 2;
Temp.Yaw += h * K1.Yaw / 2;
K2 = DerivateUpdate(Temp);
Temp.u = Temp.u + h * K2.u / 2;
Temp.v = Temp.v + h * K2.v / 2;
Temp.r = Temp.r + h * K2.r / 2;
Temp.E = Temp.E + h * K2.E / 2;
Temp.N = Temp.N + h * K2.N / 2;
Temp.Yaw = Temp.Yaw + h * K2.Yaw / 2;
K3 = DerivateUpdate(Temp);
Temp.u = Temp.u + h * K3.u;
Temp.v = Temp.v + h * K3.v;
Temp.r = Temp.r + h * K3.r;
Temp.E = Temp.E + h * K3.E;
Temp.N = Temp.N + h * K3.N;
Temp.Yaw = Temp.Yaw + h * K3.Yaw;
K4 = DerivateUpdate(Temp);
State NextState;
NextState.u = recState.u + h * (K1.u + 2 * K2.u + 2 * K3.u + K4.u) / 6;
NextState.v = recState.v + h * (K1.v + 2 * K2.v + 2 * K3.v + K4.v) / 6;
NextState.r = recState.r + h * (K1.r + 2 * K2.r + 2 * K3.r + K4.r) / 6;
NextState.E = recState.E + h * (K1.E + 2 * K2.E + 2 * K3.E + K4.E) / 6;
NextState.N = recState.N + h * (K1.N + 2 * K2.N + 2 * K3.N + K4.N) / 6;
NextState.Yaw = recState.Yaw + h * (K1.Yaw + 2 * K2.Yaw + 2 * K3.Yaw + K4.Yaw) / 6;
if (recState.Yaw > NextState.Yaw)
{
    printf(" ");
}
if (fabs(NextState.Yaw) > 2 * PI)
{
    //if(fabs(NextState.Yaw)<(-3*PI))
    //{        nt a;
    //    a=floor(NextState.Yaw/(2*PI));
    //}
    NextState.Yaw = NextState.Yaw - floor(NextState.Yaw / (2 * PI)) * 2 * PI;
}
if (NextState.Yaw < 0)
    NextState.Yaw = NextState.Yaw + 2 * PI;
ShipPos = NextState;
ECFI_Pos.lat = ECFI_Origin.lat + NextState.N / (30.9 * 3600);
ECFI_Pos.lon = ECFI_Origin.lon + NextState.E / (1000 * (111.413 * cos(ECFI_Pos.lat * 3.1415927 / 3600 / 180) - 0.094 * cos(ECFI_Pos.lat * 3.1415927 / 60 / 3600)));
return NextState;

}

void MMG::Slover(double t, double h, State InitialPos)
{

int step;//定义步长
step = (int)t / h;//时间总长/步长=积分多少步
State recState = InitialPos;
State nextState;
FILE* data;
if ((data = fopen("./data.txt", "w")) != NULL)
{
for (int i = 0; i <= step; i++)
{
    nextState = RungeKutta4(recState, h);
    printf("u=%f ,v=%f ,r=%f ,N=%f ,E=%f ,Yaw=%f\n", nextState.u, nextState.v, nextState.r, nextState.N, nextState.E, nextState.Yaw * 180 / PI);
    recState = nextState;
                fprintf(data, "%.7f %.7f %f %f %f %f %f %f \n", ECFI_Pos.lat, ECFI_Pos.lon, nextState.N, nextState.E, nextState.Yaw, nextState.u, nextState.v, nextState.r);
                fflush(data);
}
}

fclose(data);

}