///////////////////////////////   RLV动力学积分部分  //////////////////////////////
// 动力学方程积分部分
// 输入：	StateVar:  状态变量
//			ControlVar:控制变量
//			auxdata:   其他参数
// 输出：	dState：   状态微分
//			integrand: 积分量
//			path:	   路径约束
// 方法：无
// 编写：李兆亭
// 时间：2020 / 09 / 17
// 其他：由用户自定义
#include <iostream>
#include <armadillo>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include "GPM.h"

#include <math.h>

using namespace std;
using namespace arma;

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

	// 动力学模型
	mat State = *StateVar;
	mat Control = *ControlVar;
	// 常数
	double g = auxdata.g;
	double Re = auxdata.Re;
	// 状态量本地化
	vec v = State.col(0);
	vec theta = State.col(1);
	vec psai = State.col(2);
	vec r = State.col(3);
	vec lamda = State.col(4);
	vec phi = State.col(5);
	// 控制量本地化
	vec alpha = Control.col(0);
	vec sigma = Control.col(1);
	// 参量计算
	vec H = r*Re - Re;
	int n = size(H)(0);
	vec Rou = zeros(n, 1);
	vec Ma = zeros(n, 1);
	vec L = zeros(n, 1);
	vec D = zeros(n, 1);
	Get_Rou_Mach( H, v*sqrt(Re*g), &Rou, &Ma);
	alpha2LD(alpha, Rou, Ma, v*sqrt(Re*g), auxdata, &L, &D);
	L = L / g;    // 这里是归一化
	D = D / g;
	// 微分方程
	vec dv = -D - sin(theta);
	vec dtheta = L%cos(sigma) / v - cos(theta) / v + v%cos(theta)/ r;
	vec dpsai = L%sin(sigma) / v / cos(theta) + v%tan(phi)%cos(theta)%sin(psai)/ r;
	vec dr = v%sin(theta);
	vec dlamda = v%cos(theta)%sin(psai)/ r/ cos(phi);
	vec dphi = v%cos(theta)%cos(psai)/ r;

	double C1_v = auxdata.C1_v;								// 与热流密度相关参数
	double Rd_v = auxdata.Rd_v;								// 与热流密度相关参数
	double n_v = auxdata.n_v;								// 与热流密度相关参数
	double m_v = auxdata.m_v;								// 与热流密度相关参数

	vec path1 = sqrt(L%L + D%D);
	vec path2 = 0.5*Rou%v%v*Re*g;

#ifdef LUOYC20220701
	printf("C1_v = %lf, Rd_v = %lf, n_v = %lf, m_v = %lf\n", C1_v, Rd_v, n_v, m_v);
	printf("Rou = \n");
	for (int i = 0; i < Rou.n_elem; i++)
	{
		printf("%lf ", *(Rou.mem + i));
	}
	printf("\n");
	printf("V = \n");
	for (int i = 0; i < v.n_elem; i++)
	{
		printf("%lf ", *(v.mem + i));
	}
	printf("\n");
#endif

#ifdef LUOYC20220701
	double tmp1 = sqrt(Rd_v);
	double tmp2 = sqrt(Re * g);
	double tmp3 = C1_v / tmp1;
	// printf("tmp1 = %lf, tmp2 = %lf, tmp3 = %lf\n", tmp1, tmp2, tmp3);
	vec vtmp1 = pow(Rou, n_v);
	/*
	int len = Rou.n_elem;
	double rou[len];
	for(int l = 0; l < len; l++)
	{
		rou[l] = *(Rou.mem + l);
	}

	printf("pow(Rou, n_v) = \n");
	for (int i = 0; i < vtmp1.n_elem; i++)
	{
		printf("%lf ", *(vtmp1.mem + i));
	}
	printf("\n");
	*/
	vec vtmp2 = v * tmp2;
	printf("vtmp2 = \n");
	for (int i = 0; i < vtmp2.n_elem; i++)
	{
		printf("%lf ", *(vtmp2.mem + i));
}
	printf("\n");

	vec vtmp3 = pow(vtmp2, m_v);
	printf("vtmp3 = \n");
#if 0
	for (int i = 0; i < vtmp3.n_elem; i++)
	{
		if(isnan(*(vtmp2.mem + i)))
		{
			*(vtmp3.mem + i) *= 0*log((double)0);

		}
		printf("%lf ", *(vtmp3.mem + i));
	}
	printf("\n");
#endif

	vec path3 = C1_v / tmp1 * vtmp1 % vtmp3;

#else
	vec path3 = C1_v / sqrt(Rd_v)*pow(Rou, n_v)%pow((v*sqrt(Re*g)),m_v);
