#include <stdlib.h>
#include <math.h>
#include <time.h>


/* 随机数 */
double randn()
{
	static int hasSpare = 0;
	static double spare;

	if (hasSpare)
	{
		hasSpare = 0;
		return spare;
	}
	else
	{
		hasSpare = 1;

		static double u, v, s;
		do
		{
			u = (rand() / ((double)RAND_MAX)) * 2.0 - 1.0;
			v = (rand() / ((double)RAND_MAX)) * 2.0 - 1.0;
			s = u * u + v * v;
		} while (s >= 1 || s == 0);

		s = sqrt(-2.0 * log(s) / s);
		spare = v * s;
		return u * s;
	}

	//double u;
	//u = (rand() / ((double)RAND_MAX)) * 2.0 - 1.0;
	//return u;
}


/* 矩阵复制函数 */
int mtxCpy(double* pMtx1, double* pMtx2, int row, int col) {
	int i, j;

	for (i = 0; i < row; i = i + 1)
		for (j = 0; j < col; j = j + 1)
			*(pMtx1 + i * col + j) = *(pMtx2 + i * col + j);

	return 1;
}


/* 向量求模 */
double norm(double* pMtx0, int cnt) {
	int i;
	static double tmp;

	tmp = 0;
	for (i = 0; i < cnt; i = i + 1)
		tmp = tmp + (*(pMtx0 + i)) * (*(pMtx0 + i));
	tmp = sqrt(tmp);

	return tmp;
}


/* 求取叉乘矩阵[Bx] */
void CrossMatrixGet(double element1[3][3], double B[3]) {

	element1[0][0] = 0; element1[0][1] = -B[2]; element1[0][2] = B[1];
	element1[1][0] = B[2]; element1[1][1] = 0; element1[1][2] = -B[0];
	element1[2][0] = -B[1]; element1[2][1] = B[0]; element1[2][2] = 0;

	return;
}


/* 四元数左乘矩阵 */
void qleft(double Q1[4][4], double qinn[4]) {
	Q1[0][0] = qinn[0]; Q1[0][1] = -qinn[1]; Q1[0][2] = -qinn[2]; Q1[0][3] = -qinn[3];
	Q1[1][0] = qinn[1]; Q1[1][1] = qinn[0]; Q1[1][2] = -qinn[3]; Q1[1][3] = qinn[2];
	Q1[2][0] = qinn[2]; Q1[2][1] = qinn[3]; Q1[2][2] = qinn[0]; Q1[2][3] = -qinn[1];
	Q1[3][0] = qinn[3]; Q1[3][1] = -qinn[2]; Q1[3][2] = qinn[1]; Q1[3][3] = qinn[0];

	return;
}


/* 矩阵乘法函数 */
int mtxMtp(double* pMtx0, double* pMtx1, int row1, int col1, double* pMtx2, int row2, int col2) {
	int i, j, k;

	if ((row1 < 2) && (col1 < 2)) {
		for (i = 0; i < row2; i = i + 1)
			for (j = 0; j < col2; j = j + 1)
				*(pMtx0 + i * col2 + j) = (*(pMtx1))
				* (*(pMtx2 + i * col2 + j));
		return 1;
	}
	if ((row2 < 2) && (col2 < 2)) {
		for (i = 0; i < row1; i = i + 1)
			for (j = 0; j < col1; j = j + 1)
				*(pMtx0 + i * col1 + j) = (*(pMtx1 + i * col1 + j))
				* (*(pMtx2));
		return 1;
	}
	if (fabs(col1 - row2) < 1) {
		for (i = 0; i < row1; i = i + 1)
			for (j = 0; j < col2; j = j + 1) {
				*(pMtx0 + i * col2 + j) = 0;
				for (k = 0; k < row2; k = k + 1)
					*(pMtx0 + i * col2 + j) += (*(pMtx1 + i * col1 + k))
					* (*(pMtx2 + k * col2 + j));
			}
		return 1;
	}

	return 0;
}


