#include "cmscp_sparseInfo.h"
/* 定义函数 */
void intRcsToCcs(int* ptrRow, int* idxCol, int* valueRcs, int numRow, int numCol, int* idxRow, int* ptrCol, int* valueCcs)
{
	// 函数功能：整数型RCS（行压缩）格式转CCS（列压缩）
	// 输入：RCS的稀疏信息
	// ptrRow：压缩的行坐标，各行首元素所在valueRcs的位置
	// idxCol：列坐标
	// valueRcs：非零元素值
	// 输出：
	// 输出：CCS的稀疏信息
	// ptrCol：压缩的列坐标，各列首元素所在valueCcs的位置
	// idxRow：行坐标
	// valueCcs：非零元素值
	// 谢磊：2022 / 6 / 25编写

	// 声明变量
	int numNz, * w, cntNz, i, j, k, q;

	// 分配内存
	w = (int*)malloc(numCol * sizeof(int)); // 临时变量

	// 计算CCS格式的ptrCol
	// 计算每列非零元个数
	numNz = ptrRow[numRow]; // 非零元素个数
	intVecFillin(w, numCol, 0); // 给w赋0
	if (numNz)
	{
		for (cntNz = 0; cntNz < numNz; cntNz++)
		{
			j = idxCol[cntNz];
			w[j]++;// 用于统计每列的非零元个数
		}
		// 将每列非零元个数累加
		ptrCol[0] = 0;
		for (j = 0; j < numCol; j++)
		{
			ptrCol[j + 1] = ptrCol[j] + w[j];
		}
		// 计算CCS格式的idxRow，valueCcs
		copyIntVec(ptrCol, w, numCol, 0); // 当前元素所在列的起始点位置
		if (valueRcs == NULL) // 只转化坐标，不对valueRcs操作
		{
			for (i = 0; i < numRow; i++)
			{
				for (k = ptrRow[i]; k < ptrRow[i + 1]; k++)
				{
					j = idxCol[k];
					q = w[j]++;
					idxRow[q] = i;
				}
			}
		}
		else
		{
			for (i = 0; i < numRow; i++)
			{
				for (k = ptrRow[i]; k < ptrRow[i + 1]; k++)
				{
					j = idxCol[k];
					q = w[j]++;
					idxRow[q] = i;
					valueCcs[q] = valueRcs[k];
				}
			}
		}
		// 释放内存
		free(w);
	}
}
void floatRcsToCcs(int* ptrRow, int* idxCol, double* valueRcs, int numRow, int numCol, int* idxRow, int* ptrCol, double* valueCcs)
{
	// 函数功能：浮点型RCS（行压缩）格式转CCS（列压缩）
	// 输入：RCS的稀疏信息
	// ptrRow：压缩的行坐标，各行首元素所在valueRcs的位置
	// idxCol：列坐标
	// valueRcs：非零元素值
	// 输出：
	// 输出：CCS的稀疏信息
	// ptrCol：压缩的列坐标，各列首元素所在valueCcs的位置
	// idxRow：行坐标
	// valueCcs：非零元素值
	// 谢磊：2022 / 6 / 25编写

	// 声明变量
	int numNz, * w, cntNz, i, j, k, q;

	// 分配内存
	w = (int*)malloc(numCol * sizeof(int)); // 临时变量

	// 计算CCS格式的ptrCol
	// 计算每列非零元个数
	numNz = ptrRow[numRow]; // 非零元素个数
	intVecFillin(w, numCol, 0); // 给w赋0
	if (numNz)
	{
		for (cntNz = 0; cntNz < numNz; cntNz++)
		{
			j = idxCol[cntNz];
			w[j]++;// 用于统计每列的非零元个数
		}
		// 将每列非零元个数累加
		ptrCol[0] = 0;
		for (j = 0; j < numCol; j++)
		{
			ptrCol[j + 1] = ptrCol[j] + w[j];
		}
		// 计算CCS格式的idxRow，valueCcs
		copyIntVec(ptrCol, w, numCol, 0); // 当前元素所在列的起始点位置
		if (valueRcs == NULL) // 只转化坐标，不对valueRcs操作
		{
			for (i = 0; i < numRow; i++)
			{
				for (k = ptrRow[i]; k < ptrRow[i + 1]; k++)
				{
					j = idxCol[k];
					q = w[j]++;
					idxRow[q] = i;
				}
			}
		}
		else
		{
			for (i = 0; i < numRow; i++)
			{
				for (k = ptrRow[i]; k < ptrRow[i + 1]; k++)
				{
					j = idxCol[k];
					q = w[j]++;
					idxRow[q] = i;
					valueCcs[q] = valueRcs[k];
				}
			}
		}
		free(w);
	}
}

