#include "sensor_star.h"
#include "orbit_info.h"
#include "GNCFunction.h"

#include "libconvert.h"
#include "liblog.h"

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

double Star1QuatInstall[4] = { 0.454717549603582 ,0.541041969225232 ,0.454793640506779 ,-0.541911692228529 };
double Star2QuatInstall[4] = { 0.454717962112787 ,-0.541835255189012 ,0.453848239109489 ,0.541911346092208 };

void GetStar1Meas(double MeasStar1[4], int* Star1Flag, double Star1QuatInstall[4], double QuatECI[4], double AttiVelECI[3]);
void GetStar2Meas(double MeasStar2[4], int* Star2Flag, double Star2QuatInstall[4], double QuatECI[4], double AttiVelECI[3]);

/// @brief 生成星敏1遥测帧
/// @param frame:生成的遥测帧数据 
/// @return 
uint8_t generate_star_1_frame(uint8_t *frame, double QuatECI[4], double AttiVelECI[3])
{
	CAK_TLM_1_STRUCT star;
	memset(frame,0,CAK_TLM_1_Info_Length);
	memset((uint8_t *)&star.FRAME_HEAD,0,CAK_TLM_1_Info_Length);

	
	double  MeasStar1[4]; int Star1Flag = 0;
	GetStar1Meas(MeasStar1, &Star1Flag, Star1QuatInstall, QuatECI, AttiVelECI);

	//printf("MeasStar1 is %10.3f \t %10.3f \t %10.3f \t %10.3f\r\n", MeasStar1[0], MeasStar1[1], MeasStar1[2], MeasStar1[3]);
	//printf("Star1Flag is %10.3d\r\n", Star1Flag);
	spdlog::info("MeasStar1为:{1:10.6f},{2:10.6f},{3:10.6f},{4:10.6f},星敏有效标志为{5:d}",__FUNCTION__,
					MeasStar1[0], MeasStar1[1], MeasStar1[2], MeasStar1[3],Star1Flag);

	star.FRAME_HEAD = 0xA18A;
	star.TLM_FRAME_ID = 0;
	star.q0 = htonl((int32_t)(MeasStar1[0] *2147483647.5));
	star.q1 = htonl((int32_t)(MeasStar1[1] *2147483647.5));
	star.q2 = htonl((int32_t)(MeasStar1[2] *2147483647.5));
	star.q3 = htonl((int32_t)(MeasStar1[3] *2147483647.5));
	
	star.STAR_NUM_D = 100;
	star.WD_CNT = 12;
	star.STAR_NUM_T = 23;
	star.para1 = 0x91;

	uint8_t checksum = 0;
	uint8_t *buffer = (uint8_t *)&star.FRAME_HEAD;
	for(int kc = 0;kc<CAK_TLM_1_Info_Length-1;kc++)
		checksum += buffer[kc];
	star.SUM = checksum;
	memcpy(frame,(uint8_t *)&star.FRAME_HEAD,CAK_TLM_1_Info_Length);

	spdlog::info("函数{0}STAR数据帧长度为:{1:2d} 校验和为:{2:2x}",__FUNCTION__,
			sizeof(CAK_TLM_1_STRUCT),frame[CAK_TLM_1_Info_Length-1]);

	return 1;
}


/// @brief 生成星敏2遥测帧
/// @param frame:生成的遥测帧数据 
/// @return 
uint8_t generate_star_2_frame(uint8_t *frame, double QuatECI[4], double AttiVelECI[3])
{
	CAK_TLM_1_STRUCT star;
	memset(frame,0,CAK_TLM_1_Info_Length);
	memset((uint8_t *)&star.FRAME_HEAD,0,CAK_TLM_1_Info_Length);


	double  MeasStar2[4]; int Star2Flag = 0;
	GetStar2Meas(MeasStar2, &Star2Flag, Star2QuatInstall, QuatECI, AttiVelECI);

	// printf("MeasStar2 is %10.3f \t %10.3f \t %10.3f \t %10.3f\r\n", MeasStar2[0], MeasStar2[1], MeasStar2[2], MeasStar2[3]);
	// printf("Star2Flag is %10.3d\r\n", Star2Flag);

	spdlog::info("MeasStar2为:{1:10.6f},{2:10.6f},{3:10.6f},{4:10.6f},星敏有效标志为{5:d}",__FUNCTION__,
					 MeasStar2[0], MeasStar2[1], MeasStar2[2], MeasStar2[3],Star2Flag);

	star.FRAME_HEAD = 0xA18A;
	star.TLM_FRAME_ID = 0;
	star.q0 = htonl((int32_t)(MeasStar2[0] *2147483647.5));
	star.q1 = htonl((int32_t)(MeasStar2[1] *2147483647.5));
	star.q2 = htonl((int32_t)(MeasStar2[2] *2147483647.5));
	star.q3 = htonl((int32_t)(MeasStar2[3] *2147483647.5));
	
	star.STAR_NUM_D = 100;
	star.WD_CNT = 12;
	star.STAR_NUM_T = 23;
	star.para1 = 0x91;

	uint8_t checksum = 0;
	uint8_t *buffer = (uint8_t *)&star.FRAME_HEAD;
	for(int kc = 0;kc<CAK_TLM_1_Info_Length-1;kc++)
		checksum += buffer[kc];
	star.SUM = checksum;
	memcpy(frame,(uint8_t *)&star.FRAME_HEAD,CAK_TLM_1_Info_Length);

	spdlog::info("函数{0}STAR数据帧长度为:{1:2d} 校验和为:{2:2x}",__FUNCTION__,
			sizeof(CAK_TLM_1_STRUCT),frame[CAK_TLM_1_Info_Length-1]);
	return 1;
}

