// 函数功能：伪谱法具体实现过程
// 输入：input：结构体
// 输出：output：结构体
// 方法：参考伪谱法的官方文献
// 编写：李兆亭，2019 / 12 / 11
// C++版本：李兆亭，2020/08/31

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

using namespace std;
using namespace arma;

output0* PM_Process(input0* input, functions1 functions, int *iteration_final) {

	uvec unit0; unit0 << 1 << endr;
	int IsStop = 0;							// 伪谱法停止标识
	int iteration = 1;						// 代数标识
	output0* output = new output0[100];		// 输出初始化
	int Sign_start = 1;						// 检查正确标识符

	//  零、用户给定参数检查
	//Intput_add(input);
	Dismission_check(input, &Sign_start);

	int testmode = (*input).setup.testmode;

	// Dismission_check(input, Sign_start);
	if (Sign_start == 0) {
		return output;
	}

	//  一、参数本地化
	setup0 setup = (*input).setup;
	mesh1 mesh = setup.mesh;            		// 初始网格信息
	auxdata1 auxdata = setup.auxdata;			// 公用参数信息
	guess0 guess = setup.guess;					// 初始猜测值
	char* method = setup.method;				// 伪谱法的方法
	NLP1 NLP = setup.NLP;             			// NLP求解器设置
	///////////////////////////////////////////////////////////////////////////
	int(*Dynamics_temp1)(mat*, mat*, auxdata1, mat*, mat*, mat*) = functions.continous1;
	int(*Dynamics_temp2)(cx_mat*, mat*, auxdata1, mat*, mat*, mat*) = functions.continous2;
	int(*Dynamics_temp3)(mat*, cx_mat*, auxdata1, mat*, mat*, mat*) = functions.continous3;
	int(*Objective_temp)(input0*, double*, mat*) = functions.endpoint;
	/////////////////////////////////////////////////////////////////////////////
	char* interp_method = setup.interp.method;
	char* deri_supplier = setup.derivatives.supplier;			// 导数计算方式
	int is_FSE = setup.feature.FSE;								// 是否做终端状态估计
	int niu = setup.derivatives.number;							// 至多v阶导数有全变分，至多（v - 1）阶导数可微
	double CC = setup.mesh.refinement.Conservative_coefficient; // 网格细化的保守系数

	int colpointsmax = mesh.colpointsmax;		// 网格细化产生新interval下的最大配置点数目
	int interval_num = mesh.phase.inter_num;	// 段p内的间隔数目, 整数， % 总间隔数目
	vec Npoints = mesh.phase.colpoints;			// 段p内的配置点数目，整数数组，长度与间隔数目相等

	// 输入时间归一化
	double t0 = min(guess.phase.time);                              // 时间初值
	double tf = max(guess.phase.time);                             	// 时间最大值
	vec T_guess1 = Normal(guess.phase.time, tf, t0);						// 时间归一化
	vec interval_points_ratio = mesh.phase.i_proportion;			// 初始网格由人工设置，为和为2、长度为间隔数的正向量
	
	// 问题中的常值量
	int length_state = (size(guess.phase.state))(1);
	int length_control = (size(guess.phase.control))(1);
	int length_path;
	if (setup.bounds.phase.path.lower.is_empty()){
		length_path = 0;
	}
	else {
		length_path = setup.bounds.phase.path.lower.n_rows;
	}
	int length_integral;
	if (setup.bounds.phase.integral.lower.is_empty()) {
		length_integral = 0;
	}
	else {
		length_integral = setup.bounds.phase.integral.lower.n_cols;
	}
	int length_event = (size(setup.bounds.phase.event.lower))(0);
	int length_parameter = (size(setup.auxdata.parameter))(0);

	// 状态、控制参数归一化
	mat U_guess1 = guess.phase.control;
	mat state_guess1 = guess.phase.state;

	// 待优化参数初始化
	vec parameter = auxdata.parameter;

	// 打印等级
	int print_level = NLP.ipoptoptions.print_level;

	//  二、伪谱法流程
	// 1 根据初始猜测值生成初始网格
	mat Col_Points, MeshGrid, Ts1_Te1;
	Col_Points = Points_Choose(method);             // 配置点类型选择，由方法确定
	Mesh_initial(colpointsmax, interval_num, Npoints, Col_Points, interval_points_ratio, &MeshGrid, &Ts1_Te1);// 根据网格信息，生成具体网格

	// 差分步长
	double eps0 = 1e-8;
	/*MeshGrid = MeshGrid_nextstep;
	Ts1_Te1 = Ts1_Te1_nextstep;
	tf = tf_ans;
	t0 = t0_ans;
	Y_ans1 = Y_ans;
	U_ans1 = U_ans;*/
	vec T_old1, time_ans1;
	uvec N_Col, N_interp;
	mat Y_old1, U_old1, Y_ans1, U_ans1, D_phase, deta_t_interval, deta_t_in_col, temp;
	sp_mat Jacb_struct0;
	Jacb_struct_info0 Jacb_struct_info;
	Grad_struct_info0 Grad_struct_info;
	sp_mat I_Z_struct0;
	temp.reset();
	int length_Z0, length_F0;
	while (IsStop == 0) {
		// 迭代初值
		if (iteration == 1) {			// 第0次，猜测值作为初始值
			T_old1 = T_guess1;
			Y_old1 = state_guess1;
			U_old1 = U_guess1;
		}
		else {							// 第i次，上一步的新值作为初值
			T_old1 = time_ans1;
			Y_old1 = Y_ans1;
			U_old1 = U_ans1;
		}
		//Y_old1.print("Y_old1");
		//U_old1.print("U_old1");
		printf("当前为第 %ld 代;\n", iteration);

		// 2 计算各量微分近似及相关矩阵
		interval_num = (size(MeshGrid))(0);
		uvec N = ones<uvec>(interval_num);
		N_initial(MeshGrid.col(0), &N, interval_num);						// 每个间隔内的配点阶数
		Method2Nk(method, N, &N_Col);										// 配置点数目
		N_Col2interp(method, N_Col, &N_interp);                         	// 配置点数目到插值点数目的转换
		D_pro(&MeshGrid, interval_num, &N_interp, &N_Col, &D_phase);     	// 微分矩阵
		field<mat> D_phasei(interval_num, 1);
		mat U_phase1(sum(N_interp), length_control);
		mat Y_phase1(sum(N_interp), length_state);
		D_M2Cell(&D_phase, interval_num, &N_interp, &D_phasei);
		UY_pro(MeshGrid, U_old1, Y_old1, T_old1, Ts1_Te1, N_interp, interval_num, interp_method, &U_phase1, &Y_phase1);
		deta_t_interval = Ts1_Te1.col(1) - Ts1_Te1.col(0);
		deta_t_in_col.reset();
		for (int i = 0; i < interval_num; i++) {
			temp = repelem(deta_t_interval.row(i), N_Col(i), 1);
			deta_t_in_col = join_cols(deta_t_in_col, temp);
		}
		// 计算每个间隔最后一个插值点的序号, 并在积分矩阵上剔除
		uvec last_interp = ones<uvec>(interval_num);
		uvec last_col = ones<uvec>(interval_num);
		int temp1;
		for (int i = 1; i <= interval_num; i++) {
			temp1 = sum(N_interp.rows(0, i - 1));
			last_interp(i - 1, 0) = temp1;
			last_col(i - 1, 0) = temp1 - i;
		}

		//计算n >= 2个间隔的第1个点的序号
		uvec first_col = ones<uvec>((interval_num - 1));
		for (int i = 1; i <= (interval_num - 1); i++) {
			first_col(i - 1) = sum(N_Col.rows(0, i - 1)) + 1;
		}

		// 积分权重矩阵
		mat W_phase;
		W_pro(method, &N, &N_Col, interval_num, &W_phase);

		// 3、生成NLP变量、约束和目标

		// NLP变量
		int Col_total = sum(N_Col);
		vec Z0;
		Z_pro(Y_phase1, U_phase1, parameter, last_interp, Col_total, length_state, length_control, tf, t0, &Z0);
		length_Z0 = Z0.n_rows;		

		// NLP约束
		length_F0 = Col_total*length_state + Col_total*length_path + length_integral + length_event;        // NLP约束数目

		// NLP梯度结构计算
		if (iteration == 1) {
			Grad_struct_solve(Z0, deta_t_in_col, Col_total, auxdata, W_phase, length_state, length_control, 
							length_integral, &Grad_struct_info, length_parameter, Dynamics_temp1, Objective_temp);
			if (length_integral != 0) {
				IZstruct(Z0, Col_total, length_state, length_control, length_integral, W_phase, auxdata, &I_Z_struct0, Dynamics_temp1);
			}
			else {
				I_Z_struct0.reset();
			}
		}

		// 生成变量的范围边界
		vec Fmin, Fmax, Zmin, Zmax;
		Fmin_pro(*input, Col_total, length_state, length_path, length_integral, length_event, &Fmin);
		Fmax_pro(*input, Col_total, length_state, length_path, length_integral, length_event, &Fmax);
		Zmin_pro(*input, Col_total, &Zmin);
		Zmax_pro(*input, Col_total, &Zmax);

		// 4 计算NLP的雅可比矩阵
		// 雅克比结构（稀疏形式表达）
		if (iteration == 1) {
			Jacb_struct_pro0(Z0, D_phase, first_col, deta_t_in_col, length_event, length_path, length_parameter,Col_total, auxdata, W_phase, 
							length_state, length_control, length_integral, length_F0,interval_num, last_interp, Dynamics_temp1, Objective_temp, &Jacb_struct0);
			Jacb_solve(Jacb_struct0, Col_total, length_event, length_path, length_state, length_control, length_integral, length_parameter, &Jacb_struct_info);
		}

		// 5 利用IPOPT求解NLP问题
		int var_num = length_Z0;						// 变量数目
		int constraint_num = length_F0;					// 约束数目
		int nnz_jac = Jacb_struct0.n_nonzero;			// 雅可比矩阵非零数目
		int nnz_h = 2;									// 海森矩阵非零数目
		vec z_ans, zl_temp, zu_temp, lambda_temp;
		int iter_num;
		double cpu_time;	

		ipopt_apply(var_num, constraint_num, nnz_jac, nnz_h, Z0, Fmin, Fmax, Zmin, Zmax, auxdata, W_phase, deta_t_in_col, Col_total,
			length_state, length_control, Grad_struct_info, length_integral, eps0, D_phase, first_col, length_path, length_event,
			Jacb_struct0, D_phasei, N_Col, I_Z_struct0, interval_num, length_Z0, length_parameter, Jacb_struct_info, deri_supplier, length_F0, NLP,
			Dynamics_temp1, Objective_temp, 
			&z_ans, &zl_temp, &zu_temp, &lambda_temp, &iter_num, &cpu_time);
		
		cout << "" << endl;
		cout << "第" << iteration << "次求解过程，求解器迭代次数为：" << iter_num << "。" << endl;
		cout << "同时所需CPU时间为：" << cpu_time << "s。" << endl;
		cout << "" << endl;

		// cout << parameter << endl;

		// 优化结果的性能指标
		double Objective_ans;
		Objective_ipopt(z_ans, deta_t_in_col, Col_total, auxdata, W_phase, length_state, length_control, length_parameter, Dynamics_temp1, Objective_temp, &Objective_ans);

		// 当前结果约束评估？？

		// 结果输出到output
		mat Y_phase_ans1, U_phase_ans1;
		int Col_total_state = length_state*(Col_total + 1);
		int Col_total_control = length_control*Col_total;
		if (length_parameter != 0) {
			parameter = z_ans.rows(Col_total_state + Col_total_control + 3 - 1, Col_total_state + Col_total_control + 2 + length_parameter - 1);
		}
		else {
			parameter.reset();
		}

		double tf_ans = z_ans(Col_total_state + Col_total_control + 2 - 1);
		double t0_ans = z_ans(Col_total_state + Col_total_control + 1 - 1);
		auxdata.parameter = parameter;
		(*input).setup.auxdata = auxdata;
		Z2YU_interval(z_ans, Col_total, length_state, length_control, &Y_phase_ans1, &U_phase_ans1);	// 将NLP变量分间隔重组为元胞

		// 6 结果处理
		// 时间域上的收缩、扩张
		vec Time_interval_ans;
		Time2Newtime(Ts1_Te1, MeshGrid, tf, t0, t0_ans, tf_ans, interval_num, Col_total, last_col, &Time_interval_ans);		// 对应新时间下的间隔信息（未归一化）

		// 生成输出到output的实际时间、状态、控制
		mat Y_ans = Y_phase_ans1;
		vec time_ans = zeros(Col_total + 1, 1);
		mat U_ans = zeros(Col_total + 1, length_control);
		time_ans.rows(0, Col_total - 1) = Time_interval_ans;
		U_ans.rows(0, Col_total - 1) = U_phase_ans1;

		// 对边界点进行处理（LGR需要）
		U_ans.row(Col_total) = U_phase_ans1.row(Col_total - 1);
		time_ans.row(Col_total) = tf_ans;
		uvec first_col1 = join_cols(unit0, first_col) - 1;
		vec tf_ans0; tf_ans0 << tf_ans << endr;
		mat Ts_Te_ans = join_rows(Time_interval_ans.rows(first_col1), join_cols(Time_interval_ans.rows(first_col1.rows(1, interval_num - 1)), tf_ans0));
		mat Ts1_Te1_ans = Normal(Ts_Te_ans, tf_ans, t0_ans);	// 起终点时间矩阵归一化
		time_ans1 = Normal(time_ans, tf_ans, t0_ans);			// 时间信息归一化

		// 终端误差状态估计
		mat FSE;
		FSE.reset();

		if (is_FSE) {		// 会报错，tf_ans的值有问题
			FinalStateEstimate(U_ans, Y_ans, MeshGrid, t0_ans, tf_ans, N, method, time_ans,
				interval_num, Ts1_Te1_ans, auxdata, length_state, length_control, interp_method, Dynamics_temp1, &FSE);
		}
		// 结果评价与网格细化策略
		mat Ts1_Te1_nextstep, MeshGrid_nextstep, inum_interval_increase;
		struct error0 error;
		result_assess(&MeshGrid_nextstep, &Ts1_Te1_nextstep, &error, &inum_interval_increase,
			U_ans, Y_ans, MeshGrid, time_ans1, t0_ans, tf_ans,
			N, mesh, Col_Points, method,
			interval_num, Ts1_Te1_ans, auxdata,
			length_state, length_control, niu, print_level, testmode, CC, Dynamics_temp1);
		double max_re_error = error.max_re_error_max;
		vec re_error_max = error.re_error_max;

		// 雅克比矩阵结构重构
		Jacb_struct_Re(Jacb_struct_info, MeshGrid_nextstep, length_state, length_control, length_path, length_event, length_integral, length_parameter, &Jacb_struct0);
		// 乘子更新(暂时不做)

		// output输出赋值
		output[iteration].result.time = time_ans;
		output[iteration].result.state = Y_ans;
		output[iteration].result.control = U_ans;
		output[iteration].result.objective = Objective_ans;
		output[iteration].result.mesh_N = MeshGrid.col(0).t();
		output[iteration].result.mesh_time = Ts_Te_ans;
		output[iteration].result.auxdata = auxdata;
		output[iteration].re_error_max = re_error_max;
		output[iteration].Final_State_Estimation = FSE;

		(*iteration_final) = iteration;

		// 信息输出
		output[iteration].result.max_re_error = max_re_error;
		if (print_level != 0) {
			cout << "	当前最大相对误差为" << max_re_error << endl;
			if (is_FSE) {
				cout << "	终端偏差预估值为 " << FSE.row(0) - Y_ans.row(Y_ans.n_rows - 1) << " " << endl;
			}
		}
		iteration = iteration + 1;

		// 7 是否满足终止条件
		if (max_re_error <= mesh.tolerance || iteration > mesh.maxiterration) {
			if (max_re_error <= mesh.tolerance) {
				cout << "相对误差满足要求，迭代终止;" << endl;
			}
			else if (iteration >= mesh.maxiterration) {
				cout << "迭代次数超过用户设定值，迭代终止;" << endl;
			}
			IsStop = 1;	
		}
		cout << " " << endl;
		// 8 参数更新
		MeshGrid = MeshGrid_nextstep;
		Ts1_Te1 = Ts1_Te1_nextstep;
		tf = tf_ans;
		t0 = t0_ans;
		Y_ans1 = Y_ans;
		U_ans1 = U_ans;
	}

	return output;
}