#include <iostream>
#include <armadillo>
#include <time.h>
#include <stdio.h>
#include <cstdio>
#include <stdlib.h>
#include "GPM.h"

// 用户新增头文件
#include "RLV.h"

using namespace std;
using namespace arma;

int main(){
	// 计时器开始
	double dur;
	clock_t start, end;
	start = clock();

	////////////////////////////////////参数初始化//////////////////////////////////////////////
	input0 input;
	output0* output;
	auxdata1 auxdata;
	// 伪谱法条件初始化
	Intput_Initial(&input);

	// 人为编辑定义部分
	auxdata.Sref = 0.4839;
	auxdata.M0 = 907;
	auxdata.C1_v = 2e-8;		// 与热流密度相关参数
	auxdata.Rd_v = 0.006;		// 与热流密度相关参数
	auxdata.n_v = 0.5;			// 与热流密度相关参数
	auxdata.m_v = 3.15;
	auxdata.g = 9.80665;
	auxdata.Re = 6371110;
	input.setup.auxdata = auxdata;

	// 归一化单位向量
	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);
	vec state_c; state_c << vc << 1.0 << 1.0 << rc << 1.0 << 1.0 << endr;

	// 范围赋值
	int length_state = 6;
	int length_control = 2;

	double initialtime_lower = 0;
	double initialtime_upper = 0;

	double finaltime_lower = 0;
	double finaltime_upper = 500.0 / tc;
	
	vec initialstate_lower; initialstate_lower << 6000.0 << 0.0 / 180.0 * pi << 50.0 / 180.0 * pi << 60.0 * 1e3 + Re << 0.0 / 180.0 * pi << 0.0 / 180.0 * pi << endr;
	vec initialstate_upper; initialstate_upper << 6000.0 << 0.0 / 180.0 * pi << 50.0 / 180.0 * pi << 60.0 * 1e3 + Re << 0.0 / 180.0 * pi << 0.0 / 180.0 * pi << endr;

	vec state_lower; state_lower << 5400.0 << -15.0 / 180.0 * pi << 35.0 / 180.0 * pi << 40.0 * 1e3 + Re <<  0.0 / 180.0 * pi <<  0.0 / 180.0 * pi << endr;
	vec state_upper; state_upper << 6000.0 << +15.0 / 180.0 * pi << 55.0 / 180.0 * pi << 60.0 * 1e3 + Re << 10.0 / 180.0 * pi << 12.0 / 180.0 * pi << endr;

	vec finalstate_lower; finalstate_lower << 5400.0 << -0.001 / 180.0 * pi <<  35.0 / 180.0 * pi << 54.0 * 1e3 + Re << 8.0 / 180.0 * pi << 9.0 / 180.0 * pi << endr;
	vec finalstate_upper; finalstate_upper << 5600.0 << +0.001 / 180.0 * pi <<  51.0 / 180.0 * pi << 56.0 * 1e3 + Re << 10.0 / 180.0 * pi << 11.0 / 180.0 * pi << endr;

	vec control_lower; control_lower << 0.0 / 180.0 * pi << -90.0 / 180.0 * pi << endr;
	vec control_upper; control_upper << 30.0 / 180.0 * pi << 90.0 / 180.0 * pi << endr;

	vec path_lower; path_lower << 0.0 << 0.0 << 0.0 << endr;
	vec path_upper; path_upper << 3.0 << 100.0 * 1e3 << 1500.0 * 1e3 << endr;

	rowvec integral_lower; integral_lower << -10.0 << endr;
	rowvec integral_upper; integral_upper << 10.0 << endr;

	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.integral.lower = integral_lower;
	input.setup.bounds.phase.integral.upper = integral_upper;
	input.setup.bounds.phase.event.lower.reset();
	input.setup.bounds.phase.event.upper.reset();

	// 猜测值
	guess0 guess;
	int length_path = 3;
	vec guess_time; guess_time << 0.0 << 300.0 / tc << endr;
	mat guess_state(2, length_state, fill::zeros);
	mat guess_control(2, length_control, fill::zeros);
	mat guess_path(2, length_path, fill::zeros);
	rowvec guess_integral;
	guess_state.row(0) = input.setup.bounds.phase.initialstate.lower.t();
	guess_state.row(1) = input.setup.bounds.phase.finalstate.lower.t();
	guess_control.row(0) = input.setup.bounds.phase.control.lower.t();
	guess_control.row(1) = input.setup.bounds.phase.control.upper.t();
	guess_integral << 0 << endr;
	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.integral = guess_integral;
	guess.phase.path = guess_path;
	input.setup.guess = guess;

	// 配点设置
	int inter_num = 2;
	input.setup.name = "RLV";							// 问题
	mesh1 mesh;
	mesh.method = "p-h";								// 网格细化方法的名字，字符串表示
	mesh.tolerance = 1e-5;								// 网格细化精度的允许值
	mesh.maxiterration = 3;								// 网格细化最大迭代次数
	mesh.colpointsmin = 2;								// 网格细化产生新interval下的最小配点数目
	mesh.colpointsmax = 20;								// 网格细化产生新interval下的最大配点数目
	mesh.splitmult = 1.2;								// 比例因子，确定创建新网格的间隔数目
	mesh.curveratio = 2;								// 网格间隔中解的最大平均曲率比的阈值，以确定是否增加新间隔
	mesh.phase.inter_num = inter_num;					// 段p内的间隔数目, 整数
	mesh.phase.colpoints = 2 * 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.Scal = 0;
	input.setup.warmstart = 0;
	input.setup.interp.method = "linear";

	// 函数设置 
	functions1 functions;
	functions.continous1 = Dynamics123;
	functions.endpoint = Objective123;

	// 求解器设置
	NLP1 NLP;
	NLP.ipoptoptions.print_level = 1;
	NLP.ipoptoptions.hessian_approximation = "limited-memory";
	NLP.ipoptoptions.mu_strategy = "adaptive";
	NLP.ipoptoptions.linear_solver = "ma27"; //mumps、pardiso
	NLP.ipoptoptions.tolerance = 1e-2;
	NLP.ipoptoptions.maxiterration = 3000;
	input.setup.NLP = NLP;
	input.setup.derivatives.supplier = "FD";

	//input.setup.feature.FSE = 1;	///////

	// 伪谱法过程实现
	int iteration_final;
	output = PM_Process(&input, functions, &iteration_final);

	// 计时结束
	end = clock();
	dur = (double)(end - start);
#ifndef LUOYC20220707
	printf("Use Time: %f \n", (dur / CLOCKS_PER_SEC));
#else
	printf_s("Use Time: %f \n", (dur / CLOCKS_PER_SEC));

#endif

	getchar();
	return 1;
}