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

using namespace std;
using namespace arma;

int Intput_Initial(input0* input) {

	// 归一化向量
	double g = 9.80665;
	double Re = 6371110;
	double tc = sqrt(Re / g);
	double rc = Re;
	double vc = sqrt(Re*g);

	// 常值向量
	double pi = 3.141592653;
	double GMe = 1.0;
	double Sref = 0.4839;
	double M0 = 907;
	double C1_v = 2e-8;
	double Rd_v = 0.006;
	double n_v = 0.5;
	double m_v = 3.15;

	// 数据初始化
	int length_state = 6;
	int length_control = 2;
	int length_parameter = 0; 
	int length_path = 3;

	// 猜测值
	vec guess_time; guess_time << 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;

	// 算法参数设置
	char name[] = "RLV";
	char mesh_method[] = "normal";					// 网格细化方法的名字，字符串表示
	double mesh_tolerance = 1e-5;					// 网格细化精度的允许值
	int mesh_maxiterration = 3;						// 网格细化最大迭代次数
	int mesh_colpointsmin = 2;						// 网格细化产生新interval下的最小配点数目
	int mesh_colpointsmax = 19;						// 网格细化产生新interval下的最大配点数目
	double mesh_splitmult = 1.2;					// 比例因子，确定创建新网格的间隔数目
	double mesh_curveratio = 2;						// 网格间隔中解的最大平均曲率比的阈值，以确定是否增加新间隔
	int mesh_inter_num = 4;							// 段p内的间隔数目, 整数
	vec mesh_colpoints; mesh_colpoints << 2 << 2 <<2 << 2 << endr;   				// 段p内的配置点数目，整数数组，长度与间隔数目相等
	vec mesh_i_proportion = 2.0 / 4.0 * ones(4, 1);		// 初始网格划分的比例（当前为均等划分，和为2）
	double Scal = 0;
	char interp_method[] = "linear";

	int print_level = 0;
	char hessian_approximation[] = "limited-memory";
	char mu_strategy[] = "adaptive";
	char linear_solver[] = "ma57";
	double tol = 1e-7;
	int max_iter = 3000;


//////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// 1. input.setup结构体所有参数
	// 1.1 name
	setup0 setup = (*input).setup;
	setup.name = "Trajectory Optimization";

	// 1.2 functions
	//setup.functions.continous = "temp";
	//setup.functions.endpoint = "temp";
	// 状态变量长度
	setup.state_length = length_state;
	// 控制变量长度
	setup.control_length = length_control;
	// 待优化参数长度
	setup.parameter_length = length_parameter;
	
	// 1.3 bounds
	bounds1 bounds = setup.bounds;
	bounds.phase.initialtime.lower = 0;			// 初始时间下限
	bounds.phase.initialtime.upper = 0;			// 初始时间上限
	bounds.phase.finaltime.lower = 0;			// 默认终端时间下限
	bounds.phase.finaltime.upper = 0;			// 默认终端时间上限
	bounds.phase.state.lower.reset();			// 默认状态下限
	bounds.phase.state.upper.reset();			// 默认状态上限
	bounds.phase.initialstate.lower.reset();	// 默认初始状态下限
	bounds.phase.initialstate.upper.reset();	// 默认初始状态上限
	bounds.phase.finalstate.lower.reset();		// 默认终端状态下限
	bounds.phase.finalstate.upper.reset();		// 默认终端状态上限
	bounds.phase.control.lower.reset();         // 默认控制下限
	bounds.phase.control.upper.reset();			// 默认控制上限
	bounds.phase.path.lower.reset();			// 默认路径约束下限
	bounds.phase.path.upper.reset();			// 默认路径约束上限
	bounds.phase.integral.lower.reset();		// 默认积分量下限
	bounds.phase.integral.upper.reset();		// 默认积分量上限
	bounds.phase.eventgroup.lower.reset();		// 默认段间链接约束
	bounds.phase.eventgroup.upper.reset();
	bounds.phase.event.lower.reset();	
	bounds.phase.event.upper.reset();	
	bounds.phase.parameter.lower.reset();       // 待优化参数下限
	bounds.phase.parameter.upper.reset();       // 待优化参数上限
	setup.bounds = bounds;

	// 1.4 auxdata
	setup.auxdata.GMe = GMe;
	setup.auxdata.Sref = Sref;
	setup.auxdata.M0 = M0;
	setup.auxdata.C1_v = C1_v;					// 与热流密度相关参数
	setup.auxdata.Rd_v = Rd_v;					// 与热流密度相关参数
	setup.auxdata.n_v = n_v;					// 与热流密度相关参数
	setup.auxdata.m_v = m_v;
	
	setup.auxdata.parameter.reset();			// 待优化参数

	// 1.5 derivatives
	setup.derivatives.supplier = "BD";				// 此项可用的选项包括：“CD”,“FD”,“BD”,“CSD”
	setup.derivatives.derivativelevel = 1;			// 目前只使用一阶导数
	setup.derivatives.dependencies = "full";		//
	setup.derivatives.number = 2;					// v阶导数有全变分，(v-1)阶导数可微

	// 1.6 scales
	setup.scales = Scal;

	// 1.7 method
	setup.method = "RPM";

	// 1.8 mesh
	setup.mesh.method = mesh_method;							// 网格细化方法的名字，字符串表示
	setup.mesh.tolerance = mesh_tolerance;						// 网格细化精度的允许值
	setup.mesh.maxiterration = mesh_maxiterration;				// 网格细化最大迭代次数
	setup.mesh.colpointsmin = mesh_colpointsmin;				// 网格细化产生新interval下的最小插值点数目
	setup.mesh.colpointsmax = mesh_colpointsmax;				// 网格细化产生新interval下的最大插值点数目
	setup.mesh.splitmult = mesh_splitmult;						// 比例因子，确定创建新网格的间隔数目
	setup.mesh.curveratio = mesh_curveratio;					// 网格间隔中解的最大平均曲率比的阈值，以确定是否增加新间隔
	setup.mesh.phase.inter_num = mesh_inter_num;				// 段p内的间隔数目, 整数
	setup.mesh.phase.colpoints = mesh_colpoints;				// 段p内的配置点数目，整数数组，长度与间隔数目相等
	setup.mesh.phase.i_proportion = mesh_i_proportion;			// 初始网格划分的比例
	setup.mesh.refinement.Conservative_coefficient = 0.5;		// p-h-v网格细化方法的保守系数

	// 1.9 guess
	// 为充分采用用户经验，这里输入即为用户经验值。
	setup.guess.phase.time = guess_time;
	setup.guess.phase.state = guess_state;
	setup.guess.phase.control = guess_control;
	setup.guess.phase.integral = guess_integral;
	setup.guess.phase.interval_points << 1 << endr;

	// 1.10 NLP
	setup.NLP.solver = "ipopt";
	setup.NLP.ipoptoptions.print_level = print_level;
	setup.NLP.ipoptoptions.hessian_approximation = hessian_approximation;
	setup.NLP.ipoptoptions.mu_strategy = mu_strategy;
	setup.NLP.ipoptoptions.linear_solver = linear_solver;
	setup.NLP.ipoptoptions.tolerance = tol;
	setup.NLP.ipoptoptions.maxiterration = max_iter;

	// 1.11 插值方式
	setup.interp.method = "linear";		

	// 1.12 feature
	setup.feature.FSE = 0;

	// 1.13 几种测试模式
	setup.testmode = 0;		// 正常模式；
	// setup.testmode = 1;		// 测试单个间隔、阶数不断递增时的收敛情况；

	// 组合为input结构体
	setup.warmstart = 0;
	(*input).setup = setup;

///////////////////////////////////////////////////////////////////
	return 1;
}