void intDematNzPos(int* demat, int numRow, int numCol, int** ptrRow0, int** idxCol0, int** idxRow0, int** ptrCol0) {
	// 函数功能：获取稠密矩阵的非零元位置（包含分配相应变量的内存空间）
	// 输入：
	// demat：无连续函数稀疏信息的结构体
	// numRow：demat的行数
	// numCol：demat的列数
	// 输出：
	// ptrRow0, idxCol0：行压缩格式存储的行列坐标的指针变量的地址
	// idxRow0, ptrCol0：列压缩格式存储的行列坐标
	// 假设函数的自变量按照[x, u, p]排序
	// 谢磊：2022 / 6 / 25编写

	// 声明变量
	int numNz, i, j, dematij, * numNzRow, cntNz, * ptrRow, * idxCol, * idxRow, * ptrCol;
	// 统计非零元个数
	numNz = 0;
	for (i = 0; i < numRow; i++)
	{
		for (j = 0; j < numCol; j++)
		{
			if (demat[i + numRow * j]) // 判断demat的第ij个元素是否为1
			{
				numNz++;
			}
		}
	}
	// 分配内存
	if (numNz)
	{
		ptrRow = (int*)malloc((numRow + 1) * sizeof(int));// 用于存储每行中第1个非零元素在idxCol中的位置
		idxCol = (int*)malloc(numNz * sizeof(int));// 非零元素列坐标
		idxRow = (int*)malloc(numNz * sizeof(int));// 非零元素行坐标
		ptrCol = (int*)malloc((numCol + 1) * sizeof(int));// 用于存储每列中第1个非零元素在idxCol中的位置
		numNzRow = (int*)malloc(numRow * sizeof(int));// 用于统计每行的非零元个数
		cntNz = 0;
		for (i = 0; i < numRow; i++)
		{
			numNzRow[i] = 0;
			for (j = 0; j < numCol; j++)
			{
				dematij = demat[i + numRow * j];
				if (dematij)
				{
					numNzRow[i] = numNzRow[i] + 1;
					idxCol[cntNz++] = j;
				}
			}
		}
		ptrRow[0] = 0;
		for (i = 0; i < numRow; i++)
		{
			ptrRow[i + 1] = ptrRow[i] + numNzRow[i];
		}
		// Rcs转化为Ccs格式
		intRcsToCcs(ptrRow, idxCol, NULL, numRow, numCol, idxRow, ptrCol, NULL);
		free(numNzRow);
	}
	else
	{
		ptrRow = (int*)malloc((numRow + 1) * sizeof(int));
		intVecFillin(ptrRow, numRow + 1, 0);
		idxCol = NULL;
		idxRow = NULL;
		ptrCol = (int*)malloc((numCol + 1) * sizeof(int));
		intVecFillin(ptrCol, numCol + 1, 0);

	}
	// 输出稀疏信息
	*ptrRow0 = ptrRow;
	*idxCol0 = idxCol;
	*idxRow0 = idxRow;
	*ptrCol0 = ptrCol;
}
void mallocSparseInfo(cmscp_sparseInfo** sparseInfo0, int numPhase) {
	// 函数功能：用函数给sparseInfo分配空间
	// 输入：
	// sparseInfo的结构体地址：sparseInfo0 = &sparseInfo
	// 段数：numPhase
	// 输出：
	// 分配好的内存的sparseInfo
	// 谢磊：2022 / 6 / 25编写

	// 声明变量
	cmscp_sparseInfo* sparseInfo;
	// 1级分配
	sparseInfo = (cmscp_sparseInfo*)malloc(numPhase * sizeof(cmscp_sparseInfo));
	// 2级分配
	sparseInfo->contNzPos = (cmscp_nzPos_cont**)malloc(numPhase * sizeof(cmscp_nzPos_cont*));
	sparseInfo->objEventNzPos = (cmscp_nzPos_endp**)malloc(numPhase * sizeof(cmscp_nzPos_endp*));
	sparseInfo->endpNzPos = (cmscp_nzPos_endp**)malloc(numPhase * sizeof(cmscp_nzPos_endp*));
	sparseInfo->linkNzPos = (cmscp_nzPos_link**)malloc(numPhase * sizeof(cmscp_nzPos_link*));
	// 3级和4级分配
	int iphase;
	for (iphase = 0; iphase < numPhase; iphase++) {
		// 3级分配
		sparseInfo->contNzPos[iphase] = (cmscp_nzPos_cont*)malloc(sizeof(cmscp_nzPos_cont));

		sparseInfo->objEventNzPos[iphase] = (cmscp_nzPos_endp*)malloc(sizeof(cmscp_nzPos_endp));
		sparseInfo->endpNzPos[iphase] = (cmscp_nzPos_endp*)malloc(sizeof(cmscp_nzPos_endp));
		
		// 4级分配
		sparseInfo->contNzPos[iphase]->state = (cmscp_nzPos*)malloc(sizeof(cmscp_nzPos));
		sparseInfo->contNzPos[iphase]->state->idxCol = NULL;
		sparseInfo->contNzPos[iphase]->state->idxRow = NULL;
		sparseInfo->contNzPos[iphase]->state->ptrCol = NULL;
		sparseInfo->contNzPos[iphase]->state->ptrRow = NULL;

		sparseInfo->contNzPos[iphase]->control = (cmscp_nzPos*)malloc(sizeof(cmscp_nzPos));
		sparseInfo->contNzPos[iphase]->control->idxCol = NULL;
		sparseInfo->contNzPos[iphase]->control->idxRow = NULL;
		sparseInfo->contNzPos[iphase]->control->ptrCol = NULL;
		sparseInfo->contNzPos[iphase]->control->ptrRow = NULL;

		sparseInfo->contNzPos[iphase]->parameter = (cmscp_nzPos*)malloc(sizeof(cmscp_nzPos));
		sparseInfo->contNzPos[iphase]->parameter->idxCol = NULL;
		sparseInfo->contNzPos[iphase]->parameter->idxRow = NULL;
		sparseInfo->contNzPos[iphase]->parameter->ptrCol = NULL;
		sparseInfo->contNzPos[iphase]->parameter->ptrRow = NULL;

		sparseInfo->objEventNzPos[iphase]->iniTime = (cmscp_nzPos*)malloc(sizeof(cmscp_nzPos));
		sparseInfo->objEventNzPos[iphase]->iniTime->idxCol = NULL;
		sparseInfo->objEventNzPos[iphase]->iniTime->idxRow = NULL;
		sparseInfo->objEventNzPos[iphase]->iniTime->ptrCol = NULL;
		sparseInfo->objEventNzPos[iphase]->iniTime->ptrRow = NULL;


		sparseInfo->objEventNzPos[iphase]->finTime = (cmscp_nzPos*)malloc(sizeof(cmscp_nzPos));
		sparseInfo->objEventNzPos[iphase]->finTime->idxCol = NULL;
		sparseInfo->objEventNzPos[iphase]->finTime->idxRow = NULL;
		sparseInfo->objEventNzPos[iphase]->finTime->ptrCol = NULL;
		sparseInfo->objEventNzPos[iphase]->finTime->ptrRow = NULL;


		sparseInfo->objEventNzPos[iphase]->iniState = (cmscp_nzPos*)malloc(sizeof(cmscp_nzPos));
		sparseInfo->objEventNzPos[iphase]->iniState->idxCol = NULL;
		sparseInfo->objEventNzPos[iphase]->iniState->idxRow = NULL;
		sparseInfo->objEventNzPos[iphase]->iniState->ptrCol = NULL;
		sparseInfo->objEventNzPos[iphase]->iniState->ptrRow = NULL;

		sparseInfo->objEventNzPos[iphase]->finState = (cmscp_nzPos*)malloc(sizeof(cmscp_nzPos));
		sparseInfo->objEventNzPos[iphase]->finState->idxCol = NULL;
		sparseInfo->objEventNzPos[iphase]->finState->idxRow = NULL;
		sparseInfo->objEventNzPos[iphase]->finState->ptrCol = NULL;
		sparseInfo->objEventNzPos[iphase]->finState->ptrRow = NULL;

		sparseInfo->objEventNzPos[iphase]->parameter = (cmscp_nzPos*)malloc(sizeof(cmscp_nzPos));
		sparseInfo->objEventNzPos[iphase]->parameter->idxCol = NULL;
		sparseInfo->objEventNzPos[iphase]->parameter->idxRow = NULL;
		sparseInfo->objEventNzPos[iphase]->parameter->ptrCol = NULL;
		sparseInfo->objEventNzPos[iphase]->parameter->ptrRow = NULL;

		sparseInfo->endpNzPos[iphase]->iniTime = (cmscp_nzPos*)malloc(sizeof(cmscp_nzPos));
		sparseInfo->endpNzPos[iphase]->iniTime->idxCol = NULL;
		sparseInfo->endpNzPos[iphase]->iniTime->idxRow = NULL;
		sparseInfo->endpNzPos[iphase]->iniTime->ptrCol = NULL;
		sparseInfo->endpNzPos[iphase]->iniTime->ptrRow = NULL;

		sparseInfo->endpNzPos[iphase]->finTime = (cmscp_nzPos*)malloc(sizeof(cmscp_nzPos));
		sparseInfo->endpNzPos[iphase]->finTime->idxCol = NULL;
		sparseInfo->endpNzPos[iphase]->finTime->idxRow = NULL;
		sparseInfo->endpNzPos[iphase]->finTime->ptrCol = NULL;
		sparseInfo->endpNzPos[iphase]->finTime->ptrRow = NULL;

		sparseInfo->endpNzPos[iphase]->iniState = (cmscp_nzPos*)malloc(sizeof(cmscp_nzPos));
		sparseInfo->endpNzPos[iphase]->iniState->idxCol = NULL;
		sparseInfo->endpNzPos[iphase]->iniState->idxRow = NULL;
		sparseInfo->endpNzPos[iphase]->iniState->ptrCol = NULL;
		sparseInfo->endpNzPos[iphase]->iniState->ptrRow = NULL;

		sparseInfo->endpNzPos[iphase]->finState = (cmscp_nzPos*)malloc(sizeof(cmscp_nzPos));
		sparseInfo->endpNzPos[iphase]->finState->idxCol = NULL;
		sparseInfo->endpNzPos[iphase]->finState->idxRow = NULL;
		sparseInfo->endpNzPos[iphase]->finState->ptrCol = NULL;
		sparseInfo->endpNzPos[iphase]->finState->ptrRow = NULL;

		sparseInfo->endpNzPos[iphase]->parameter = (cmscp_nzPos*)malloc(sizeof(cmscp_nzPos));
		sparseInfo->endpNzPos[iphase]->parameter->idxCol = NULL;
		sparseInfo->endpNzPos[iphase]->parameter->idxRow = NULL;
		sparseInfo->endpNzPos[iphase]->parameter->ptrCol = NULL;
		sparseInfo->endpNzPos[iphase]->parameter->ptrRow = NULL;


		if (iphase < numPhase - 1) {
			// 3级分配
			sparseInfo->linkNzPos[iphase] = (cmscp_nzPos_link*)malloc(sizeof(cmscp_nzPos_link));
			// 4级分配
			sparseInfo->linkNzPos[iphase]->leftTf = (cmscp_nzPos*)malloc(sizeof(cmscp_nzPos));
			sparseInfo->linkNzPos[iphase]->leftTf->idxCol = NULL;
			sparseInfo->linkNzPos[iphase]->leftTf->idxRow = NULL;
			sparseInfo->linkNzPos[iphase]->leftTf->ptrCol = NULL;
			sparseInfo->linkNzPos[iphase]->leftTf->ptrRow = NULL;

			sparseInfo->linkNzPos[iphase]->leftStatef = (cmscp_nzPos*)malloc(sizeof(cmscp_nzPos));
			sparseInfo->linkNzPos[iphase]->leftStatef->idxCol = NULL;
			sparseInfo->linkNzPos[iphase]->leftStatef->idxRow = NULL;
			sparseInfo->linkNzPos[iphase]->leftStatef->ptrCol = NULL;
			sparseInfo->linkNzPos[iphase]->leftStatef->ptrRow = NULL;

			sparseInfo->linkNzPos[iphase]->leftParameter = (cmscp_nzPos*)malloc(sizeof(cmscp_nzPos));
			sparseInfo->linkNzPos[iphase]->leftParameter->idxCol = NULL;
			sparseInfo->linkNzPos[iphase]->leftParameter->idxRow = NULL;
			sparseInfo->linkNzPos[iphase]->leftParameter->ptrCol = NULL;
			sparseInfo->linkNzPos[iphase]->leftParameter->ptrRow = NULL;

			sparseInfo->linkNzPos[iphase]->rightT0 = (cmscp_nzPos*)malloc(sizeof(cmscp_nzPos));
			sparseInfo->linkNzPos[iphase]->rightT0->idxCol = NULL;
			sparseInfo->linkNzPos[iphase]->rightT0->idxRow = NULL;
			sparseInfo->linkNzPos[iphase]->rightT0->ptrCol = NULL;
			sparseInfo->linkNzPos[iphase]->rightT0->ptrRow = NULL;

			sparseInfo->linkNzPos[iphase]->rightState0 = (cmscp_nzPos*)malloc(sizeof(cmscp_nzPos));
			sparseInfo->linkNzPos[iphase]->rightState0->idxCol = NULL;
			sparseInfo->linkNzPos[iphase]->rightState0->idxRow = NULL;
			sparseInfo->linkNzPos[iphase]->rightState0->ptrCol = NULL;
			sparseInfo->linkNzPos[iphase]->rightState0->ptrRow = NULL;

			sparseInfo->linkNzPos[iphase]->rightParameter = (cmscp_nzPos*)malloc(sizeof(cmscp_nzPos));
			sparseInfo->linkNzPos[iphase]->rightParameter->idxCol = NULL;
			sparseInfo->linkNzPos[iphase]->rightParameter->idxRow = NULL;
			sparseInfo->linkNzPos[iphase]->rightParameter->ptrCol = NULL;
			sparseInfo->linkNzPos[iphase]->rightParameter->ptrRow = NULL;
		}
	}
	// 输出sparseInfo
	*sparseInfo0 = sparseInfo;
}