#endif

#ifdef LUOYC20220701
	printf("Dynamics123 path3 = :\n");
	for (int j = 0; j < 6; j++)
	{
		printf("%lf ", *(path3.mem + j));
	}
	printf("\n");
#endif

	mat dState0(n, 6);
	dState0.col(0) = dv;   
	dState0.col(1) = dtheta;
	dState0.col(2) = dpsai;
	dState0.col(3) = dr;
	dState0.col(4) = dlamda;
	dState0.col(5) = dphi;
	mat path0(n, 3);
	path0.col(0) = path1;
	path0.col(1) = path2;
	path0.col(2) = path3;
	mat integrand0(n, 1);
	integrand0.col(0) = abs(dtheta);
	*dState = dState0;
#ifdef LUOYC20220706
	printf("RLV_example row = %d, col = %d, n_elem = %d\n", dState->n_rows, dState->n_cols, dState->n_elem);
#endif
	*integrand = integrand0;
#ifdef LUOYC20220706
	printf("RLV_example1 row = %d, col = %d, n_elem = %d\n", dState->n_rows, dState->n_cols, dState->n_elem);
#endif
	*path = path0;
#ifdef LUOYC20220706
	printf("RLV_example2 row = %d, col = %d, n_elem = %d\n", dState->n_rows, dState->n_cols, dState->n_elem);
#endif
	int aaaa = 1;

	return 1;
}

int Dynamics123(cx_mat* StateVar, mat* ControlVar, auxdata1 auxdata, mat* dState, mat* integrand, mat* path) {
	// 待修改。
	// 2021.08.13

	// 动力学模型
	cx_mat State = *StateVar;
	mat Control = *ControlVar;
	// 常数
	double g = auxdata.g;
	double Re = auxdata.Re;
	// 状态量本地化
	vec v = abs(State.col(0));
	vec theta = abs(State.col(1));
	vec psai = abs(State.col(2));
	vec r = abs(State.col(3));
	vec lamda = abs(State.col(4));
	vec phi = abs(State.col(5));
	// 控制量本地化
	vec alpha = Control.col(0);
	vec sigma = Control.col(1);
	// 参量计算
	vec H = r*Re - Re;
	int n = size(H)(0);
	vec Rou = zeros(n, 1);
	vec Ma = zeros(n, 1);
	vec L = zeros(n, 1);
	vec D = zeros(n, 1);
	Get_Rou_Mach(H, v*sqrt(Re*g), &Rou, &Ma);
	alpha2LD(alpha, Rou, Ma, v*sqrt(Re*g), auxdata, &L, &D);
	L = L / g;    // 这里是归一化
	D = D / g;
	// 微分方程
	vec dv = -D - sin(theta);
	vec dtheta = L%cos(sigma) / v - cos(theta) / v + v%cos(theta) / r;
	vec dpsai = L%sin(sigma) / v / cos(theta) + v%tan(phi) % cos(theta) % sin(psai) / r;
	vec dr = v%sin(theta);
	vec dlamda = v%cos(theta) % sin(psai) / r / cos(phi);
	vec dphi = v%cos(theta) % cos(psai) / r;

	double C1_v = auxdata.C1_v;								// 与热流密度相关参数
	double Rd_v = auxdata.Rd_v;								// 与热流密度相关参数
	double n_v = auxdata.n_v;								// 与热流密度相关参数
	double m_v = auxdata.m_v;								// 与热流密度相关参数

	vec path1 = sqrt(L%L + D%D);
	vec path2 = 0.5*Rou%v%v*Re*g;
	vec path3 = C1_v / sqrt(Rd_v)*pow(Rou, n_v) % pow((v*sqrt(Re*g)), m_v);

	mat dState0(n, 6);
	// dState0.col(0) = cx_vec(dv,zeros(size(dState0.col(0))))	// 复数形式
	dState0.col(0) = dv;
	dState0.col(1) = dtheta;
	dState0.col(2) = dpsai;
	dState0.col(3) = dr;
	dState0.col(4) = dlamda;
	dState0.col(5) = dphi;
	mat path0(n, 3);
	path0.col(0) = path1;
	path0.col(1) = path2;
	path0.col(2) = path3;
	mat integrand0(n, 1);
	integrand0.col(0) = abs(dtheta);
	*dState = dState0;
	*integrand = integrand0;
	*path = path0;
	int aaaa = 1;

	return 1;
}