/* 欧拉角度(312)转四元数 */
void euler2qua(double q[4], double euler_angle[3]) {

	/* 欧拉角度312转四元数模块 */
	/* 输入量 */
	/* euler_angle	312欧拉角度 */
	/* 输出量 */
	/* q			四元数 */

	double pufai = euler_angle[0] * 3.1415926535 / 180.0;
	double fai = euler_angle[1] * 3.1415926535 / 180.0;
	double theta = euler_angle[2] * 3.1415926535 / 180.0;
	//计算四元数的各个分量
	double q0 = cos(theta / 2) * cos(fai / 2) * cos(pufai / 2) - sin(theta / 2) * sin(fai / 2) * sin(pufai / 2);
	double q1 = cos(theta / 2) * sin(fai / 2) * cos(pufai / 2) - sin(theta / 2) * cos(fai / 2) * sin(pufai / 2);
	double q2 = sin(theta / 2) * cos(fai / 2) * cos(pufai / 2) + cos(theta / 2) * sin(fai / 2) * sin(pufai / 2);
	double q3 = sin(theta / 2) * sin(fai / 2) * cos(pufai / 2) + cos(theta / 2) * cos(fai / 2) * sin(pufai / 2);
	//将四元数存入数组中
	q[0] = q0;
	q[1] = q1;
	q[2] = q2;
	q[3] = q3;
	//如果第一个分量小于0，则取负
	if (q[0] < 0)
	{
		q[0] = -q[0];
		q[1] = -q[1];
		q[2] = -q[2];
		q[3] = -q[3];
	}
}


/* 矩阵行列式 */
double mtxDet(double* pMtx0, int n) {
	int r, c, m;
	int lop = 0;
	double result = 0;
	double mid = 1;

	if (n != 1) {
		lop = (n == 2) ? 1 : n;
		for (m = 0; m < lop; m++) {
			mid = 1;
			for (r = 0, c = m; r < n; r++, c++) {
				mid = mid * (*(pMtx0 + r * n + c % n));
			}
			result += mid;
		}
		for (m = 0; m < lop; m++) {
			mid = 1;
			for (r = 0, c = n - 1 - m + n; r < n; r++, c--) {
				mid = mid * (*(pMtx0 + r * n + c % n));
			}
			result -= mid;
		}
	}
	else
		result = *pMtx0;
	return result;
}


/* 矩阵逆*/
void mtxInv(double* pMtx0, double* pMtx1, int n) {
	int i, j, k, t;
	double det;
	double temp[9];              //只能计算3X3矩阵的逆矩阵，当定义*temp时分配地址出错

	det = mtxDet((double*)pMtx1, n);

	for (i = 0; i < n; i++) {
		for (j = 0; j < n; j++) {
			for (k = 0; k < n - 1; k++) {
				for (t = 0; t < n - 1; t++) {
					if (k >= i)
						if (t >= j)
							*(temp + k * (n - 1) + t) = *(pMtx1 + n * (k + 1)
								+ t + 1);
						else
							*(temp + k * (n - 1) + t) = *(pMtx1 + n * (k + 1)
								+ t);
					else if (t >= j)
						*(temp + k * (n - 1) + t) = *(pMtx1 + n * k + t + 1);
					else
						*(temp + k * (n - 1) + t) = *(pMtx1 + n * k + t);
				}
			}
			if ((i + j) % 2 == 1)
				*(pMtx0 + j * n + i) = -mtxDet((double*)temp, n - 1) / det;
			else
				*(pMtx0 + j * n + i) = mtxDet((double*)temp, n - 1) / det;
		}
	}
}


/* 矩阵转置 */
void mtxT(double* pMtx0, double* pMtx1, int row, int col) {
	int i, j;

	for (i = 0; i < row; i = i + 1)
		for (j = 0; j < col; j = j + 1)
			*(pMtx0 + i * col + j) = *(pMtx1 + j * row + i);

	return;
}