void sparseInfoObjEvent(cmscp_sparseInfo** sparseInfo0, cmscp_trajInfo** refTraj, cmscp_setup* setup)
{
	// 函数功能：获取目标函数和事件函数稀疏信息
	// 输入：
	// sparseInfo0：无目标函数和事件约束稀疏信息的结构体的地址
	// refTraj：参考轨迹
	// setup：问题设置参数
	// 输出：
	// sparseInfo：存储有目标函数和事件约束稀疏信息的结构体
	// 假设目标函数和事件约束函数的自变量按照
	// [{x0, xf, t0, tf, p}_1, ..., { x0,xf,t0,tf,p }_iphase, ..., { x0,xf,t0,tf,p }_numPhase]排序
	// 谢磊：2022 / 4 / 28编写

	// 变量声明
	cmscp_dimension* dimension;
	int numPhase, numObjEventOutput, cntCol, iphase, numState, numParameter, * objEventOutputv,
		* ptrRow, * idxCol, * idxRow, * ptrCol;
	cmscp_nzPos_endp** nzPosInfo;
	intDemat* objEventDependMatrix;

	// 分配空间
	objEventDependMatrix = (intDemat*)malloc(sizeof(intDemat));

	// 1-> 基本参数
	dimension = setup->dimension;// 问题维数
	numPhase = setup->numPhase;// 阶段数
	nzPosInfo = (*sparseInfo0)->objEventNzPos;// 存放稀疏信息的结构体

	// 2-> 获取雅可比矩阵相关性矩阵
	if (strcmp(setup->sparseModel, "autoSparse") == 0)
	{
	}
	else
	{
		setup->functions->objEventDepend(objEventDependMatrix);
	}
	numObjEventOutput = objEventDependMatrix->m;

	// 3-> 以列和行压缩格式记录各个分块非零元位置
	// 统计非零元个数，用于分配内存
	cntCol = 0;// objEventDependMatrix矩阵中列位置计数变量
	for (iphase = 0; iphase < numPhase; iphase++)
	{
		numState = dimension->phase[iphase]->state;
		numParameter = dimension->phase[iphase]->parameter;

		// 初始状态部分
		// 稀疏矩阵
		objEventOutputv = objEventDependMatrix->v + numObjEventOutput * cntCol;
		// 获取非零元素位置
		intDematNzPos(objEventOutputv, numObjEventOutput, numState, &ptrRow, &idxCol, &idxRow, &ptrCol);
		// 保存矩阵稀疏信息
		nzPosInfo[iphase]->iniState->numRow = numObjEventOutput;
		nzPosInfo[iphase]->iniState->numCol = numState;
		nzPosInfo[iphase]->iniState->idxRow = idxRow;
		nzPosInfo[iphase]->iniState->ptrCol = ptrCol;
		nzPosInfo[iphase]->iniState->ptrRow = ptrRow;
		nzPosInfo[iphase]->iniState->idxCol = idxCol;
		// 更新objEventDependMatrix矩阵列计数变量
		cntCol = cntCol + numState;

		// 终端状态部分
		// 稀疏矩阵
		objEventOutputv = objEventDependMatrix->v + numObjEventOutput * cntCol;
		// 获取非零元素位置
		intDematNzPos(objEventOutputv, numObjEventOutput, numState, &ptrRow, &idxCol, &idxRow, &ptrCol);
		// 保存矩阵稀疏信息
		nzPosInfo[iphase]->finState->numRow = numObjEventOutput;
		nzPosInfo[iphase]->finState->numCol = numState;
		nzPosInfo[iphase]->finState->idxRow = idxRow;
		nzPosInfo[iphase]->finState->ptrCol = ptrCol;
		nzPosInfo[iphase]->finState->ptrRow = ptrRow;
		nzPosInfo[iphase]->finState->idxCol = idxCol;
		// 更新objEventDependMatrix矩阵列计数变量
		cntCol = cntCol + numState;

		// 初始时刻部分
		// 稠密矩阵
		objEventOutputv = objEventDependMatrix->v + numObjEventOutput * cntCol;
		// 获取非零元素位置
		intDematNzPos(objEventOutputv, numObjEventOutput, 1, &ptrRow, &idxCol, &idxRow, &ptrCol);
		// 保存矩阵稀疏信息
		nzPosInfo[iphase]->iniTime->numRow = numObjEventOutput;
		nzPosInfo[iphase]->iniTime->numCol = 1;
		nzPosInfo[iphase]->iniTime->idxRow = idxRow;
		nzPosInfo[iphase]->iniTime->ptrCol = ptrCol;
		nzPosInfo[iphase]->iniTime->ptrRow = ptrRow;
		nzPosInfo[iphase]->iniTime->idxCol = idxCol;
		// 更新objEventDependMatrix矩阵列计数变量
		cntCol = cntCol + 1;

		// 终端时刻部分
		// 稠密矩阵
		objEventOutputv = objEventDependMatrix->v + numObjEventOutput * cntCol;
		// 获取非零元素位置
		intDematNzPos(objEventOutputv, numObjEventOutput, 1, &ptrRow, &idxCol, &idxRow, &ptrCol);
		// 保存矩阵稀疏信息
		nzPosInfo[iphase]->finTime->numRow = numObjEventOutput;
		nzPosInfo[iphase]->finTime->numCol = 1;
		nzPosInfo[iphase]->finTime->idxRow = idxRow;
		nzPosInfo[iphase]->finTime->ptrCol = ptrCol;
		nzPosInfo[iphase]->finTime->ptrRow = ptrRow;
		nzPosInfo[iphase]->finTime->idxCol = idxCol;
		// 更新objEventDependMatrix矩阵列计数变量
		cntCol = cntCol + 1;

		// 静态参数变量部分
		// 稠密矩阵
		objEventOutputv = objEventDependMatrix->v + numObjEventOutput * cntCol;
		// 获取非零元素位置
		intDematNzPos(objEventOutputv, numObjEventOutput, numParameter, &ptrRow, &idxCol, &idxRow, &ptrCol);
		// 保存矩阵稀疏信息
		nzPosInfo[iphase]->parameter->numRow = numObjEventOutput;
		nzPosInfo[iphase]->parameter->numCol = numParameter;
		nzPosInfo[iphase]->parameter->idxRow = idxRow;
		nzPosInfo[iphase]->parameter->ptrCol = ptrCol;
		nzPosInfo[iphase]->parameter->ptrRow = ptrRow;
		nzPosInfo[iphase]->parameter->idxCol = idxCol;
		// 更新objEventDependMatrix矩阵列计数变量
		cntCol = cntCol + numParameter;
	}
	// 释放空间
	free(objEventDependMatrix->v);
	free(objEventDependMatrix);
}


