///////////////////////////////   rp动力学积分部分  //////////////////////////////
// 动力学方程积分部分
// 输入：	StateVar:  状态变量
//			ControlVar:控制变量
//			auxdata: 其他参数
// 输出：	    dState: 状态微分
//			integrand: 积分量
//			path:	   路径约束
#include <iostream>
#include <armadillo>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include "GPM.h"
#include "HCV.h"
using namespace std;
using namespace arma;

int Dynamics_HCV(mat* StateVar, mat* ControlVar, auxdata1 auxdata, mat* dState, mat* integrand, mat* path)
{

	// 动力学模型
	mat State = *StateVar;
	mat Control = *ControlVar;
	//禁飞区设置
#ifndef LIULU20220629_PSLINUX  //原书写方式在Linux下会以为double不能转换为int类型而报错
	int a[] = { 100,48,500000 };
	int b[] = { 90,43,360000 };
	int c[] = { 62,43,320000 };
#else
	int a[] = { 100,48,500e3 };
	int b[] = { 90,43,360e3 };
	int c[] = { 62,43,320e3 };
#endif


	// 常数
	double pi = datum::pi;
	double g = auxdata.g;
	double Re = auxdata.Re;
	double rad2deg = 180 / pi;
	double tc = sqrt(Re / g);
	// 状态量本地化
	vec phi = State.col(0);
	vec lam = State.col(1);
	vec v = State.col(2);
	vec sigma = State.col(3);
	vec m = State.col(4);

	// 控制量本地化
	vec phit = Control.col(0);
	vec alpha = Control.col(1);
	vec niu = Control.col(2);
	// 参量计算
	int n = size(v)(0);
	vec Rou = zeros(n, 1);
	vec H = 27000 * ones(n, 1) / auxdata.re;
	vec Ma = zeros(n, 1);
	vec L = zeros(n, 1);
	vec D = zeros(n, 1);
	vec IspDim = zeros(n, 1);
	vec CT = zeros(n, 1);
	vec Theta = zeros(n, 1);
	vec r = H + 1;
	Get_Rou_Mach(H*Re, v*sqrt(Re*g), &Rou, &Ma);
	alpha2LD_HCV(alpha, Rou, Ma, v*sqrt(Re*g), auxdata, &L, &D);
	vec temp = 0.5*Rou%v*sqrt(Re*g) % v*sqrt(Re*g) * 150 / 100000;
	L = temp%L / g;
	D = temp%D / g;

	// 微分方程
	calp(Ma, phit, alpha, &CT, &IspDim);
	vec T = 0.029*phit%IspDim%Rou%v*sqrt(Re*g) % CT*auxdata.Ac / auxdata.M0;
	vec Isp = IspDim / tc;
	// 加速度计算
	/*推力*/
	vec calpha = cos(alpha);
	vec salpha = sin(alpha);
	vec cniu = cos(niu);
	vec sniu = sin(niu);
	//cout <<  cniu << endl;
	mat acc_P[] = { calpha%T / m, salpha%cniu%T / m, salpha%sniu%T / m };
	mat acc_R[] = { -D / m, cniu%L / m, sniu%L / m };
	vec sphi = sin(phi);
	vec cphi = cos(phi);
	vec sTheta = sin(Theta);
	vec cTheta = cos(Theta);
	vec csigma = cos(sigma);
	vec ssigma = sin(sigma);
	mat gr = -auxdata.GMe / ((r*auxdata.Re) % (r*auxdata.Re)) % (1 + 1.5*auxdata.J2*(auxdata.ae / (r*auxdata.re)) % (auxdata.ae / (r*auxdata.re)) % (1 - 5 * sphi%sphi));          //   gr[列向量]
	mat	gw = -2 * auxdata.GMe / ((r*auxdata.re) % (r*auxdata.re))*1.5*auxdata.J2 % (auxdata.ae / (r*auxdata.re)) % (auxdata.ae / (r*auxdata.re)) % sphi;
	mat	gr_Vec[] = { sTheta%gr, cTheta%gr,zeros(n,1) };
	mat gw_Vec[] = { gw % (csigma%cTheta%cphi + sTheta%sphi), gw % (cTheta%sphi - csigma%sTheta%cphi), gw%ssigma%cphi };
	mat acc_gr[] = { sTheta%gr / g, cTheta%gr / g,zeros(n,1) };
	mat	acc_gw[] = { gw % (csigma%cTheta%cphi + sTheta%sphi) / g, gw % (cTheta%sphi - csigma%sTheta%cphi) / g, gw%ssigma%cphi / g };
	// 惯性力[无量纲]
	mat Iner_Vec[] = { cphi%cphi%sTheta % (auxdata.omige*auxdata.omige*r*auxdata.re) - cphi%sphi%csigma%cTheta % (auxdata.omige*auxdata.omige*r*auxdata.re), auxdata.omige*auxdata.omige*r*auxdata.re % (cphi%cphi%cTheta + cphi%sphi%csigma%sTheta), auxdata.omige*auxdata.omige*r*auxdata.re%cphi%sphi%ssigma };
	mat acc_Iner[] = { (cphi%cphi%sTheta % (auxdata.omige*auxdata.omige*r*auxdata.re) - cphi%sphi%csigma%cTheta % (auxdata.omige*auxdata.omige*r*auxdata.re)) / g, auxdata.omige*auxdata.omige*r*auxdata.re % (cphi%cphi%cTheta + cphi%sphi%csigma%sTheta) / g, auxdata.omige*auxdata.omige*auxdata.re*r%cphi%sphi%ssigma / g };
	// 科式力[无量纲]
	mat Cori_Vec[] = { zeros(n,1), 2 * auxdata.omige*v*sqrt(Re*g) % ssigma%cphi, 2 * auxdata.omige*v*sqrt(Re*g) % (csigma%sTheta / cTheta%cphi - sphi) };
	mat acc_Cori[] = { zeros(n,1), 2 * auxdata.omige*v*sqrt(Re*g) % ssigma%cphi / g, 2 * auxdata.omige*v*sqrt(Re*g) % (csigma%sTheta / cTheta%cphi - sphi) / g };
	// 状态量求导
	vec Hdot = v%sTheta;
	vec phidot = v%cTheta%csigma / r;
	vec lamdot = v%cTheta%ssigma / r / cphi;
	vec vdot = acc_P[0] + acc_R[0] + acc_gr[0] + acc_gw[0] + acc_Iner[0] + acc_Cori[0];
	vec Thetadot = acc_P[1] / v + acc_R[1] / v + acc_gr[1] / v + v%cTheta / r + acc_gw[1] / v + acc_Iner[1] / v + acc_Cori[1] / v;
	vec sigmadot = acc_P[2] / v / cTheta + acc_R[2] / v / cTheta + acc_gr[2] + v%sphi / cphi%cTheta%ssigma / r - acc_gw[2] / v / cTheta + acc_Iner[2] / v / cTheta - acc_Cori[2] / v;
	vec mdot = -T / Isp;
	vec hdot = v%sTheta;
	// 路径约束设置
	vec path1 = vdot;
	vec path2 = Thetadot;
	vec path3 = acos(cos(phi)*cos(a[1] / rad2deg) % cos(a[0] / rad2deg - lam) + sin(phi)*sin(a[1] / rad2deg));
	vec path4 = acos(cos(phi)*cos(b[1] / rad2deg) % cos(b[0] / rad2deg - lam) + sin(phi)*sin(b[1] / rad2deg));
	vec path5 = acos(cos(phi)*cos(c[1] / rad2deg) % cos(c[0] / rad2deg - lam) + sin(phi)*sin(c[1] / rad2deg));
	mat dState0(n, 5);
	dState0.col(0) = phidot;
	dState0.col(1) = lamdot;
	dState0.col(2) = vdot;
	dState0.col(3) = sigmadot;
	dState0.col(4) = mdot;
	mat path0(n, 5);
	path0.col(0) = path1;
	path0.col(1) = path2;
	path0.col(2) = path3;
	path0.col(3) = path4;
	path0.col(4) = path5;
	*dState = dState0;
	*path = path0;
	(*integrand).reset();
	int aaaa = 1;
	return 1;
}
int Objective_HCV(input0* input, double* output, mat* event) {
	double pi = datum::pi;
	double rad2deg = 180 / pi;
	int re = 6371004;
	//飞行器末端位置
	double state[] = { 116,47,500 };
	double is1 = (*input).phase.initialstate(0);
	double fs1 = (*input).phase.finalstate(0);
	double fs2 = (*input).phase.finalstate(1);
	(*output) = -(*input).phase.finalstate(4);
	mat event_temp;
	event_temp.reset();
	(*event) = event_temp;
	return 1;
}