/* 矩阵相加 */
void mtxAdd(double* pMtx0, double* pMtx1, double* pMtx2, int row, int col) {
	int i, j;

	for (i = 0; i < row; i = i + 1)
		for (j = 0; j < col; j = j + 1)
			*(pMtx0 + i * col + j) = (*(pMtx1 + i * col + j))
			+ (*(pMtx2 + i * col + j));

	return;
}


/* 矩阵相减 */
void mtxSub(double* pMtx0, double* pMtx1, double* pMtx2, int row, int col) {
	int i, j;

	for (i = 0; i < row; i = i + 1)
		for (j = 0; j < col; j = j + 1)
			*(pMtx0 + i * col + j) = (*(pMtx1 + i * col + j))
			- (*(pMtx2 + i * col + j));

	return;
}


/* 四元数表示的姿态矩阵 */
void TransQ(double Q[3][3], double q[4]) {

	/* 盛一鸣，2023.08.22 */
	/* 姿态矩阵四元数表达模块 */
	/* 输入量 */
	/* q			四元数 */
	/* 输出量 */
	/* Q			姿态矩阵 */

	double q1[1] = { q[0] };
	double q2[1] = { q[1] };
	double q3[1] = { q[2] };
	double q4[1] = { q[3] };

	Q[0][0] = q1[0] * q1[0] + q2[0] * q2[0] - q3[0] * q3[0] - q4[0] * q4[0]; Q[0][1] = 2 * (q2[0] * q3[0] + q1[0] * q4[0]);								Q[0][2] = 2 * (q2[0] * q4[0] - q1[0] * q3[0]);
	Q[1][0] = 2 * (q2[0] * q3[0] - q1[0] * q4[0]);							 Q[1][1] = q1[0] * q1[0] - q2[0] * q2[0] + q3[0] * q3[0] - q4[0] * q4[0];   Q[1][2] = 2 * (q3[0] * q4[0] + q1[0] * q2[0]);
	Q[2][0] = 2 * (q2[0] * q4[0] + q1[0] * q3[0]);							 Q[2][1] = 2 * (q3[0] * q4[0] - q1[0] * q2[0]);								Q[2][2] = q1[0] * q1[0] - q2[0] * q2[0] - q3[0] * q3[0] + q4[0] * q4[0];

}


/* 根据轨道信息获得惯性系到轨道系的姿态矩阵 */
void MtxJtoOGet(double MtxJtoO[3][3], double orbInfo[6]) {
	static double XVec[3], YVec[3], ZVec[3], VVec[3];
	static double tmp, tmpM[3][3];

	tmp = -1.0 / norm(orbInfo, 3);
	mtxMtp(ZVec, orbInfo, 3, 1, &tmp, 1, 1);
	/* 获得轨道系Z在惯性系的单位矢量 */
	tmp = 1.0 / norm(&orbInfo[3], 3);
	mtxMtp(VVec, &orbInfo[3], 3, 1, &tmp, 1, 1);
	tmpM[0][0] = 0.0;
	tmpM[0][1] = -ZVec[2];
	tmpM[0][2] = ZVec[1];
	tmpM[1][0] = ZVec[2];
	tmpM[1][1] = 0.0;
	tmpM[1][2] = -ZVec[0];
	tmpM[2][0] = -ZVec[1];
	tmpM[2][1] = ZVec[0];
	tmpM[2][2] = 0.0;
	mtxMtp(YVec, (double*)tmpM, 3, 3, VVec, 3, 1);
	tmp = 1.0 / norm(YVec, 3);
	mtxMtp(YVec, YVec, 3, 1, &tmp, 1, 1);
	/* 获得轨道系Y在惯性系的单位矢量 */
	tmpM[0][0] = 0.0;
	tmpM[0][1] = -YVec[2];
	tmpM[0][2] = YVec[1];
	tmpM[1][0] = YVec[2];
	tmpM[1][1] = 0.0;
	tmpM[1][2] = -YVec[0];
	tmpM[2][0] = -YVec[1];
	tmpM[2][1] = YVec[0];
	tmpM[2][2] = 0.0;
	mtxMtp(XVec, (double*)tmpM, 3, 3, ZVec, 3, 1);
	/* 获得轨道系Z在惯性系的单位矢量 */
	mtxCpy(MtxJtoO[0], XVec, 1, 3);
	mtxCpy(MtxJtoO[1], YVec, 1, 3);
	mtxCpy(MtxJtoO[2], ZVec, 1, 3);
	/* 利用XYZ矢量构成姿态矩阵 */
	return;
}


