#include "gradInfoFunctions.h"

void copyObjEventInput(cmscp_objEventInput*x, cmscp_objEventInput* y, cmscp_setup* setup)
{
	// 函数功能：复制copyObjEventInput形变量
	// 输入：
	// x：原本
	// setup：问题设置参数
	// 输出：
	// y：复本
	// 谢磊：2022 / 6 / 28编写
	int iphase, numPhase, numState, numParameter;
	cmscp_dimension* dimension;

	numPhase = setup->numPhase;	// 阶段数
	dimension = setup->dimension;
	for ( iphase = 0; iphase < numPhase; iphase++)
	{
		numState = dimension->phase[iphase]->state;
		numParameter = dimension->phase[iphase]->parameter;
		y->phase[iphase]->initialtime = x->phase[iphase]->initialtime;
		y->phase[iphase]->finaltime = x->phase[iphase]->finaltime;
		copyFloatVec(x->phase[iphase]->initialstate, y->phase[iphase]->initialstate, numState, 0);
		copyFloatVec(x->phase[iphase]->finalstate, y->phase[iphase]->finalstate, numState, 0);
		copyFloatVec(x->phase[iphase]->parameter, y->phase[iphase]->parameter, numParameter, 0);
	}

}

void copyEndpInput(cmscp_endpInput* x, cmscp_endpInput* y, int iphase, cmscp_setup* setup)
{
	// 函数功能：复制cmscp_endpInput形变量
	// 输入：
	// x：原本
	// setup：问题设置参数
	// 输出：
	// y：复本
	// 谢磊：2022 / 6 / 28编写
	int numPhase;
	cmscp_dimension* dimension;

	numPhase = setup->numPhase;	// 阶段数
	dimension = setup->dimension;
	copyFloatVec(x->initialstate, y->initialstate, dimension->phase[iphase]->state, 0);
	copyFloatVec(x->finalstate, y->finalstate, dimension->phase[iphase]->state, 0);
	copyFloatVec(x->parameter, y->parameter, dimension->phase[iphase]->parameter, 0);
	y->initialtime = x->initialtime;
	y->finaltime = x->finaltime;
}

void copyContInput(cmscp_contInput* x, cmscp_contInput* y, int numState, int numControl, int numParameter, cmscp_setup* setup)
{
	// 函数功能：复制cmscp_endpInput形变量
	// 输入：
	// x：原本
	// setup：问题设置参数
	// 输出：
	// y：复本
	// 谢磊：2022 / 6 / 28编写
	cmscp_dimension* dimension;
	dimension = setup->dimension;
	copyFloatVec(x->state, y->state, numState, 0);
	copyFloatVec(x->control, y->control, numControl, 0);
	copyFloatVec(x->parameter, y->parameter, numParameter, 0);
}

