#include "cmscp.h"
#include "cmscp_sparseInfo.h"
#include "cmscp_consTypeInfo.h"
#include "cmscp_socpInfo.h"
#include "timer.h"
#include "printFunction.h"
#include "socSolveShell.h"
#include "freeSparseInfo.h"
#include "freeConsTypeInfo.h"
#include "freeSocpInfo.h"
#include "freeTrajInfo.h"

int cmscp(cmscp_trajInfo** optTraj, cmscp_setup* setup) {

	// 变量声明
	int iphase, iIter, stopFlag;
	double time;
	timer timer;
	cmscp_trajInfo** refTraj;
	cmscp_sparseInfo* sparseInfo;
	cmscp_consTypeInfo* consTypeInfo;
	cmscp_socpInfo* socpInfo;
	
	///* 函数名 */
	cmscp_getIniRefTraj getIniRefTrajFunc = setup->functions->getIniRefTraj; // 获取参考轨迹的函数
	cmscp_stopCondition stopConditionFunc = setup->functions->stopCondition; // 计算停止条件的函数
	cmscp_updateMeshInfo updateMeshInfoFunc = setup->functions->updateMeshInfo; // 更新网格信息的函数
	cmscp_updateTrustInfo updateTrustInfoFunc = setup->functions->updateTrustInfo; // 更新信赖域信息的函数
	cmscp_updateRefTraj updateRefTrajFunc = setup->functions->updateRefTraj; // 更新参考轨迹信息的函数

	/* 1. 获取参考轨迹 */
	refTraj = (cmscp_trajInfo**)malloc(setup->numPhase * sizeof(cmscp_trajInfo*));
	getIniRefTrajFunc(refTraj, setup);

	/* 2. 获取各个矩阵的稀疏信息 */
	getSparseInfo(&sparseInfo, refTraj, setup);// 不想在外面给refTraj赋值（给指针赋值就是分配空间），就把使用refTraj的地址了（声明变量时候自带的）

	/* 3. 获取约束的类型信息 */
	getConsTypeInfo(&consTypeInfo, setup);

	for (iIter = 0; iIter < setup->scpInfo->maxIter; iIter++)
	{
		/* 4. 获取二阶锥问题的参数 */
		tic(&timer);
		getSocpInfo(&socpInfo, refTraj, sparseInfo, consTypeInfo, setup);
		time = toc(&timer);

		/* 6. 求解子问题最优解 */
		socSolveShell(iIter, optTraj, socpInfo, setup);
		
		/* 7. 停止条件 */
		stopFlag = stopConditionFunc(iIter, refTraj, optTraj, setup);

		/* 8. 更新信赖域 */
		updateTrustInfoFunc(iIter, refTraj, optTraj, setup);

		/* 9. 更新网格 */
		updateMeshInfoFunc(iIter, refTraj, optTraj, setup);

		/* 10. 更新参考轨迹 */
		updateRefTrajFunc(refTraj, optTraj, setup);

		// 停止条件达到就停止
		if (stopFlag > 0)
		{
			setup->scpInfo->cntIter = iIter;
			break;
		}
	}
	// 释放SparseInfo
	freeSparseInfo(&sparseInfo, setup);
	// 释放ConsTypeInfo
	freeConsTypeInfo(&consTypeInfo, setup);
	// 输出最优轨迹
	for ( iphase = 0; iphase < setup->numPhase; iphase++)
	{
		optTraj[iphase] = refTraj[iphase];
	}
	free(refTraj);
	// 输出停止标志位
	return stopFlag;
}