int Dynamics123(mat* StateVar, cx_mat* ControlVar, auxdata1 auxdata, mat* dState, mat* integrand, mat* path) {
	// 待修改。
	// 2021.08.13

	// 动力学模型
	mat State = *StateVar;
	cx_mat Control = *ControlVar;
	// 常数
	double g = auxdata.g;
	double Re = auxdata.Re;
	// 状态量本地化
	vec v = State.col(0);
	vec theta = State.col(1);
	vec psai = State.col(2);
	vec r = State.col(3);
	vec lamda = State.col(4);
	vec phi = State.col(5);
	// 控制量本地化
	vec alpha = abs(Control.col(0));
	vec sigma = abs(Control.col(1));
	// 参量计算
	vec H = r*Re - Re;
	int n = size(H)(0);
	vec Rou = zeros(n, 1);
	vec Ma = zeros(n, 1);
	vec L = zeros(n, 1);
	vec D = zeros(n, 1);
	Get_Rou_Mach(H, v*sqrt(Re*g), &Rou, &Ma);
	alpha2LD(alpha, Rou, Ma, v*sqrt(Re*g), auxdata, &L, &D);
	L = L / g;    // 这里是归一化
	D = D / g;
	// 微分方程
	vec dv = -D - sin(theta);
	vec dtheta = L%cos(sigma) / v - cos(theta) / v + v%cos(theta) / r;
	vec dpsai = L%sin(sigma) / v / cos(theta) + v%tan(phi) % cos(theta) % sin(psai) / r;
	vec dr = v%sin(theta);
	vec dlamda = v%cos(theta) % sin(psai) / r / cos(phi);
	vec dphi = v%cos(theta) % cos(psai) / r;

	double C1_v = auxdata.C1_v;								// 与热流密度相关参数
	double Rd_v = auxdata.Rd_v;								// 与热流密度相关参数
	double n_v = auxdata.n_v;								// 与热流密度相关参数
	double m_v = auxdata.m_v;								// 与热流密度相关参数

	vec path1 = sqrt(L%L + D%D);
	vec path2 = 0.5*Rou%v%v*Re*g;
	vec path3 = C1_v / sqrt(Rd_v)*pow(Rou, n_v) % pow((v*sqrt(Re*g)), m_v);

	mat dState0(n, 6);
	dState0.col(0) = dv;
	dState0.col(1) = dtheta;
	dState0.col(2) = dpsai;
	dState0.col(3) = dr;
	dState0.col(4) = dlamda;
	dState0.col(5) = dphi;
	mat path0(n, 3);
	path0.col(0) = path1;
	path0.col(1) = path2;
	path0.col(2) = path3;
	mat integrand0(n, 1);
	integrand0.col(0) = abs(dtheta);
	*dState = dState0;
	*integrand = integrand0;
	*path = path0;
	int aaaa = 1;

	return 1;
}


///////////////////////////////////////////  性能指标部分  ////////////////////////////////
// 性能指标部分
// 输入：	input:  输入
// 输出：	output: 输出
//			event:  事件约束
// 方法：无
// 编写：李兆亭
// 时间：2020 / 09 / 17
// 其他：由用户自定义

int Objective123(input0* input, double* output, mat* event) {

	// Inputs
	// input.phase(phasenumber).initialstate -- row
	// input.phase(phasenumber).finalstate -- row
	// input.phase(phasenumber).initialtime -- scalar
	// input.phase(phasenumber).finaltime -- scalar
	// input.phase(phasenumber).integral -- row
	// input.parameter -- row

	// input.auxdata = auxiliary information

	// Output
	// output.objective -- scalar
	// output.eventgroup(eventnumber).event -- row
	
	double is1 = (*input).phase.initialstate(0);
	double fs1 = (*input).phase.finalstate(0);
	double is2 = (*input).phase.initialstate(1);
	double fs2 = (*input).phase.finalstate(1);
	(*output) = (*input).phase.finaltime;
	(*event).reset();
	return 1;
}




/*function[output, event] = Objective(input)
% 功能：计算终端点处的有关目标函数。
% 利用Matlab矩阵计算较快的特点

% Inputs
% input.phase(phasenumber).initialstate -- row
% input.phase(phasenumber).finalstate -- row
% input.phase(phasenumber).initialtime -- scalar
% input.phase(phasenumber).finaltime -- scalar
% input.phase(phasenumber).integral -- row
%
% input.parameter -- row
% input.auxdata = auxiliary information

% Output
% output.objective -- scalar
% output.eventgroup(eventnumber).event -- row

output = input.phase.finaltime;
is1 = input.phase.initialstate(1);
fs1 = input.phase.finalstate(1);
is2 = input.phase.initialstate(2);
fs2 = input.phase.finalstate(2);
event = [fs1 + fs2];
end*/