void sparseInfoCont(cmscp_sparseInfo** sparseInfo0, cmscp_trajInfo** refTraj, cmscp_setup* setup)
{
	// 函数功能：获取连续函数列和行压缩格式稀疏信息
	// 输入：
	// sparseInfo0：无连续函数稀疏信息的结构体
	// refTraj：参考轨迹
	// setup：问题设置参数
	// 输出：
	// sparseInfo：存储有连续函数稀疏信息的结构体
	// 假设函数的自变量按照 [x,u,p]排序
	// 谢磊：2022 / 6 / 25编写

	// 变量声明
	cmscp_dimension* dimension;
	int numPhase, numContOutput, cntCol, iphase, numState, numParameter, * contOutputv,
		* ptrRow, * idxCol, * idxRow, * ptrCol, numControl;
	cmscp_nzPos_cont** nzPosInfo;
	intDemat* contDependMatrix;

	// 分配空间
	contDependMatrix = (intDemat*)malloc(sizeof(intDemat));

	// 1-> 基本参数
	dimension = setup->dimension;// 问题维数
	numPhase = setup->numPhase;// 阶段数
	nzPosInfo = (*sparseInfo0)->contNzPos;// 存放稀疏信息的结构体

	// 2-> 以列压缩格式记录各段中各个分块非零元位置 
	for ( iphase = 0; iphase < numPhase; iphase++)
	{
		// % 2.1. 维数信息
		numState = dimension->phase[iphase]->state;
		numControl = dimension->phase[iphase]->control;
		numParameter = dimension->phase[iphase]->parameter;

		// 2.2.获取雅可比矩阵相关性矩阵
		if (strcmp(setup->sparseModel, "autoSparse") == 0)
		{
		}
		else
		{
			setup->functions->contDepend(contDependMatrix, iphase, setup);
		}
		numContOutput = contDependMatrix->m;
		// 2.3. 以列压缩格式记录各个分块非零元位置 
		// 2.3.1. 状态部分 （dCont/dx）包括动力学和路径约束
		cntCol = 0; // contOutput列计数变量
		// 稠密矩阵
		contOutputv = contDependMatrix->v + numContOutput * cntCol;
		// 获取非零元素位置
		intDematNzPos(contOutputv, numContOutput, numState, &ptrRow, &idxCol, &idxRow, &ptrCol);
		// 保存矩阵稀疏信息
		nzPosInfo[iphase]->state->numRow = numContOutput;
		nzPosInfo[iphase]->state->numCol = numState;
		nzPosInfo[iphase]->state->idxRow = idxRow;
		nzPosInfo[iphase]->state->ptrCol = ptrCol;
		nzPosInfo[iphase]->state->ptrRow = ptrRow;
		nzPosInfo[iphase]->state->idxCol = idxCol;

		// 2.3.2.控制部分 （dCont / du）包括动力学和路径约束
		cntCol = cntCol + numState;  // contOutput列计数变量
		// 稠密矩阵
		contOutputv = contDependMatrix->v + numContOutput * cntCol;
		// 获取非零元素位置
		intDematNzPos(contOutputv, numContOutput, numControl, &ptrRow, &idxCol, &idxRow, &ptrCol);
		// 保存矩阵稀疏信息
		nzPosInfo[iphase]->control->numRow = numContOutput;
		nzPosInfo[iphase]->control->numCol = numControl;
		nzPosInfo[iphase]->control->idxRow = idxRow;
		nzPosInfo[iphase]->control->ptrCol = ptrCol;
		nzPosInfo[iphase]->control->ptrRow = ptrRow;
		nzPosInfo[iphase]->control->idxCol = idxCol;

		// 2.3.2.控制部分 （dCont / du）包括动力学和路径约束
		cntCol = cntCol + numControl;  // contOutput列计数变量
		// 稠密矩阵
		contOutputv = contDependMatrix->v + numContOutput * cntCol;
		// 获取非零元素位置
		intDematNzPos(contOutputv, numContOutput, numParameter, &ptrRow, &idxCol, &idxRow, &ptrCol);
		// 保存矩阵稀疏信息
		nzPosInfo[iphase]->parameter->numRow = numContOutput;
		nzPosInfo[iphase]->parameter->numCol = numParameter;
		nzPosInfo[iphase]->parameter->idxRow = idxRow;
		nzPosInfo[iphase]->parameter->ptrCol = ptrCol;
		nzPosInfo[iphase]->parameter->ptrRow = ptrRow;
		nzPosInfo[iphase]->parameter->idxCol = idxCol;

		// 释放contDependMatrix底层空间
		free(contDependMatrix->v);
	}
	// 释放contDependMatrix空间
	free(contDependMatrix);
}

