/*
% 函数功能：
%       评估IPOPT求解器的求解结果，并据此进行网格的细化
% 输入：
%       Y_phase_ans:状态量矩阵
%       U_phase_ans：控制量矩阵
%       tf：最大时间
%       t0_ans：新的最小时间
%       tf_ans：新的最大时间
%       interval_num:间隔数目
% 输出：
%       B_phase：阶段内的边界约束矩阵
% 编写：
%       
		2021/08/16
*/

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

using namespace std;
using namespace arma;

int FinalStateEstimate(mat U_ans, mat Y_ans, mat MeshGrid, double t0_ans, double tf_ans, uvec N, char *method, vec time_ans, 
						int interval_num, mat Ts1_Te1_ans, auxdata1 auxdata, int length_state, int length_control,char *interp_method,
						int(*Dynamics_ptr)(mat* StateVar, mat* ControlVar, auxdata1 auxdata, mat* dState, mat* integrand, mat* path),
						mat* FSE) {

	// 升阶后的时间
	uvec N_Col, N_interp, last_col;
	Method2Nk(method, N, &N_Col);						// 新的配置点数目
	int Col_total = sum(N_Col);
	N_Col2interp(method, N_Col, &N_interp);             // 配置点数目到插值点数目的转换
	last_col = ones<uvec>(interval_num);
	for (int i = 1; i <= interval_num; i++) {
		int temp1 = sum(N_interp.rows(0, i - 1));
		last_col(i - 1) = temp1 - i;
	}
	vec Time_interval_ans;
	Time2Newtime(Ts1_Te1_ans, MeshGrid, tf_ans, t0_ans, t0_ans, tf_ans, interval_num, Col_total, last_col, &Time_interval_ans);	// 得到列向量
	vec time_ans_new = zeros(Col_total + 1, 1);
	time_ans_new.rows(0, Col_total - 1) = Time_interval_ans;
	time_ans_new(size(time_ans_new)(0) - 1) = tf_ans;
	mat U_average(Col_total + 1, length_control, fill::zeros); vec U_average_vec;
	for (int i = 1; i <= length_control; i++ ) {
		vec U_temp = U_ans.col(i - 1);
		interp1(time_ans, U_temp, time_ans_new, U_average_vec, interp_method);
		U_average.col(i - 1) = U_average_vec;
	}
	int n_length = size(U_average)(0);

	//////////////////////考虑积分的伪谱法修改////////////////////////
	// 计算由每个起点积分到终点时，终点的差值。
	(*FSE) = zeros(interval_num, Y_ans.n_cols);
	for (int i = 1; i <= interval_num; i++) {
		uvec last_col1;
		if (i == 1) {
			last_col1 = last_col.rows(i - 1, last_col.n_elem - 1);
		}
		else {
			last_col1 = last_col.rows(i - 1, last_col.n_elem - 1) - last_col(i - 2);
		}
		vec time_ans1 = zeros(last_col1(last_col1.n_elem - 1) + 1);
		vec time_ans1_temp = time_ans1.rows(0, last_col1(last_col1.n_elem - 1));
		Time2Newtime(Ts1_Te1_ans.rows(i - 1, Ts1_Te1_ans.n_rows - 1), MeshGrid.rows(i - 1, MeshGrid.n_rows - 1), 
			tf_ans, t0_ans, t0_ans, tf_ans, interval_num - i + 1, sum(N_Col.rows(i - 1, N_Col.n_elem - 1)), 
			last_col1, &time_ans1_temp);
		time_ans1.rows(0, last_col1(last_col1.n_elem - 1) - 1) = time_ans1_temp;
		time_ans1(last_col1(last_col1.n_elem - 1)) = tf_ans;
		vec t_interp; t_interp.reset();
		for (int i2 = 1; i2 <= last_col1(last_col1.n_elem - 1); i2++) {
			vec t_interp0 = linspace(time_ans1(i2 - 1), time_ans1(i2), 12);
			t_interp0 = t_interp0.rows(0, t_interp0.n_elem - 2);	// 删去最后一行
			t_interp = join_cols(t_interp, t_interp0);
		}
		vec tf_ans_vec(1); tf_ans_vec(0) = tf_ans;
		t_interp = join_cols(t_interp, tf_ans_vec);
		mat U_average1(size(t_interp)(0), length_control, fill::zeros);
		for (int i = 1; i <= length_control; i++) {
			vec U_temp = U_ans.col(i - 1);
			interp1(time_ans, U_temp, t_interp, U_average_vec, interp_method);
			U_average1.col(i - 1) = U_average_vec;
		}
		//interp1(time_ans, U_ans, t_interp, U_average, interp_method);
		n_length = size(U_average1)(0);

		// 梯形欧拉积分
		mat state_int = zeros(n_length, Y_ans.n_cols);
		state_int.row(0) = Y_ans.row(0);
		for (int i_num = 1; i_num <= n_length - 1; i_num++) {
			mat dy, dy1, integrand, path;
			mat state_int_temp = state_int.row(i_num - 1);
			mat U_average_temp = U_average1.row(i_num - 1);
			(*Dynamics_ptr)(&state_int_temp, &U_average_temp, auxdata, &dy, &integrand, &path);
			state_int.row(i_num - 1) = state_int_temp; U_average1.row(i_num - 1) = U_average_temp;
			mat state_temp = state_int.row(i_num - 1) + (t_interp(i_num) - t_interp(i_num - 1))*dy;
			(*Dynamics_ptr)(&state_temp, &U_average_temp, auxdata, &dy1, &integrand, &path);
			state_int.row(i_num) = state_int.row(i_num - 1) + (t_interp(i_num) - t_interp(i_num - 1))*(dy1 + dy) / 2;
		}
		(*FSE).row(i - 1) = state_int.row(state_int.n_rows - 1);
	}

	return 1;
}