#include "socSolveShell.h"
#include "denseBlas.h"
#include "freeSocpInfo.h"
#include "printFunction.h"
void socSolveShell(int iIter, cmscp_trajInfo** optTraj, cmscp_socpInfo* socpInfo, cmscp_setup* setup)
{
	
	int exitflag_solver = ECOS_FATAL, iphase, i;
	double* X = NULL, *temp, obj;
	int numMesh, numState, numControl, numParameter, numPhase, *numPhaseOptVar, lengthState, lengthControl, idxOptVarState, idxOptVarPhaseStart, idxOptVarControl, idxOptVarT0, idxOptVarTf, idxOptVarParameter;
	cmscp_dimension* dimension;
	cmscp_mesh** mesh;
	pwork* ecosWork = NULL;
	csocp_prob* prob = NULL;
	// 问题维数
	dimension = setup->dimension;
	numPhase = setup->numPhase;
	mesh = setup->mesh;
	if (strcmp(setup->scpInfo->solver->name, "ecos") == 0)
	{
		ecosWork = ECOS_setup(socpInfo->numOptVar, socpInfo->ACone1->m, socpInfo->AEqua1->m,
			socpInfo->dimCone->l, socpInfo->dimCone->nsoc, socpInfo->dimCone->q, 0, socpInfo->ACone1->v,
			socpInfo->ACone1->ptrCol, socpInfo->ACone1->idxRow, socpInfo->AEqua1->v, socpInfo->AEqua1->ptrCol,
			socpInfo->AEqua1->idxRow, socpInfo->c, socpInfo->bCone, socpInfo->bEqua);
		exitflag_solver = ECOS_solve(ecosWork);
		X = ecosWork->x;
	}
	else 
	{
		// CSOCP
		CSOCP_create(&prob);
		CSOCP_setdblparam(prob, CSOCP_DBLPARAM_FEASTOL, setup->scpInfo->solver->feastol);  // 设置可行性容差
		CSOCP_setdblparam(prob, CSOCP_DBLPARAM_ABSTOL, setup->scpInfo->solver->abstol);
		CSOCP_setdblparam(prob, CSOCP_DBLPARAM_RELTOL, setup->scpInfo->solver->reltol);

		CSOCP_setintparam(prob, CSOCP_INTPARAM_ITERLIMIT, setup->scpInfo->solver->maxit);  // 设置最大迭代次数
		CSOCP_loadsocp(prob, socpInfo->numOptVar, socpInfo->ACone1->m, socpInfo->AEqua1->m,
			socpInfo->dimCone->l, socpInfo->dimCone->nsoc, socpInfo->dimCone->q, socpInfo->ACone1->v,
			socpInfo->ACone1->ptrCol, socpInfo->ACone1->idxRow, socpInfo->AEqua1->v, socpInfo->AEqua1->ptrCol,
			socpInfo->AEqua1->idxRow, socpInfo->c, socpInfo->bCone, socpInfo->bEqua);
		CSOCP_solve(prob);
		X = (double*)malloc(socpInfo->numOptVar * sizeof(double));
		CSOCP_getsolution(prob, X, NULL, NULL, NULL);
		
	}


	// 目标函数
	obj = 0;
	for ( i = 0; i < socpInfo->numOptVar; i++)
	{
		obj = obj + socpInfo->c[i] * X[i];
	}
	setup->scpInfo->obj[iIter] = obj / (cmscp_norm2(socpInfo->c, socpInfo->numOptVar) + 1);
	// 插值获取参考轨迹
	numPhaseOptVar = socpInfo->numPhaseOptVar;
	idxOptVarPhaseStart = 0;
	for (iphase = 0; iphase < numPhase; iphase++)
	{
		numMesh = mesh[iphase]->num;
		numState = dimension->phase[iphase]->state;
		numControl = dimension->phase[iphase]->control;
		numParameter = dimension->phase[iphase]->parameter;
		lengthState = numMesh * numState;
		lengthControl = numMesh * numControl;
		idxOptVarState = idxOptVarPhaseStart;
		idxOptVarControl = idxOptVarState + lengthState;
		idxOptVarT0 = idxOptVarControl + lengthControl;
		idxOptVarTf = idxOptVarT0 + 1;
		idxOptVarParameter = idxOptVarTf + 1;

		optTraj[iphase] = (cmscp_trajInfo*)malloc(sizeof(cmscp_trajInfo));
		optTraj[iphase]->state = (cmscp_floatDemat*)malloc(sizeof(cmscp_floatDemat));
		optTraj[iphase]->state->m = numState;
		optTraj[iphase]->state->n = numMesh;
		optTraj[iphase]->state->v = (double*)malloc(lengthState * sizeof(double));
		optTraj[iphase]->control = (cmscp_floatDemat*)malloc(sizeof(cmscp_floatDemat));
		optTraj[iphase]->control->m = numControl;
		optTraj[iphase]->control->n = numMesh;
		optTraj[iphase]->control->v = (double*)malloc(lengthControl * sizeof(double));
		optTraj[iphase]->parameter = (double*)malloc(numParameter * sizeof(double));
		
		temp = X + idxOptVarState;
		copyFloatVec(temp, optTraj[iphase]->state->v, lengthState, 0);
		temp = X + idxOptVarControl;
		copyFloatVec(temp, optTraj[iphase]->control->v, lengthControl, 0);
		temp = X + idxOptVarParameter;
		copyFloatVec(temp, optTraj[iphase]->parameter, numParameter, 0);
		temp = X + idxOptVarT0;
		optTraj[iphase]->initialtime = *temp;
		temp = X + idxOptVarTf;
		optTraj[iphase]->finaltime = *temp;

		idxOptVarPhaseStart = idxOptVarPhaseStart + numPhaseOptVar[iphase];
	}
	if (strcmp(setup->scpInfo->solver->name, "ecos") == 0)
	{
		ECOS_cleanup(ecosWork, 0); // 有一个内存释放不掉
	}
	else
	{
		free(X);
		CSOCP_free(&prob);
	}
	freeSocpInfo(&socpInfo, setup, 1); // 释放socpInfo的全部数据空间
}