void sparseInfoEndp(cmscp_sparseInfo** sparseInfo0, cmscp_trajInfo** refTraj, cmscp_setup* setup)
{
	// 函数功能：获取端点函数列和行压缩格式稀疏信息
	// 输入：
	// sparseInfo0：无端点函数稀疏信息的结构体指针的地址
	// refTraj：参考轨迹
	// setup：问题设置参数
	// 输出：
	// sparseInfo：端点函数有连续函数稀疏信息的结构体
	// 假设函数的自变量按照[x0,xf, t0, tf, 排序
	// 谢磊：2022 / 6 / 25编写

	// 变量声明
	cmscp_dimension* dimension;
	int numPhase, numEndpOutput, cntCol, iphase, numState, numParameter, * endpOutputv,
		* ptrRow, * idxCol, * idxRow, * ptrCol;
	cmscp_nzPos_endp** nzPosInfo;
	intDemat* endpDependMatrix;

	// 分配空间
	endpDependMatrix = (intDemat*)malloc(sizeof(intDemat));

	// 1-> 基本参数
	dimension = setup->dimension;// 问题维数
	numPhase = setup->numPhase;// 阶段数
	nzPosInfo = (*sparseInfo0)->endpNzPos;// 存放稀疏信息的结构体

	// 2-> 以列压缩格式记录各段中各个分块非零元位置 
	for (iphase = 0; iphase < numPhase; iphase++)
	{
		// % 2.1. 维数信息
		numState = dimension->phase[iphase]->state;
		numParameter = dimension->phase[iphase]->parameter;

		// 2.2.获取雅可比矩阵相关性矩阵
		if (strcmp(setup->sparseModel, "autoSparse") == 0)
		{
		}
		else
		{
			setup->functions->endpDepend(endpDependMatrix, iphase, setup);
		}
		numEndpOutput = endpDependMatrix->m;
		// 2.3. 以列压缩格式记录各个分块非零元位置 
		// 2.3.1. 初始状态部分 （dEndp/dx0）
		cntCol = 0; // endpOutput列计数变量
		// 稠密矩阵
		endpOutputv = endpDependMatrix->v + numEndpOutput * cntCol;
		// 获取非零元素位置
		intDematNzPos(endpOutputv, numEndpOutput, numState, &ptrRow, &idxCol, &idxRow, &ptrCol);
		// 保存矩阵稀疏信息
		nzPosInfo[iphase]->iniState->numRow = numEndpOutput;
		nzPosInfo[iphase]->iniState->numCol = numState;
		nzPosInfo[iphase]->iniState->idxRow = idxRow;
		nzPosInfo[iphase]->iniState->ptrCol = ptrCol;
		nzPosInfo[iphase]->iniState->ptrRow = ptrRow;
		nzPosInfo[iphase]->iniState->idxCol = idxCol;

		// 2.3.2.终端状态部分 （dEndp/dxf）
		cntCol = cntCol + numState;  // endpOutput列计数变量
		// 稠密矩阵
		endpOutputv = endpDependMatrix->v + numEndpOutput * cntCol;
		// 获取非零元素位置
		intDematNzPos(endpOutputv, numEndpOutput, numState, &ptrRow, &idxCol, &idxRow, &ptrCol);
		// 保存矩阵稀疏信息
		nzPosInfo[iphase]->finState->numRow = numEndpOutput;
		nzPosInfo[iphase]->finState->numCol = numState;
		nzPosInfo[iphase]->finState->idxRow = idxRow;
		nzPosInfo[iphase]->finState->ptrCol = ptrCol;
		nzPosInfo[iphase]->finState->ptrRow = ptrRow;
		nzPosInfo[iphase]->finState->idxCol = idxCol;

		// 2.3.3.初始时刻部分 （dEndp/dt0）
		cntCol = cntCol + numState;  // endpOutput列计数变量
		// 稠密矩阵
		endpOutputv = endpDependMatrix->v + numEndpOutput * cntCol;
		// 获取非零元素位置
		intDematNzPos(endpOutputv, numEndpOutput, 1, &ptrRow, &idxCol, &idxRow, &ptrCol);
		// 保存矩阵稀疏信息
		nzPosInfo[iphase]->iniTime->numRow = numEndpOutput;
		nzPosInfo[iphase]->iniTime->numCol = 1;
		nzPosInfo[iphase]->iniTime->idxRow = idxRow;
		nzPosInfo[iphase]->iniTime->ptrCol = ptrCol;
		nzPosInfo[iphase]->iniTime->ptrRow = ptrRow;
		nzPosInfo[iphase]->iniTime->idxCol = idxCol;

		// 2.3.4.终端时刻部分 （dEndp/dtf）
		cntCol = cntCol + 1;  // endpOutput列计数变量
		// 稠密矩阵
		endpOutputv = endpDependMatrix->v + numEndpOutput * cntCol;
		// 获取非零元素位置
		intDematNzPos(endpOutputv, numEndpOutput, 1, &ptrRow, &idxCol, &idxRow, &ptrCol);
		// 保存矩阵稀疏信息
		nzPosInfo[iphase]->finTime->numRow = numEndpOutput;
		nzPosInfo[iphase]->finTime->numCol = 1;
		nzPosInfo[iphase]->finTime->idxRow = idxRow;
		nzPosInfo[iphase]->finTime->ptrCol = ptrCol;
		nzPosInfo[iphase]->finTime->ptrRow = ptrRow;
		nzPosInfo[iphase]->finTime->idxCol = idxCol;

		// 2.3.5.静态参数部分 （dEndp/dp）
		cntCol = cntCol + 1;  // endpOutput列计数变量
		// 稠密矩阵
		endpOutputv = endpDependMatrix->v + numEndpOutput * cntCol;
		// 获取非零元素位置
		intDematNzPos(endpOutputv, numEndpOutput, numParameter, &ptrRow, &idxCol, &idxRow, &ptrCol);
		// 保存矩阵稀疏信息
		nzPosInfo[iphase]->parameter->numRow = numEndpOutput;
		nzPosInfo[iphase]->parameter->numCol = numParameter;
		nzPosInfo[iphase]->parameter->idxRow = idxRow;
		nzPosInfo[iphase]->parameter->ptrCol = ptrCol;
		nzPosInfo[iphase]->parameter->ptrRow = ptrRow;
		nzPosInfo[iphase]->parameter->idxCol = idxCol;

		// 释放endpDependMatrix底层空间
		free(endpDependMatrix->v);
	}
	// 释放endpDependMatrix空间
	free(endpDependMatrix);
}