void A2q(double q[4], double A[3][3]) {
	double c0 = 0.5 * sqrt(1 + A[0][0] + A[1][1] + A[2][2]);
	double c1 = 0.5 * sqrt(1 + A[0][0] - A[1][1] - A[2][2]);
	double c2 = 0.5 * sqrt(fabs(1 - A[0][0] + A[1][1] - A[2][2]));
	double c3 = 0.5 * sqrt(1 - A[0][0] - A[1][1] + A[2][2]);

	if (c0 == 0) {
		q[0] = c0;
		q[1] = c1;
		q[2] = c2;
		q[3] = c3;
	}
	else {
		q[0] = c0;
		if (A[1][2] - A[2][1] >= 0) {
			q[1] = c1;
		}
		else {
			q[1] = -c1;
		}
		if (A[2][0] - A[0][2] >= 0) {
			q[2] = c2;
		}
		else {
			q[2] = -c2;
		}
		if (A[0][1] - A[1][0] >= 0) {
			q[3] = c3;
		}
		else {
			q[3] = -c3;
		}
	}
}


/* 符号函数 */
int sign(double x) {
	if (x > 0)
		return 1;
	else if (x == 0)
		return 0;
	else
		return -1;
}


/* 四元数转欧拉角(312)角度制 */
void qua2euler(double euler_angle[3], double q[4], double euler_angle_last[3]) {

	/* 盛一鸣，2023.08.22 */
	/* 四元数转欧拉角模块 */
	/* 输入量 */
	/* q					四元数 */
	/* euler_angle_last		欧拉角 */
	/* 输出量 */
	/* euler_angle			欧拉角 */

	double Q[3][3];
	TransQ(Q, q);
	double angle_roll = asin(Q[1][2]) * 180.0 / 3.14159265358979323846;
	if (fabs(angle_roll) > 89.95) {
		angle_roll = sign(angle_roll) * 89.95;
		euler_angle[0] = euler_angle_last[0];
		euler_angle[1] = angle_roll;
		euler_angle[2] = euler_angle_last[2];
	}
	else {
		double angle_yaw = atan2(-Q[1][0], Q[1][1]) * 180.0 / 3.14159265358979323846;
		double angle_pitch = atan2(-Q[0][2], Q[2][2]) * 180.0 / 3.14159265358979323846;
		euler_angle[0] = angle_yaw;
		euler_angle[1] = angle_roll;
		euler_angle[2] = angle_pitch;
	}
}


/* 矢量叉乘法 */
void VecCross(double* pMtx0, double* pMtx1, double* pMtx2) {
	double Mtx[3][3];

	Mtx[0][0] = 0;
	Mtx[0][1] = -(*(pMtx1 + 2));
	Mtx[0][2] = (*(pMtx1 + 1));
	Mtx[1][0] = (*(pMtx1 + 2));
	Mtx[1][1] = 0;
	Mtx[1][2] = -(*pMtx1);
	Mtx[2][0] = -(*(pMtx1 + 1));
	Mtx[2][1] = (*pMtx1);
	Mtx[2][2] = 0;

	mtxMtp(pMtx0, (double*)Mtx, 3, 3, pMtx2, 3, 1);

	return;
}