/* 星敏感器1测量模拟 */
void GetStar1Meas(double MeasStar1[4], int* Star1Flag, double Star1QuatInstall[4], double QuatECI[4], double AttiVelECI[3]) {

	/* 星敏感器1测量模拟模块 */
	/* 输入量 */
	/* QuatECI			惯性系下姿态四元数qbi */
	/* AttiVelECI		角速度Wbi */
	/* Star1QuatInstall 星敏感器1总安装四元数（从本体到真实） */
	/* 输出量 */
	/* MeasStar1		星敏感器1输出值Wgi_meas */
	/* Star1Flag		星敏感器1有效标志位 */

	double StarBias_axis, StarBias_plane, StarNoise_axis, StarNoise_plane, AttiVelECI_norm, ii;
	double AttiVelECI_deg[3], MeasErr[3], q_sr_i_true_1[4], e_sr_i_err_1[3], q_sr_i_err_1[4], q_sr_i_meas_1[4], CrossMatrixQuatECI[4][4], CrossMatrixQ_sr_i_true_1[4][4];
	double StarMeasErr[3][4] = { {60.0,5.0,0.0,0.0},{170.0,28.0,0.0,0.0},{250.0,50.0,0.0,0.0} };

	AttiVelECI_deg[0] = AttiVelECI[0] * 180.0 / 3.1415926535;
	AttiVelECI_deg[1] = AttiVelECI[1] * 180.0 / 3.1415926535;
	AttiVelECI_deg[2] = AttiVelECI[2] * 180.0 / 3.1415926535;

	AttiVelECI_norm = sqrt(pow(AttiVelECI_deg[0], 2) + pow(AttiVelECI_deg[1], 2) + pow(AttiVelECI_deg[2], 2));
	if (AttiVelECI_norm == 0)
	{
		StarNoise_axis = StarMeasErr[0][0] / 3600.0;
		StarNoise_plane = StarMeasErr[0][1] / 3600.0;
		StarBias_axis = StarMeasErr[0][2] / 3600.0;
		StarBias_plane = StarMeasErr[0][3] / 3600.0;
	}
	else if ((0 < AttiVelECI_norm) && (AttiVelECI_norm < 0.5))
	{
		StarNoise_axis = StarMeasErr[1][0] / 3600.0;
		StarNoise_plane = StarMeasErr[1][1] / 3600.0;
		StarBias_axis = StarMeasErr[1][2] / 3600.0;
		StarBias_plane = StarMeasErr[1][3] / 3600.0;
	}
	else if (0.5 <= AttiVelECI_norm)
	{
		StarNoise_axis = StarMeasErr[2][0] / 3600.0;
		StarNoise_plane = StarMeasErr[2][1] / 3600.0;
		StarBias_axis = StarMeasErr[2][2] / 3600.0;
		StarBias_plane = StarMeasErr[2][3] / 3600.0;
	}
	//生成随机误差量
	for (int i = 0; i < 3; i++)
	{
		MeasErr[i] = (rand() / ((double)RAND_MAX)) * 2.0 - 1.0;
	}

	//计算q_sr_i_true_1
	qleft(CrossMatrixQuatECI, QuatECI);
	mtxMtp((double*)q_sr_i_true_1, (double*)CrossMatrixQuatECI, 4, 4, (double*)Star1QuatInstall, 4, 1);
	ii = -1.0;
	if (q_sr_i_true_1[0] < 0) {
		mtxMtp((double*)q_sr_i_true_1, (double*)q_sr_i_true_1, 4, 1, &ii, 1, 1);
	}

	e_sr_i_err_1[0] = StarBias_axis + StarNoise_axis * MeasErr[0];
	e_sr_i_err_1[1] = StarBias_plane + StarNoise_plane * MeasErr[1];
	e_sr_i_err_1[2] = StarBias_plane + StarNoise_plane * MeasErr[2];
	euler2qua(q_sr_i_err_1, e_sr_i_err_1);
	qleft(CrossMatrixQ_sr_i_true_1, q_sr_i_true_1);
	mtxMtp((double*)q_sr_i_meas_1, (double*)CrossMatrixQ_sr_i_true_1, 4, 4, (double*)q_sr_i_err_1, 4, 1);
	if (q_sr_i_meas_1[0] < 0) {
		mtxMtp((double*)q_sr_i_meas_1, (double*)q_sr_i_meas_1, 4, 1, &ii, 1, 1);
	}

	MeasStar1[0] = q_sr_i_meas_1[0];
	MeasStar1[1] = q_sr_i_meas_1[1];
	MeasStar1[2] = q_sr_i_meas_1[2];
	MeasStar1[3] = q_sr_i_meas_1[3];
	*Star1Flag = 1;

}