void sparseInfoLink(cmscp_sparseInfo** sparseInfo0, cmscp_trajInfo** refTraj, cmscp_setup* setup)
{
	// 函数功能：获取衔接函数列和行压缩格式稀疏信息
	// 输入：
	// sparseInfo0：无衔接函数稀疏信息的结构体指针的地址
	// refTraj：参考轨迹
	// setup：问题设置参数
	// 输出：
	// sparseInfo：衔接函数有连续函数稀疏信息的结构体
	// 假设函数的自变量按照[x0,xf, t0, tf, 排序
	// 谢磊：2022 / 6 / 25编写

	// 变量声明
	cmscp_dimension* dimension;
	int numPhase, numLinkOutput, cntCol, iphase, * linkOutputv,
		* ptrRow, * idxCol, * idxRow, * ptrCol, numLeftState, numRightState, numLeftParameter, numRightParameter;
	cmscp_nzPos_link** nzPosInfo;
	intDemat* linkDependMatrix;

	// 分配空间
	linkDependMatrix = (intDemat*)malloc(sizeof(intDemat));

	// 1-> 基本参数
	dimension = setup->dimension;// 问题维数
	numPhase = setup->numPhase;// 阶段数
	nzPosInfo = (*sparseInfo0)->linkNzPos;// 存放稀疏信息的结构体

	// 2-> 以列压缩格式记录各段中各个分块非零元位置 
	for (iphase = 0; iphase < numPhase-1; iphase++)
	{
		// % 2.1. 维数信息
		numLeftState = dimension->phase[iphase]->state;
		numRightState = dimension->phase[iphase+1]->state;
		numLeftParameter = dimension->phase[iphase]->parameter;
		numRightParameter = dimension->phase[iphase+1]->parameter;

		void* linkDepend1;
		// 2.2.获取雅可比矩阵相关性矩阵
		if (strcmp(setup->sparseModel, "autoSparse") == 0)
		{
		}
		else
		{
			linkDepend1 = sparseInfoEndp;
			//linkDepend1(linkDependMatrix, iphase, setup);
			setup->functions->linkDepend(linkDependMatrix, iphase, setup);
		}
		numLinkOutput = linkDependMatrix->m;
		// 2.3. 以列压缩格式记录各个分块非零元位置 
		// 2.3.1. 左侧状态部分 （dLink/dLeftStatef）
		cntCol = 0; // linkOutput列计数变量
		// 稠密矩阵
		linkOutputv = linkDependMatrix->v + numLinkOutput * cntCol;
		// 获取非零元素位置
		intDematNzPos(linkOutputv, numLinkOutput, numLeftState, &ptrRow, &idxCol, &idxRow, &ptrCol);
		// 保存矩阵稀疏信息
		nzPosInfo[iphase]->leftStatef->numRow = numLinkOutput;
		nzPosInfo[iphase]->leftStatef->numCol = numLeftState;
		nzPosInfo[iphase]->leftStatef->idxRow = idxRow;
		nzPosInfo[iphase]->leftStatef->ptrCol = ptrCol;
		nzPosInfo[iphase]->leftStatef->ptrRow = ptrRow;
		nzPosInfo[iphase]->leftStatef->idxCol = idxCol;

		// 2.3.2. 左侧时间部分 （dLink/dLeftTf）
		cntCol = cntCol + numLeftState;  // linkOutput列计数变量
		// 稠密矩阵
		linkOutputv = linkDependMatrix->v + numLinkOutput * cntCol;
		// 获取非零元素位置
		intDematNzPos(linkOutputv, numLinkOutput, 1, &ptrRow, &idxCol, &idxRow, &ptrCol);
		// 保存矩阵稀疏信息
		nzPosInfo[iphase]->leftTf->numRow = numLinkOutput;
		nzPosInfo[iphase]->leftTf->numCol = 1;
		nzPosInfo[iphase]->leftTf->idxRow = idxRow;
		nzPosInfo[iphase]->leftTf->ptrCol = ptrCol;
		nzPosInfo[iphase]->leftTf->ptrRow = ptrRow;
		nzPosInfo[iphase]->leftTf->idxCol = idxCol;

		// 2.3.3. 左侧静态参数部分 （dLink/dLeftParameter）
		cntCol = cntCol + 1;  // linkOutput列计数变量
		// 稠密矩阵
		linkOutputv = linkDependMatrix->v + numLinkOutput * cntCol;
		// 获取非零元素位置
		intDematNzPos(linkOutputv, numLinkOutput, numLeftParameter, &ptrRow, &idxCol, &idxRow, &ptrCol);
		// 保存矩阵稀疏信息
		nzPosInfo[iphase]->leftParameter->numRow = numLinkOutput;
		nzPosInfo[iphase]->leftParameter->numCol = numLeftParameter;
		nzPosInfo[iphase]->leftParameter->idxRow = idxRow;
		nzPosInfo[iphase]->leftParameter->ptrCol = ptrCol;
		nzPosInfo[iphase]->leftParameter->ptrRow = ptrRow;
		nzPosInfo[iphase]->leftParameter->idxCol = idxCol;

		// 2.3.4. 右侧状态部分 （dLink/drightState0）
		cntCol = cntCol + numLeftParameter;  // linkOutput列计数变量
		// 稠密矩阵
		linkOutputv = linkDependMatrix->v + numLinkOutput * cntCol;
		// 获取非零元素位置
		intDematNzPos(linkOutputv, numLinkOutput, numRightState, &ptrRow, &idxCol, &idxRow, &ptrCol);
		// 保存矩阵稀疏信息
		nzPosInfo[iphase]->rightState0->numRow = numLinkOutput;
		nzPosInfo[iphase]->rightState0->numCol = numRightState;
		nzPosInfo[iphase]->rightState0->idxRow = idxRow;
		nzPosInfo[iphase]->rightState0->ptrCol = ptrCol;
		nzPosInfo[iphase]->rightState0->ptrRow = ptrRow;
		nzPosInfo[iphase]->rightState0->idxCol = idxCol;

		// 2.3.5. 右侧时间部分 （dLink/drightT0）
		cntCol = cntCol + numRightState;  // linkOutput列计数变量
		// 稠密矩阵
		linkOutputv = linkDependMatrix->v + numLinkOutput * cntCol;
		// 获取非零元素位置
		intDematNzPos(linkOutputv, numLinkOutput, 1, &ptrRow, &idxCol, &idxRow, &ptrCol);
		// 保存矩阵稀疏信息
		nzPosInfo[iphase]->rightT0->numRow = numLinkOutput;
		nzPosInfo[iphase]->rightT0->numCol = 1;
		nzPosInfo[iphase]->rightT0->idxRow = idxRow;
		nzPosInfo[iphase]->rightT0->ptrCol = ptrCol;
		nzPosInfo[iphase]->rightT0->ptrRow = ptrRow;
		nzPosInfo[iphase]->rightT0->idxCol = idxCol;

		// 2.3.5. 右侧静态参数部分 （dLink/dRightParameter）
		cntCol = cntCol + 1;  // linkOutput列计数变量
		// 稠密矩阵
		linkOutputv = linkDependMatrix->v + numLinkOutput * cntCol;
		// 获取非零元素位置
		intDematNzPos(linkOutputv, numLinkOutput, numRightParameter, &ptrRow, &idxCol, &idxRow, &ptrCol);
		// 保存矩阵稀疏信息
		nzPosInfo[iphase]->rightParameter->numRow = numLinkOutput;
		nzPosInfo[iphase]->rightParameter->numCol = numRightParameter;
		nzPosInfo[iphase]->rightParameter->idxRow = idxRow;
		nzPosInfo[iphase]->rightParameter->ptrCol = ptrCol;
		nzPosInfo[iphase]->rightParameter->ptrRow = ptrRow;
		nzPosInfo[iphase]->rightParameter->idxCol = idxCol;

		// 释放linkDependMatrix底层空间
		free(linkDependMatrix->v);
	}
	// 释放linkDependMatrix空间
	free(linkDependMatrix);
}