void copyLinkInput(cmscp_linkInput* x, cmscp_linkInput* y, int iphase, cmscp_setup* setup)
{
	// 函数功能：复制cmscp_endpInput形变量
	// 输入：
	// x：原本
	// setup：问题设置参数
	// 输出：
	// y：复本
	// 谢磊：2022 / 6 / 28编写
	cmscp_dimension* dimension;
	dimension = setup->dimension;
	y->leftFinTime = x->leftFinTime;
	y->rightIniTime = x->rightIniTime;
	copyFloatVec(x->leftFinState, y->leftFinState, dimension->phase[iphase]->state, 0);
	copyFloatVec(x->leftParameter, y->leftParameter, dimension->phase[iphase]->parameter, 0);
	copyFloatVec(x->rightIniState, y->rightIniState, dimension->phase[iphase+1]->state, 0);
	copyFloatVec(x->rightParameter, y->rightParameter, dimension->phase[iphase+1]->parameter, 0);
}
void gradInfoSparseObjEvent(cmscp_derInfo** objDerInfo0, cmscp_derInfo** eventDerInfo0, cmscp_sparseInfo* sparseInfo, cmscp_trajInfo** refTraj, cmscp_setup* setup)
{
	// 函数功能：获取二阶锥规划问题的参数的维数以及分配相应的空间
	// 输入：
	// refTraj：参考轨迹
	// sparseInfo：稀疏（非零元位置）信息
	// setup：问题设置参数
	// 输出：
	// objDerInfo：目标函数的一阶导数信息
	// eventDerInfo：事件函数的一阶导数信息
	// Tri表示软信赖域边界
	// 谢磊：2022 / 6 / 27编写

	// 变量声明
	int numPhase, numObjEventInput, numEventOutput, iphase, numState, numControl, numParameter, numMesh, cntObjEventInput, *iniStateNzIdxRow, * finStateNzIdxRow, * iniTimeNzIdxRow, * finTimeNzIdxRow, * parameterNzIdxRow, *iniStateNzPtrCol, * finStateNzPtrCol, * iniTimeNzPtrCol, * finTimeNzPtrCol, * parameterNzPtrCol, j, p, i, idxRowEventGrd;
	double derStep, derStepInv, refObjOutput, pertObjOutput, *refEventOutput, *pertEventOutput, *objGrd, * eventGrd;
	cmscp_dimension* dimension;
	cmscp_mesh** mesh;
	cmscp_nzPos_endp** objEventNzPos;
	cmscp_objEventInput* objEventInput, *pertObjEventInput;
	cmscp_objEvent objEventFunc;

	//  1. 基本参数
	dimension = setup->dimension;	// 问题维数信息
	mesh = setup->mesh;	// 网格信息
	numPhase = setup->numPhase;	// 阶段数
	derStep = setup->derStep; // 求导步长
	derStepInv = 1 / derStep;
	objEventFunc = setup->functions->objEvent;
	numObjEventInput = 0; // 端点函数的输入变量个数
	objEventNzPos = sparseInfo->objEventNzPos; // 稀疏信息

	// 事件函数的输出维数
	if (dimension->event->soc->n)
	{
		numEventOutput = dimension->event->nl + cmscp_sum(dimension->event->soc->q, dimension->event->soc->n);
	}
	else
	{
		numEventOutput = dimension->event->nl;
	}
	// 函数的输入
	objEventInput = (cmscp_objEventInput*)malloc(sizeof(cmscp_objEventInput));
	objEventInput->phase = (cmscp_objEventInputPhase**)malloc(setup->numPhase * sizeof(cmscp_objEventInputPhase*));
	objEventInput->auxdata = setup->auxdata;
	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;
		objEventInput->phase[iphase]  = (cmscp_objEventInputPhase*)malloc(sizeof(cmscp_objEventInputPhase));
		objEventInput->phase[iphase]->initialtime = refTraj[iphase]->initialtime;
		objEventInput->phase[iphase]->finaltime = refTraj[iphase]->finaltime;
		objEventInput->phase[iphase]->initialstate = refTraj[iphase]->state->v;
		objEventInput->phase[iphase]->finalstate = refTraj[iphase]->state->v + (numMesh - 1) * numState;
		objEventInput->phase[iphase]->parameter = refTraj[iphase]->parameter;

		numObjEventInput = numObjEventInput + 2 * dimension->phase[iphase]->state + 2 + dimension->phase[iphase]->parameter;
	}
	cntObjEventInput = 0; // 端点函数输入变量计数
	refEventOutput = (double*)malloc(numEventOutput * sizeof(double));
	objEventFunc(&refObjOutput, refEventOutput, objEventInput); // 参考输出值

	objGrd = (double*)malloc(numObjEventInput * sizeof(double)); // 目标函数梯度空间分配
	eventGrd = (double*)malloc(numEventOutput * numObjEventInput * sizeof(double));
	//if (numEventOutput)
	//{
	//	eventGrd = (double*)malloc(numEventOutput * numObjEventInput * sizeof(double));
	//}

	// 扰动空间分配
	pertEventOutput = (double*)malloc(numEventOutput * sizeof(double));
	pertObjEventInput = (cmscp_objEventInput*)malloc(sizeof(cmscp_objEventInput));
	pertObjEventInput->phase = (cmscp_objEventInputPhase**)malloc(setup->numPhase * sizeof(cmscp_objEventInputPhase*));
	for ( iphase = 0; iphase < numPhase; iphase++)
	{
		pertObjEventInput->phase[iphase] = (cmscp_objEventInputPhase*)malloc(sizeof(cmscp_objEventInputPhase));
		pertObjEventInput->phase[iphase]->initialstate = (double*)malloc(numState * sizeof(double));
		pertObjEventInput->phase[iphase]->finalstate = (double*)malloc(numState * sizeof(double));
		pertObjEventInput->phase[iphase]->parameter = (double*)malloc(numParameter * sizeof(double));
	}

	for (iphase = 0; iphase < numPhase; iphase++)
	{
		// 离散点个数和各种变量的维数
		numState = dimension->phase[iphase]->state;
		numParameter = dimension->phase[iphase]->parameter;
		// 稀疏信息

		iniStateNzIdxRow = objEventNzPos[iphase]->iniState->idxRow;
		finStateNzIdxRow = objEventNzPos[iphase]->finState->idxRow;
		iniTimeNzIdxRow = objEventNzPos[iphase]->iniTime->idxRow;
		finTimeNzIdxRow = objEventNzPos[iphase]->finTime->idxRow;
		parameterNzIdxRow = objEventNzPos[iphase]->parameter->idxRow;
		iniStateNzPtrCol = objEventNzPos[iphase]->iniState->ptrCol;
		finStateNzPtrCol = objEventNzPos[iphase]->finState->ptrCol;
		iniTimeNzPtrCol = objEventNzPos[iphase]->iniTime->ptrCol;
		finTimeNzPtrCol = objEventNzPos[iphase]->finTime->ptrCol;
		parameterNzPtrCol = objEventNzPos[iphase]->parameter->ptrCol;

		pertObjEventInput->auxdata = setup->auxdata;
		numMesh = mesh[iphase]->num;
		numState = dimension->phase[iphase]->state;
		numControl = dimension->phase[iphase]->control;
		numParameter = dimension->phase[iphase]->parameter;

		// 初始状态
		for (j = 0; j < numState; j++)
		{
			// 复制扰动输入
			copyObjEventInput(objEventInput, pertObjEventInput, setup);
			pertObjEventInput->phase[iphase]->initialstate[j] = pertObjEventInput->phase[iphase]->initialstate[j] + derStep;
			// 施加扰动
			objEventFunc(&pertObjOutput, pertEventOutput, pertObjEventInput);// 扰动输出
			for (p = iniStateNzPtrCol[j]; p < iniStateNzPtrCol[j + 1]; p++)
			{
				i = iniStateNzIdxRow[p]; //行
				if (i == 0)
				{
					objGrd[cntObjEventInput] = (pertObjOutput - refObjOutput) * derStepInv;
				}
				else
				{
					idxRowEventGrd = (i - 1);
					eventGrd[idxRowEventGrd + numEventOutput * cntObjEventInput] = (pertEventOutput[idxRowEventGrd] - refEventOutput[idxRowEventGrd]) * derStepInv;
				}
			}
			cntObjEventInput++;
		}


		// 终端状态
		for (j = 0; j < numState; j++)
		{
			// 复制扰动输入
			copyObjEventInput(objEventInput, pertObjEventInput, setup);
			pertObjEventInput->phase[iphase]->finalstate[j] = pertObjEventInput->phase[iphase]->finalstate[j] + derStep;
			// 施加扰动
			objEventFunc(&pertObjOutput, pertEventOutput, pertObjEventInput);// 扰动输出
			for (p = finStateNzPtrCol[j]; p < finStateNzPtrCol[j + 1]; p++)
			{
				i = finStateNzIdxRow[p]; //行
				if (i == 0)
				{
					objGrd[cntObjEventInput] = (pertObjOutput - refObjOutput) * derStepInv;
				}
				else
				{
					idxRowEventGrd = (i - 1);
					eventGrd[idxRowEventGrd + numEventOutput * cntObjEventInput] = (pertEventOutput[idxRowEventGrd] - refEventOutput[idxRowEventGrd]) * derStepInv;
				}
			}
			cntObjEventInput++;
		}

		// 初始时刻
		// 复制扰动输入
		copyObjEventInput(objEventInput, pertObjEventInput, setup);
		pertObjEventInput->phase[iphase]->initialtime = pertObjEventInput->phase[iphase]->initialtime + derStep;
		// 施加扰动
		objEventFunc(&pertObjOutput, pertEventOutput, pertObjEventInput);// 扰动输出
		for (p = iniTimeNzPtrCol[0]; p < iniTimeNzPtrCol[1]; p++)
		{
			i = iniTimeNzIdxRow[p]; //行
			if (i == 0)
			{
				objGrd[cntObjEventInput] = (pertObjOutput - refObjOutput) * derStepInv;
			}
			else
			{
				idxRowEventGrd = (i - 1);
				eventGrd[idxRowEventGrd + numEventOutput * cntObjEventInput] = (pertEventOutput[idxRowEventGrd] - refEventOutput[idxRowEventGrd]) * derStepInv;
			}
		}
		cntObjEventInput++;
		// 终端时刻
		// 复制扰动输入
		copyObjEventInput(objEventInput, pertObjEventInput, setup);
		pertObjEventInput->phase[iphase]->finaltime = pertObjEventInput->phase[iphase]->finaltime + derStep;
		// 施加扰动
		objEventFunc(&pertObjOutput, pertEventOutput, pertObjEventInput);// 扰动输出
		for (p = finTimeNzPtrCol[0]; p < finTimeNzPtrCol[1]; p++)
		{
			i = finTimeNzIdxRow[p]; //行
			if (i == 0)
			{
				objGrd[cntObjEventInput] = (pertObjOutput - refObjOutput) * derStepInv;
			}
			else
			{
				idxRowEventGrd = (i - 1);
				eventGrd[idxRowEventGrd + numEventOutput * cntObjEventInput] = (pertEventOutput[idxRowEventGrd] - refEventOutput[idxRowEventGrd]) * derStepInv;
			}
		}
		cntObjEventInput++;

		// 终端状态
		for (j = 0; j < numParameter; j++)
		{
			// 复制扰动输入
			copyObjEventInput(objEventInput, pertObjEventInput, setup);
			pertObjEventInput->phase[iphase]->parameter[j] = pertObjEventInput->phase[iphase]->parameter[j] + derStep;
			// 施加扰动
			objEventFunc(&pertObjOutput, pertEventOutput, pertObjEventInput);// 扰动输出
			for (p = parameterNzPtrCol[j]; p < parameterNzPtrCol[j + 1]; p++)
			{
				i = parameterNzIdxRow[p]; //行
				if (i == 0)
				{
					objGrd[cntObjEventInput] = (pertObjOutput - refObjOutput) * derStepInv;
				}
				else
				{
					idxRowEventGrd = (i - 1);
					eventGrd[idxRowEventGrd + numEventOutput * cntObjEventInput] = (pertEventOutput[idxRowEventGrd] - refEventOutput[idxRowEventGrd]) * derStepInv;
				}
			}
			cntObjEventInput++;
		}
	}
	// 输出
	// 内存分配
	(*objDerInfo0) = (cmscp_derInfo*)malloc(sizeof(cmscp_derInfo));
	(*objDerInfo0)->ref = (double*)malloc(sizeof(double));
	(*objDerInfo0)->ref[0] = refObjOutput;
	(*objDerInfo0)->der = objGrd;
	(*eventDerInfo0) = (cmscp_derInfo*)malloc(sizeof(cmscp_derInfo));
	(*eventDerInfo0)->ref = refEventOutput;
	(*eventDerInfo0)->der = eventGrd;

	// free 
	free(pertEventOutput);
	for (iphase = 0; iphase < numPhase; iphase++)
	{
		free(pertObjEventInput->phase[iphase]->initialstate);
		free(pertObjEventInput->phase[iphase]->finalstate);
		free(pertObjEventInput->phase[iphase]->parameter);
		free(pertObjEventInput->phase[iphase]);
		free(objEventInput->phase[iphase]);
	}
	free(pertObjEventInput->phase);
	free(pertObjEventInput);
	free(objEventInput->phase);
	free(objEventInput);
	
}
void gradInfoSparseEndp(cmscp_derInfo** gradInfo, cmscp_sparseInfo* sparseInfo, cmscp_trajInfo** refTraj, cmscp_setup* setup) {
	// 变量声明
	int numPhase, iphase, numState, numParameter, * iniStateNzIdxRow, * finStateNzIdxRow, * iniTimeNzIdxRow, * finTimeNzIdxRow, * parameterNzIdxRow, * iniStateNzPtrCol, * finStateNzPtrCol, * iniTimeNzPtrCol, * finTimeNzPtrCol, * parameterNzPtrCol, j, p, i, numEndpInput, numEndpOutput, cntCol;
	double derStep, derStepInv, *refOutput, *pertOutput, * endpGrd;
	cmscp_dimension* dimension;
	cmscp_mesh** mesh;
	cmscp_nzPos_endp** endpNzPos;
	cmscp_endpInput* endpInput, * pertEndpInput;
	cmscp_endpoint endpFunc;

	//  1. 基本参数
	dimension = setup->dimension;	// 问题维数信息
	mesh = setup->mesh;	// 网格信息
	numPhase = setup->numPhase;	// 阶段数
	derStep = setup->derStep; // 求导步长
	derStepInv = 1 / derStep;
	endpNzPos = sparseInfo->endpNzPos;

	// 参考输入
	endpInput = (cmscp_endpInput*)malloc(sizeof(cmscp_endpInput));
	for ( iphase = 0; iphase < numPhase; iphase++)
	{
	
		numState = dimension->phase[iphase]->state;
		numParameter = dimension->phase[iphase]->parameter;
		numEndpInput = 2 * numState + numParameter + 2;
		if (dimension->phase[iphase]->endpoint->soc->n)
		{
			numEndpOutput = dimension->phase[iphase]->endpoint->nl + cmscp_sum(dimension->phase[iphase]->endpoint->soc->q, dimension->phase[iphase]->endpoint->soc->n);
		}
		else
		{
			numEndpOutput = dimension->phase[iphase]->endpoint->nl;
		}
		// 各段稀疏信息
		iniStateNzIdxRow = endpNzPos[iphase]->iniState->idxRow;
		iniStateNzPtrCol = endpNzPos[iphase]->iniState->ptrCol;
		finStateNzIdxRow = endpNzPos[iphase]->finState->idxRow;
		finStateNzPtrCol = endpNzPos[iphase]->finState->ptrCol;
		iniTimeNzIdxRow = endpNzPos[iphase]->iniTime->idxRow;
		iniTimeNzPtrCol = endpNzPos[iphase]->iniTime->ptrCol;
		finTimeNzIdxRow = endpNzPos[iphase]->finTime->idxRow;
		finTimeNzPtrCol = endpNzPos[iphase]->finTime->ptrCol;
		parameterNzIdxRow = endpNzPos[iphase]->parameter->idxRow;
		parameterNzPtrCol = endpNzPos[iphase]->parameter->ptrCol;

		// 参考输入
		endpInput->auxdata = setup->auxdata;
		endpInput->initialstate = refTraj[iphase]->state->v;
		endpInput->finalstate = refTraj[iphase]->state->v + refTraj[iphase]->state->m* (refTraj[iphase]->state->n-1);
		endpInput->initialtime = refTraj[iphase]->initialtime;
		endpInput->finaltime = refTraj[iphase]->finaltime;
		endpInput->parameter = refTraj[iphase]->parameter;
		endpFunc = setup->functions->phase[iphase]->endpoint;

		// 扰动输入空间分配
		pertEndpInput = (cmscp_endpInput*)malloc(sizeof(cmscp_endpInput));
		pertEndpInput->initialstate = (double*)malloc(numState * sizeof(double));
		pertEndpInput->finalstate = (double*)malloc(numState * sizeof(double));
		pertEndpInput->parameter = (double*)malloc(numParameter * sizeof(double));
		pertEndpInput->auxdata = setup->auxdata;
		
		// 参考输出空间分配
		refOutput = (double*)malloc(numEndpOutput * sizeof(double));
		// 扰动输出
		pertOutput = (double*)malloc(numEndpOutput * sizeof(double));

		// 雅可比矩阵的输入
		endpGrd = (double*)malloc(numEndpOutput * numEndpInput * sizeof(double));

		// 计算参考输出
		endpFunc(refOutput, endpInput);
		// 初始状态
		cntCol = 0;
		for ( j = 0; j < numState; j++)
		{
			copyEndpInput(endpInput, pertEndpInput, iphase, setup);
			pertEndpInput->initialstate[j] = pertEndpInput->initialstate[j] + derStep;
			endpFunc(pertOutput, pertEndpInput);
			for ( p = iniStateNzPtrCol[j]; p < iniStateNzPtrCol[j+1]; p++)
			{
				i = iniStateNzIdxRow[p];
				endpGrd[i + cntCol * numEndpOutput] = (pertOutput[i]- refOutput[i])*derStepInv;
			}
			cntCol++;
		}
		// 终端状态
		for (j = 0; j < numState; j++)
		{
			copyEndpInput(endpInput, pertEndpInput, iphase, setup);
			pertEndpInput->finalstate[j] = pertEndpInput->finalstate[j] + derStep;
			endpFunc(pertOutput, pertEndpInput);
			for (p = finStateNzPtrCol[j]; p < finStateNzPtrCol[j + 1]; p++)
			{
				i = finStateNzIdxRow[p];
				endpGrd[i + cntCol * numEndpOutput] = (pertOutput[i] - refOutput[i]) * derStepInv;
			}
			cntCol++;
		}
		// 初始时刻
		for (j = 0; j < 1; j++)
		{
			copyEndpInput(endpInput, pertEndpInput, iphase, setup);
			pertEndpInput->initialtime = pertEndpInput->initialtime + derStep;
			endpFunc(pertOutput, pertEndpInput);
			for (p = iniTimeNzPtrCol[j]; p < iniTimeNzPtrCol[j + 1]; p++)
			{
				i = iniTimeNzIdxRow[p];
				endpGrd[i + cntCol * numEndpOutput] = (pertOutput[i] - refOutput[i]) * derStepInv;
			}
			cntCol++;
		}
		// 终端时刻
		for (j = 0; j < 1; j++)
		{
			copyEndpInput(endpInput, pertEndpInput, iphase, setup);
			pertEndpInput->finaltime = pertEndpInput->finaltime + derStep;
			endpFunc(pertOutput, pertEndpInput);
			for (p = finTimeNzPtrCol[j]; p < finTimeNzPtrCol[j + 1]; p++)
			{
				i = finTimeNzIdxRow[p];
				endpGrd[i + cntCol * numEndpOutput] = (pertOutput[i] - refOutput[i]) * derStepInv;
			}
			cntCol++;
		}
		// 静态参数
		for (j = 0; j < numParameter; j++)
		{
			copyEndpInput(endpInput, pertEndpInput, iphase, setup);
			pertEndpInput->parameter[j] = pertEndpInput->parameter[j] + derStep;
			endpFunc(pertOutput, pertEndpInput);
			for (p = parameterNzPtrCol[j]; p < parameterNzPtrCol[j + 1]; p++)
			{
				i = parameterNzIdxRow[p];
				endpGrd[i + cntCol * numEndpOutput] = (pertOutput[i] - refOutput[i]) * derStepInv;
			}
			cntCol++;
		}
		gradInfo[iphase] = (cmscp_derInfo*)malloc(sizeof(cmscp_derInfo));
		gradInfo[iphase]->ref = refOutput;
		gradInfo[iphase]->der = endpGrd;

		// free空间
		free(pertEndpInput->initialstate);
		free(pertEndpInput->finalstate);
		free(pertEndpInput->parameter);
		free(pertEndpInput);
		free(pertOutput);
	}
	free(endpInput);
}