// ECI坐标系转到LVLH坐标系
void Fun_Mtx_ECI2LVLH(double* R, double* V, double* Mtx_ECI2LVLH) {
	double I[3], J[3], K[3], H_RV[3];
	double r = sqrt(R[0] * R[0] + R[1] * R[1] + R[2] * R[2]);
	I[0] = R[0] / r;
	I[1] = R[1] / r;
	I[2] = R[2] / r;
	H_RV[0] = R[1] * V[2] - R[2] * V[1];
	H_RV[1] = R[2] * V[0] - R[0] * V[2];
	H_RV[2] = R[0] * V[1] - R[1] * V[0];
	double h_rv = sqrt(H_RV[0] * H_RV[0] + H_RV[1] * H_RV[1] + H_RV[2] * H_RV[2]);
	K[0] = H_RV[0] / h_rv;
	K[1] = H_RV[1] / h_rv;
	K[2] = H_RV[2] / h_rv;
	J[0] = K[1] * I[2] - K[2] * I[1];
	J[1] = K[2] * I[0] - K[0] * I[2];
	J[2] = K[0] * I[1] - K[1] * I[0];
	Mtx_ECI2LVLH[0] = I[0];
	Mtx_ECI2LVLH[1] = I[1];
	Mtx_ECI2LVLH[2] = I[2];
	Mtx_ECI2LVLH[3] = J[0];
	Mtx_ECI2LVLH[4] = J[1];
	Mtx_ECI2LVLH[5] = J[2];
	Mtx_ECI2LVLH[6] = K[0];
	Mtx_ECI2LVLH[7] = K[1];
	Mtx_ECI2LVLH[8] = K[2];
}


/* ECI坐标系转LVLH坐标系 */
void FunECI2LVLH(double RLVLH[3], double RVLVLH[6], double RVChaECI[6], double RVTarECI[6])
{
	double RTarECI[3], VTarECI[3];
	double RChaECI[3], VChaECI[3];
	double MtxECI2LVLH[3][3];
	double Temp;
	double Temp1[3], Temp2[3], Temp3[3];
	double VLVLH[3];
	double one = 1;
	int ii;

	for (ii = 0; ii < 3; ii++)
	{
		RTarECI[ii] = RVTarECI[ii];
		VTarECI[ii] = RVTarECI[ii + 3];
		RChaECI[ii] = RVChaECI[ii];
		VChaECI[ii] = RVChaECI[ii + 3];
	}
	Fun_Mtx_ECI2LVLH((double*)RTarECI, (double*)VTarECI, (double*)MtxECI2LVLH);    /* 计算转化矩阵Q */
	VecCross(Temp1, RTarECI, VTarECI);                  /* H = cross(R_Tar,V_Tar) */
	Temp = norm(RTarECI, 3);                            /* r_Tar = norm(R_Tar) */
	Temp = one / (Temp * Temp);                       /* Temp = 1/ r_Tar^2 */
	mtxMtp(Temp2, Temp1, 3, 1, &Temp, 1, 1);            /* Omega = H / r_Tar^2 */
	mtxSub(Temp1, RChaECI, RTarECI, 3, 1);              /* RelR = R_Cha - R_Tar */
	VecCross(Temp3, Temp2, Temp1);                      /* Temp3 = cross(Omega,RelR) */
	mtxAdd(Temp2, Temp3, VTarECI, 3, 1);                /* Temp2 = V_Tar + cross(Omega,RelR) */
	mtxSub(Temp3, VChaECI, Temp2, 3, 1);                /* RelV = V_Cha - V_Tar - cross(Omega,RelR)*/
	mtxMtp(RLVLH, (double*)MtxECI2LVLH, 3, 3, (double*)Temp1, 3, 1);      /* RLVLH = MtxECI2LVLH * RelR */
	mtxMtp(VLVLH, (double*)MtxECI2LVLH, 3, 3, (double*)Temp3, 3, 1);      /* VLVLH = MtxECI2LVLH * RelV */
	for (ii = 0; ii < 3; ii++)
	{
		RVLVLH[ii] = RLVLH[ii];
		RVLVLH[ii + 3] = VLVLH[ii];
	}
}