void getSparseInfo(cmscp_sparseInfo** sparseInfo0, cmscp_trajInfo** refTraj, cmscp_setup* setup) {
	// 函数功能：获取目标函数和事件函数稀疏信息
	// 输入：
	// sparseInfo：无目标函数和事件约束稀疏信息的结构体
	// refTraj：参考轨迹
	// setup：问题设置参数
	// 输出：
	// sparseInfo：存储有目标函数和事件约束稀疏信息的结构体
	// 假设目标函数和事件约束函数的自变量按照
	// [{x0, xf, t0, tf, p}_1, ..., { x0,xf,t0,tf,p }_iphase, ..., { x0,xf,t0,tf,p }_numPhase]排序
	// 谢磊：2022 / 4 / 28编写

	// 变量声明
	int numPhase;
	numPhase = setup->numPhase;

	/* 分配稀疏信息结构体变量 */
	mallocSparseInfo(sparseInfo0, numPhase);

	/* 目标函数和事件约束函数的稀疏信息 */
	sparseInfoObjEvent(sparseInfo0, refTraj, setup);

	/* 动力学和路径约束函数的稀疏信息 */
	sparseInfoCont(sparseInfo0, refTraj, setup);

	/* 端点函数的稀疏信息 */
	sparseInfoEndp(sparseInfo0, refTraj, setup);

	/* 衔接函数的稀疏信息 */
	sparseInfoLink(sparseInfo0, refTraj, setup);

}