/* 星敏感器2测量模拟 */
void GetStar2Meas(double MeasStar2[4], int* Star2Flag, double Star2QuatInstall[4], double QuatECI[4], double AttiVelECI[3]) {

	/* 星敏感器1测量模拟模块 */
	/* 输入量 */
	/* QuatECI			惯性系下姿态四元数qbi */
	/* AttiVelECI		角速度Wbi */
	/* Star2QuatInstall 星敏感器2总安装四元数（从本体到真实） */
	/* 输出量 */
	/* MeasStar2		星敏感器2输出值Wgi_meas */
	/* Star2Flag		星敏感器2有效标志位 */

	double StarBias_axis, StarBias_plane, StarNoise_axis, StarNoise_plane, AttiVelECI_norm, ii;
	double AttiVelECI_deg[3], MeasErr[3], q_sr_i_true_2[4], e_sr_i_err_2[3], q_sr_i_err_2[4], q_sr_i_meas_2[4], CrossMatrixQuatECI[4][4], CrossMatrixQ_sr_i_true_2[4][4];
	double StarMeasErr[3][4] = { {60.0,5.0,0.0,0.0},{170.0,28.0,0.0,0.0},{250.0,50.0,0.0,0.0} };

	AttiVelECI_deg[0] = AttiVelECI[0] * 180.0 / 3.1415926535;
	AttiVelECI_deg[1] = AttiVelECI[1] * 180.0 / 3.1415926535;
	AttiVelECI_deg[2] = AttiVelECI[2] * 180.0 / 3.1415926535;

	AttiVelECI_norm = sqrt(pow(AttiVelECI_deg[0], 2) + pow(AttiVelECI_deg[1], 2) + pow(AttiVelECI_deg[2], 2));
	if (AttiVelECI_norm == 0)
	{
		StarNoise_axis = StarMeasErr[0][0] / 3600.0;
		StarNoise_plane = StarMeasErr[0][1] / 3600.0;
		StarBias_axis = StarMeasErr[0][2] / 3600.0;
		StarBias_plane = StarMeasErr[0][3] / 3600.0;
	}
	else if ((0 < AttiVelECI_norm) && (AttiVelECI_norm < 0.5))
	{
		StarNoise_axis = StarMeasErr[1][0] / 3600.0;
		StarNoise_plane = StarMeasErr[1][1] / 3600.0;
		StarBias_axis = StarMeasErr[1][2] / 3600.0;
		StarBias_plane = StarMeasErr[1][3] / 3600.0;
	}
	else if (0.5 <= AttiVelECI_norm)
	{
		StarNoise_axis = StarMeasErr[2][0] / 3600.0;
		StarNoise_plane = StarMeasErr[2][1] / 3600.0;
		StarBias_axis = StarMeasErr[2][2] / 3600.0;
		StarBias_plane = StarMeasErr[2][3] / 3600.0;
	}
	//生成随机误差量
	for (int i = 0; i < 3; i++)
	{
		MeasErr[i] = (rand() / ((double)RAND_MAX)) * 2.0 - 1.0;
	}

	//计算q_sr_i_true_1
	qleft(CrossMatrixQuatECI, QuatECI);
	mtxMtp((double*)q_sr_i_true_2, (double*)CrossMatrixQuatECI, 4, 4, (double*)Star2QuatInstall, 4, 1);
	ii = -1.0;
	if (q_sr_i_true_2[0] < 0) {
		mtxMtp((double*)q_sr_i_true_2, (double*)q_sr_i_true_2, 4, 1, &ii, 1, 1);
	}

	e_sr_i_err_2[0] = StarBias_axis + StarNoise_axis * MeasErr[0];
	e_sr_i_err_2[1] = StarBias_plane + StarNoise_plane * MeasErr[1];
	e_sr_i_err_2[2] = StarBias_plane + StarNoise_plane * MeasErr[2];
	euler2qua(q_sr_i_err_2, e_sr_i_err_2);
	qleft(CrossMatrixQ_sr_i_true_2, q_sr_i_true_2);
	mtxMtp((double*)q_sr_i_meas_2, (double*)CrossMatrixQ_sr_i_true_2, 4, 4, (double*)q_sr_i_err_2, 4, 1);
	if (q_sr_i_meas_2[0] < 0) {
		mtxMtp((double*)q_sr_i_meas_2, (double*)q_sr_i_meas_2, 4, 1, &ii, 1, 1);
	}

	MeasStar2[0] = q_sr_i_meas_2[0];
	MeasStar2[1] = q_sr_i_meas_2[1];
	MeasStar2[2] = q_sr_i_meas_2[2];
	MeasStar2[3] = q_sr_i_meas_2[3];
	*Star2Flag = 1;

}