#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;
#ifndef Liulu20220630_PSLINUX / //修改的内容
int main_HCV() {
#else
int main_HCV() {
#endif
	// 计时器开始
	double dur;
	clock_t start, end;
	start = clock();
	////////////////////////////////////参数初始化//////////////////////////////////////////////
	input0 input;
	output0* output;
	auxdata1 auxdata;
	// 伪谱法条件初始化
	Intput_Initial(&input);
	//基本参数
	auxdata.Sref = 150;//飞行器截面积
	auxdata.M0 = 100e3;    //飞行器质量，单位：kg
	auxdata.g = 9.80665;
	auxdata.Re = 6371004;//假设地球为一个完美的球体
	auxdata.GMe = 3.986005e14;//地球质量
	auxdata.J2 = 1.08263e-3; //考虑J2项
	auxdata.omige = 7.292115e-5;//考虑地球自转
	auxdata.Ac = 10;
	auxdata.ae = 6378137;
	auxdata.re = 6371004;
	//归一化处理
	double pi = datum::pi;
	double g = auxdata.g;
	double Re = auxdata.Re;
	double tc = sqrt(Re / g);
	double rc = Re;
	double vc = sqrt(Re*g);
	double rad2deg = 180 / pi;
	input.setup.auxdata = auxdata;
	//禁飞区设置
	double  a[] = { 100,48,500e3 };
	double  b[] = { 90,43,300e3 };
	double  c[] = { 62,43,360e3 };
	//飞行器初始状态
	double state0[] = { 49,90,2100,70,1 };
	//飞行器中间状态
	double state1[] = { 0,0,2040,0,0.1,-3 };
	double state2[] = { 180,180,2380,180,1,21 };
	//飞行器末端位置
	double state[] = { 116,47 };
	//控制量范围约束
	double con1[] = { 0.1,-1,-pi / 2 };
	double con2[] = { 1,1,pi / 2 };
	// 范围赋值
	////////////////////////////////////////////////
	int length_state = 5;
	int length_control = 3;

	double initialtime_lower = 0;
	double initialtime_upper = 0;

	double finaltime_lower = 0 / tc;
	double finaltime_upper = 1500 / tc;

	vec state_c; state_c << rad2deg << rad2deg << vc << rad2deg << 1 << endr;
	vec initialstate_lower; initialstate_lower << state0[0] << state0[1] << state0[2] << state0[3] << state0[4] << endr;
	vec initialstate_upper; initialstate_upper << state0[0] << state0[1] << state0[2] << state0[3] << state0[4] << endr;

	vec state_lower; state_lower << state1[1] << state1[0] << state1[2] << state1[3] << state1[4] << endr;
	vec state_upper; state_upper << state2[1] << state2[0] << state2[2] << state2[3] << state2[4] << endr;

	vec finalstate_lower; finalstate_lower << state[1] << state[0] << state1[2] << state1[3] << state1[4] << endr;
	vec finalstate_upper; finalstate_upper << state[1] << state[0] << state2[2] << state2[3] << state2[4] << endr;

	vec control_lower; control_lower << con1[0] << -3 / rad2deg << con1[2] << endr;
	vec control_upper; control_upper << con2[0] << 21 / rad2deg << con2[2] << endr;


	vec path_lower; path_lower << -0.001 << 0 << 480e3 / Re << 350e3 / Re << 360e3 / Re << endr;
	vec path_upper; path_upper << 0.001 << 0 << 2 << 2 << 2 << endr;

	vec event_lower; event_lower.reset();
	vec event_upper; event_upper.reset();


	input.setup.state_length = length_state;
	input.setup.control_length = length_control;

	input.setup.bounds.phase.initialtime.lower = initialtime_lower;
	input.setup.bounds.phase.initialtime.upper = initialtime_upper;

	input.setup.bounds.phase.finaltime.lower = finaltime_lower;
	input.setup.bounds.phase.finaltime.upper = finaltime_upper;

	input.setup.bounds.phase.initialstate.lower = initialstate_lower / state_c;
	input.setup.bounds.phase.initialstate.upper = initialstate_upper / state_c;

	input.setup.bounds.phase.state.lower = state_lower / state_c;
	input.setup.bounds.phase.state.upper = state_upper / state_c;

	input.setup.bounds.phase.finalstate.lower = finalstate_lower / state_c;
	input.setup.bounds.phase.finalstate.upper = finalstate_upper / state_c;

	input.setup.bounds.phase.control.lower = control_lower;
	input.setup.bounds.phase.control.upper = control_upper;

	input.setup.bounds.phase.path.lower = path_lower;
	input.setup.bounds.phase.path.upper = path_upper;

	input.setup.bounds.phase.event.lower = event_lower;
	input.setup.bounds.phase.event.upper = event_upper;

	// 猜测值
	guess0 guess;
	vec guess_time; guess_time << 0 << endr
		<< 300 / tc << endr
		<< 1400 / tc << endr;
	mat guess_state; guess_state << state0[0] / rad2deg << state0[1] / rad2deg << state0[2] / vc << state0[3] / rad2deg << state0[4] << endr
		<< 54 / rad2deg << 100 / rad2deg << 2100 / vc << 50 / rad2deg << 0.6 << endr
		<< state[1] / rad2deg << state[0] / rad2deg << 2100 / vc << 130 / rad2deg << 0.3 << endr;
	mat guess_control; guess_control << 0.7 << 15 / rad2deg << -40 / rad2deg << endr
		<< 0.6 << 13 / rad2deg << 40 / rad2deg << endr
		<< 0.5 << 9 / rad2deg << 0 / rad2deg << endr;
	mat guess_path(3, 5, fill::zeros);
	guess_path.row(0) = input.setup.bounds.phase.path.lower.t();
	guess_path.row(1) = input.setup.bounds.phase.path.upper.t();

	guess.phase.time = guess_time;
	guess.phase.state = guess_state;
	guess.phase.control = guess_control;
	guess.phase.path = guess_path;
	input.setup.guess = guess;

	// 配点设置
	int inter_num = 8;
	input.setup.name = "Cruise_opt";							// 问题
	mesh1 mesh;
	mesh.method = "p-h";								// 网格细化方法的名字，字符串表示
	mesh.tolerance = 1e-4;								// 网格细化精度的允许值
	mesh.maxiterration = 20;								// 网格细化最大迭代次数
	mesh.colpointsmin = 8;								// 网格细化产生新interval下的最小配点数目
	mesh.colpointsmax = 20;								// 网格细化产生新interval下的最大配点数目
	mesh.splitmult = 1.2;								// 比例因子，确定创建新网格的间隔数目
	mesh.curveratio = 2;								// 网格间隔中解的最大平均曲率比的阈值，以确定是否增加新间隔
	mesh.phase.inter_num = inter_num;					// 段p内的间隔数目, 整数
	mesh.phase.colpoints = 8 * ones(inter_num, 1);		// 段p内的配置点数目，整数数组，长度与间隔数目相等
	mesh.phase.i_proportion = 2.0 / inter_num * ones(inter_num, 1); // 初始网格划分的比例（当前为均等划分，和为2）
	input.setup.mesh = mesh;							// 组合setup.mesh结构体
	input.setup.warmstart = 0;
	input.setup.Scal = 0;
	input.setup.interp.method = "linear";

	// 函数设置 
	functions1 functions;
	functions.continous1 = Dynamics_HCV;
	functions.endpoint = Objective_HCV;
	// 求解器设置
	NLP1 NLP;
	NLP.ipoptoptions.print_level = 2;
	NLP.ipoptoptions.hessian_approximation = "limited-memory";
	NLP.ipoptoptions.mu_strategy = "adaptive";
	#ifndef LIULU20220630_PSLINUX / //修改求解器
	NLP.ipoptoptions.linear_solver = "ma27";
	#else
	NLP.ipoptoptions.linear_solver = "mumps";
	#endif
	NLP.ipoptoptions.tolerance = 1e-6;
	NLP.ipoptoptions.maxiterration = 2000;
	input.setup.NLP = NLP;
	input.setup.derivatives.supplier = "FD";
	// 伪谱法过程实现
	int iteration_final;
	output = PM_Process(&input, functions, &iteration_final);

	// 计时结束
	end = clock();
	dur = (double)(end - start);
	#ifndef LIULU20220629_PSLINUX  //修改printf_s为printf
	printf("总耗时: %f \n", (dur / CLOCKS_PER_SEC));
	#else
	printf_s("总耗时: %f \n", (dur / CLOCKS_PER_SEC));
	#endif
	vec t_ans = output[iteration_final].result.time;
	mat Y_ans = output[iteration_final].result.state;
	mat U_ans = output[iteration_final].result.control;
	#ifndef LIULU20220629_PSLINUX  //将新生成的文件路径改为当前想要保存位置的路径
	t_ans.save("D:\\...\\t_ans1.txt", raw_ascii);
        Y_ans.save("D:\\...\\Y_ans1.txt", raw_ascii);
        U_ans.save("D:\\...\\U_ans1.txt", raw_ascii);
	#else
	t_ans.save("/public/home/ulijing/GPM_CPP/example/HCV_example/t_ans1.txt", raw_ascii);
 	Y_ans.save("/public/home/ulijing/GPM_CPP/example/HCV_example/Y_ans1.txt", raw_ascii);
 	U_ans.save("/public/home/ulijing/GPM_CPP/example/HCV_example/U_ans1.txt", raw_ascii);
	#endif
	getchar();
	return 1;
}