void gradInfoSparseCont(cmscp_derInfo** dynGradInfo0, cmscp_derInfo** pathGradInfo0, double* refState, double* refControl, double* refParameter,
	int* stateNzIdxRow, int* controlNzIdxRow, int* parameterNzIdxRow, int* stateNzPtrCol, int* controlNzPtrCol, int* parameterNzPtrCol,
	int numState, int numControl, int numParameter, int numPathOutput, int iphase,  cmscp_setup* setup) {

	int numPhase,  j, p, i,numContInput, cntContInput, idxRowPathGrd;
	double derStep, derStepInv, * refDynOutput, * refPathOutput, *refOutput, * pertOutput, * dynGrd, * pathGrd, * pertDynOutput, * pertPathOutput;
	cmscp_dimension* dimension;
	cmscp_mesh** mesh;
	cmscp_contInput* contInput, * pertContInput;
	cmscp_continuous contFunc;

	//  1. 基本参数
	dimension = setup->dimension;	// 问题维数信息
	mesh = setup->mesh;	// 网格信息
	numPhase = setup->numPhase;	// 阶段数
	derStep = setup->derStep; // 求导步长
	derStepInv = 1 / derStep;

	// 假设函数的自变量按照[x, u, p]排序
	// 函数的参考输入和扰动输入
	contInput = (cmscp_contInput*)malloc(sizeof(cmscp_contInput));// 参考输入
	contInput->auxdata = setup->auxdata; 
	contInput->state = refState;
	contInput->control = refControl;
	contInput->parameter = refParameter;
	pertContInput = (cmscp_contInput*)malloc(sizeof(cmscp_contInput));
	pertContInput->auxdata = setup->auxdata; // 扰动输入
	pertContInput->state = (double*)malloc(numState * sizeof(double));
	pertContInput->control = (double*)malloc(numControl * sizeof(double));
	pertContInput->parameter = (double*)malloc(numParameter * sizeof(double));

	// 连续函数
	contFunc = setup->functions->phase[iphase]->continuous;
	
	// 函数的参考输出和扰动输出
	refDynOutput = dynGradInfo0[0]->ref; // (double*)malloc(numState * sizeof(double));
	refPathOutput = pathGradInfo0[0]->ref; // (double*)malloc(numPathOutput * sizeof(double));
	contFunc(refDynOutput, refPathOutput, contInput); // 参考输出值
	pertDynOutput = (double*)malloc(numState * sizeof(double));
	pertPathOutput =  (double*)malloc(numPathOutput * sizeof(double));

	// 雅可比矩阵
	numContInput = numState + numControl + numParameter;
	dynGrd = dynGradInfo0[0]->der;// (double*)malloc(numState * numContInput * sizeof(double));
	pathGrd = pathGradInfo0[0]->der; //  (double*)malloc(numPathOutput * numContInput * sizeof(double));
	cntContInput = 0;
	for ( j = 0; j < numState; j++)
	{
		copyContInput(contInput, pertContInput, numState, numControl, numParameter, setup);
		pertContInput->state[j] = pertContInput->state[j] + derStep;
		contFunc(pertDynOutput, pertPathOutput, pertContInput);
		for ( p = stateNzPtrCol[j]; p < stateNzPtrCol[j+1]; p++)
		{
			i = stateNzIdxRow[p]; // 行
			if (i < numState)
			{
				dynGrd[i+cntContInput*numState] = (pertDynOutput[i] - refDynOutput[i]) * derStepInv;
			}
			else
			{
				idxRowPathGrd = i - numState;
				pathGrd[idxRowPathGrd + numPathOutput * cntContInput] = (pertPathOutput[idxRowPathGrd] - refPathOutput[idxRowPathGrd]) * derStepInv;;
			}
		}
		cntContInput++;
	}
	// 控制变量
	for (j = 0; j < numControl; j++)
	{
		copyContInput(contInput, pertContInput, numState, numControl, numParameter, setup);
		pertContInput->control[j] = pertContInput->control[j] + derStep;
		contFunc(pertDynOutput, pertPathOutput, pertContInput);
		for (p = controlNzPtrCol[j]; p < controlNzPtrCol[j + 1]; p++)
		{
			i = controlNzIdxRow[p]; // 行
			if (i < numState)
			{
				dynGrd[i+cntContInput * numState] = (pertDynOutput[i] - refDynOutput[i]) * derStepInv;
			}
			else
			{
				idxRowPathGrd = i - numState;
				pathGrd[idxRowPathGrd + numPathOutput * cntContInput] = (pertPathOutput[idxRowPathGrd] - refPathOutput[idxRowPathGrd]) * derStepInv;;
			}
		}
		cntContInput++;
	}
	// 静态参数变量
	for (j = 0; j < numParameter; j++)
	{
		copyContInput(contInput, pertContInput, numState, numControl, numParameter, setup);
		pertContInput->parameter[j] = pertContInput->parameter[j] + derStep;
		contFunc(pertDynOutput, pertPathOutput, pertContInput);
		for (p = parameterNzPtrCol[j]; p < parameterNzPtrCol[j + 1]; p++)
		{
			i = parameterNzIdxRow[p]; // 行
			if (i < numState)
			{
				dynGrd[i+cntContInput * numState] = (pertDynOutput[i] - refDynOutput[i]) * derStepInv;
			}
			else
			{
				idxRowPathGrd = i - numState;
				pathGrd[idxRowPathGrd + numPathOutput * cntContInput] = (pertPathOutput[idxRowPathGrd] - refPathOutput[idxRowPathGrd]) * derStepInv;;
			}
		}
		cntContInput++;
	}
	free(pertDynOutput);
	free(pertPathOutput);
	free(pertContInput->state);
	free(pertContInput->control);
	free(pertContInput->parameter);
	free(pertContInput);
	free(contInput);
	
}
void gradInfoSparseLink(cmscp_derInfo** gradInfo, cmscp_sparseInfo* sparseInfo, cmscp_trajInfo** refTraj, cmscp_setup* setup) {
	// 变量声明
	int numPhase, numObjEventInput, numEventOutput, iphase, numState, numControl, numParameter, numMesh, cntObjEventInput, * iniStateNzIdxRow, * finStateNzIdxRow, * iniTimeNzIdxRow, * finTimeNzIdxRow, * parameterNzIdxRow, * iniStateNzPtrCol, * finStateNzPtrCol, * iniTimeNzPtrCol, * finTimeNzPtrCol, * parameterNzPtrCol, j, p, i, idxRowEventGrd, numLinkInput, numLinkOutput, cntCol, * leftStatefIdxRow, * leftStatefPtrCol, * leftTfIdxRow, * leftTfPtrCol, * leftParameterIdxRow, * leftParameterPtrCol, * rightState0IdxRow, * rightState0PtrCol, * rightT0IdxRow, * rightT0PtrCol, * rightParameterIdxRow, * rightParameterPtrCol, numStateK, numStateKp1, numParameterK, numParameterKp1;;
	double derStep, derStepInv, * refOutput, * pertOutput, * linkGrd;
	cmscp_dimension* dimension;
	cmscp_mesh** mesh;
	cmscp_nzPos_link** linkNzPos;
	cmscp_linkInput* linkInput, * pertLinkInput;
	cmscp_linkage linkFunc;


	//  1. 基本参数
	dimension = setup->dimension;	// 问题维数信息
	mesh = setup->mesh;	// 网格信息
	numPhase = setup->numPhase;	// 阶段数
	derStep = setup->derStep; // 求导步长
	derStepInv = 1 / derStep;
	linkNzPos = sparseInfo->linkNzPos;

	// 参考输入
	linkInput = (cmscp_linkInput*)malloc(sizeof(cmscp_linkInput));
	pertLinkInput = (cmscp_linkInput*)malloc(sizeof(cmscp_linkInput));
	for (iphase = 0; iphase < numPhase-1; iphase++)
	{
		numStateK = dimension->phase[iphase]->state;
		numStateKp1 = dimension->phase[iphase + 1]->state;
		numParameterK = dimension->phase[iphase]->parameter;
		numParameterKp1 = dimension->phase[iphase + 1]->parameter;
		numLinkInput = numStateK + numStateKp1 + numParameterK + numParameterKp1 + 2;
		if (dimension->phase[iphase]->linkage->soc->n)
		{
			numLinkOutput = dimension->phase[iphase]->linkage->nl + cmscp_sum(dimension->phase[iphase]->linkage->soc->q, dimension->phase[iphase]->linkage->soc->n);
		}
		else
		{
			numLinkOutput = dimension->phase[iphase]->linkage->nl;
		}
		// 各段稀疏信息
		leftStatefIdxRow = linkNzPos[iphase]->leftStatef->idxRow;
		leftStatefPtrCol = linkNzPos[iphase]->leftStatef->ptrCol;
		leftTfIdxRow = linkNzPos[iphase]->leftTf->idxRow;
		leftTfPtrCol = linkNzPos[iphase]->leftTf->ptrCol;
		leftParameterIdxRow = linkNzPos[iphase]->leftParameter->idxRow;
		leftParameterPtrCol = linkNzPos[iphase]->leftParameter->ptrCol;
		rightState0IdxRow = linkNzPos[iphase]->rightState0->idxRow;
		rightState0PtrCol = linkNzPos[iphase]->rightState0->ptrCol;
		rightT0IdxRow = linkNzPos[iphase]->rightT0->idxRow;
		rightT0PtrCol = linkNzPos[iphase]->rightT0->ptrCol;
		rightParameterIdxRow = linkNzPos[iphase]->rightParameter->idxRow;
		rightParameterPtrCol = linkNzPos[iphase]->rightParameter->ptrCol;
		// 参考输入
		linkInput->auxdata = setup->auxdata;
		linkInput->leftFinState = refTraj[iphase]->state->v + refTraj[iphase]->state->m * (refTraj[iphase]->state->n - 1);
		linkInput->leftFinTime = refTraj[iphase]->finaltime;
		linkInput->leftParameter = refTraj[iphase]->parameter;
		linkInput->rightIniState = refTraj[iphase + 1]->state->v;
		linkInput->rightIniTime = refTraj[iphase + 1]->initialtime;
		linkInput->rightParameter = refTraj[iphase + 1]->parameter;
		// 衔接点函数
		linkFunc = setup->functions->phase[iphase]->linkage;

		// 扰动输入空间分配
		pertLinkInput->auxdata = setup->auxdata;
		pertLinkInput->leftFinState = (double*)malloc(dimension->phase[iphase]->state * sizeof(double));
		pertLinkInput->leftParameter = (double*)malloc(dimension->phase[iphase]->parameter * sizeof(double));
		pertLinkInput->rightIniState = (double*)malloc(dimension->phase[iphase+1]->state * sizeof(double));
		pertLinkInput->rightParameter = (double*)malloc(dimension->phase[iphase + 1]->parameter * sizeof(double));

		// 参考输出空间分配
		refOutput = (double*)malloc(numLinkOutput * sizeof(double));

		// 扰动输出
		pertOutput = (double*)malloc(numLinkOutput * sizeof(double));

		// 雅可比矩阵的输入
		linkGrd = (double*)malloc(numLinkOutput * numLinkInput * sizeof(double));

		// 计算参考输出
		linkFunc(refOutput, linkInput);

		// 左侧终端状态
		cntCol = 0;
		for (j = 0; j < numStateK; j++)
		{
			copyLinkInput(linkInput, pertLinkInput, iphase, setup);
			pertLinkInput->leftFinState[j] = pertLinkInput->leftFinState[j] + derStep;
			linkFunc(pertOutput, pertLinkInput);
			for (p = leftStatefPtrCol[j]; p < leftStatefPtrCol[j + 1]; p++)
			{
				i = leftStatefIdxRow[p];
				linkGrd[i + cntCol * numLinkOutput] = (pertOutput[i] - refOutput[i]) * derStepInv;
			}
			cntCol++;
		}
		// 左侧终端时刻
		copyLinkInput(linkInput, pertLinkInput, iphase, setup); // 扰动输入
		pertLinkInput->leftFinTime = pertLinkInput->leftFinTime + derStep;
		linkFunc(pertOutput, pertLinkInput);
		for ( p = leftTfPtrCol[0]; p < leftTfPtrCol[1]; p++)
		{
			i = leftTfIdxRow[p]; // 行
			linkGrd[i + cntCol * numLinkOutput] = (pertOutput[i] - refOutput[i]) * derStepInv;
		}
		cntCol = cntCol + 1;
		// 左侧静态参数
		for ( j = 0; j < numParameterK; j++)
		{
			copyLinkInput(linkInput, pertLinkInput, iphase, setup); // 扰动输入
			pertLinkInput->leftParameter[j] = pertLinkInput->leftParameter[j] + derStep;
			linkFunc(pertOutput, pertLinkInput);
			for ( p = leftParameterPtrCol[j]; p < leftParameterPtrCol[j + 1]; p++)
			{
				i = leftParameterIdxRow[p]; // 行
				linkGrd[i + cntCol * numLinkOutput] = (pertOutput[i] - refOutput[i]) * derStepInv;
			}
			cntCol = cntCol + 1;
		}

		// 右侧初始状态
		for (j = 0; j < numStateKp1; j++)
		{
			copyLinkInput(linkInput, pertLinkInput, iphase, setup); // 扰动输入
			pertLinkInput->rightIniState[j] = pertLinkInput->rightIniState[j] + derStep;
			linkFunc(pertOutput, pertLinkInput);
			for (p = rightState0PtrCol[j]; p < rightState0PtrCol[j + 1]; p++)
			{
				i = rightState0IdxRow[p]; // 行
				linkGrd[i + cntCol * numLinkOutput] = (pertOutput[i] - refOutput[i]) * derStepInv;
			}
			cntCol = cntCol + 1;
		}
		// 右侧终端时刻
		copyLinkInput(linkInput, pertLinkInput, iphase, setup); // 扰动输入
		pertLinkInput->rightIniTime = pertLinkInput->rightIniTime + derStep;
		linkFunc(pertOutput, pertLinkInput);
		for (p = rightT0PtrCol[0]; p < rightT0PtrCol[1]; p++)
		{
			i = rightT0IdxRow[p]; // 行
			linkGrd[i + cntCol * numLinkOutput] = (pertOutput[i] - refOutput[i]) * derStepInv;
		}
		cntCol = cntCol + 1;
		// 右侧静态参数
		for ( j = 0; j < numParameterKp1; j++)
		{
			copyLinkInput(linkInput, pertLinkInput, iphase, setup); // 扰动输入
			pertLinkInput->rightParameter[j] = pertLinkInput->rightParameter[j] + derStep;
			linkFunc(pertOutput, pertLinkInput);
			for ( p = rightParameterPtrCol[j]; p < rightParameterPtrCol[j+1]; p++)
			{
				i = rightParameterIdxRow[p]; // 行
				linkGrd[i + cntCol * numLinkOutput] = (pertOutput[i] - refOutput[i]) * derStepInv;
			}
			cntCol = cntCol + 1;
		}
		gradInfo[iphase] = (cmscp_derInfo*)malloc(sizeof(cmscp_derInfo));
		gradInfo[iphase]->ref = refOutput;
		gradInfo[iphase]->der = linkGrd;
		// free空间
		free(pertLinkInput->leftFinState);
		free(pertLinkInput->leftParameter);
		free(pertLinkInput->rightIniState);
		free(pertLinkInput->rightParameter);
		free(pertOutput);
	}
	free(pertLinkInput);
	free(linkInput);
}