#include "sensor_gyro.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>

int32_t Delta_Gyro_angle_int[3];
uint16_t Integ_time_int[3];
double  Gyro1MtxInstall[3][3] = { {-0.000872664182949154, 0.000873425726015331, 0.999999237792072},
								  {0.999999238456644, -0.000871902307307135, 0.000873425726015331},
								  {0.000872664515235150, 0.999999238456644, -0.000872664182949154} };
double GyroOutputPeriod = 1.0;

double Ox = 0.017; double Oy = 0.205; double Oz = 0.369;  //%常值漂移，单位为°/h
double Tx = 20.0;  double Ty = 20.0;  double Tz = 20.0;   //积分时间，单位为ms
double Kx = 289752.5;  double Ky = 299114.7;  double Kz = 282454.6;  //%标度因子，单位为^/°/s

void GetGyro1Meas(double MeasGyro1[3], double AttiVelECI[3], double Gyro1MtxInstall[3][3], double GyroOutputPeriod);

/// @brief 生成GYRO遥测帧
/// @param frame:生成的遥测帧数据 
/// @return 
uint8_t generate_gyro_frame(uint8_t *frame, double AttiVelECI[3])
{
	Gyro_Frame gyro;
	memset(frame,0,GYRO_FRAME_LENGTH);
	memset( (uint8_t *)&gyro.header,0,GYRO_FRAME_LENGTH);


	double  MeasGyro1[3];
	GetGyro1Meas( MeasGyro1, AttiVelECI, Gyro1MtxInstall, GyroOutputPeriod);

//	printf("MeasGyro1 is %10.3f \t %10.3f \t %10.3f\r\n", MeasGyro1[0], MeasGyro1[1], MeasGyro1[2]);
	spdlog::info("MeasGyro1为:{1:10.6f},{2:10.6f},{3:10.6f}",__FUNCTION__,
					MeasGyro1[0], MeasGyro1[1], MeasGyro1[2]);

	gyro.header = 0x97;

	gyro.x_angle_delta[0] =  (uint8_t)((Delta_Gyro_angle_int[0] & 0x00FF0000) >> 16);
	gyro.x_angle_delta[1] =  (uint8_t)((Delta_Gyro_angle_int[0] & 0x0000FF00)>>8);
	gyro.x_angle_delta[2] =  (uint8_t)((Delta_Gyro_angle_int[0] & 0x000000FF));
	gyro.x_integral_time = htons(Integ_time_int[0]);
	gyro.y_angle_delta[0] =  (uint8_t)((Delta_Gyro_angle_int[1] & 0x00FF0000) >> 16);
	gyro.y_angle_delta[1] =  (uint8_t)((Delta_Gyro_angle_int[1] & 0x0000FF00)>>8);
	gyro.y_angle_delta[2] =  (uint8_t)((Delta_Gyro_angle_int[1] & 0x000000FF));
	gyro.y_integral_time = htons(Integ_time_int[1]);
	gyro.z_angle_delta[0] =  (uint8_t)((Delta_Gyro_angle_int[2] & 0x00FF0000)>>16);
	gyro.z_angle_delta[1] =  (uint8_t)((Delta_Gyro_angle_int[2] & 0x0000FF00)>>8);
	gyro.z_angle_delta[2] =  (uint8_t)((Delta_Gyro_angle_int[2] & 0x000000FF));
	gyro.z_integral_time = htons(Integ_time_int[2]);


	uint8_t checksum = 0;
	uint8_t *buffer = (uint8_t *)&gyro.header;
	for(int kc = 0;kc<GYRO_FRAME_LENGTH-1;kc++)
		checksum +=buffer[kc];
	gyro.checksum = checksum;
	memcpy(frame,(uint8_t *)&gyro.header,GYRO_FRAME_LENGTH);

	spdlog::info("函数{0}微光纤数据帧长度为:{1:2d} 校验和为:{2:2x}",__FUNCTION__,
			sizeof(Gyro_Frame),frame[GYRO_FRAME_LENGTH-1]);

	return 1;
}


/* 微光纤陀螺测量模拟 */
void GetGyro1Meas(double MeasGyro1[3], double AttiVelECI[3], double Gyro1MtxInstall[3][3], double GyroOutputPeriod) {

	/* 微光纤陀螺测量模拟模块 */
	/* 输入量 */
	/* AttiVelECI		角速度Wbi */
	/* Gyro1MtxInstall  微光纤陀螺总安装矩阵（从本体到真实） */
	/* GyroOutputPeriod 循环周期*/
	/* 输出量 */
	/* MeasGyro1		微光纤陀螺输出值Wgi_meas */

	double wgi_true[3];
	double wgi_meas[3];
	double Gyro1MeasErr[3] = { 4.17E-5 * 3.1415926535 / 180.0, 3.33E-4 * 3.1415926535 / 180.0, 1E-4 * 3.1415926535 / 180.0 };
	static double b_true1[3] = { 7.2780e-07, 7.2780e-07, 7.2780e-07 };
	static double b_true_last1[3] = { 7.2780e-07, 7.2780e-07, 7.2780e-07 };
	double tSample = 0.05;
	double GyroNoise_N = Gyro1MeasErr[1];
	double GyroNoise_K = Gyro1MeasErr[2];

	//计算wgi_true
	mtxMtp((double*)wgi_true, (double*)Gyro1MtxInstall, 3, 3, (double*)AttiVelECI, 3, 1);
	//计算wgi_meas
	for (int i = 0; i < 3; i++)
	{
		wgi_meas[i] = wgi_true[i] + 0.5 * (b_true1[i] + b_true_last1[i]) + sqrt(pow(GyroNoise_N, 2) / tSample + 0.0833 * pow(GyroNoise_K, 2) * tSample) * ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0);
	}
	MeasGyro1[0] = wgi_meas[0];
	MeasGyro1[1] = wgi_meas[1];
	MeasGyro1[2] = wgi_meas[2];

	//保存b_true_last1
	b_true_last1[0] = b_true1[0];
	b_true_last1[1] = b_true1[1];
	b_true_last1[2] = b_true1[2];

	//更新b_true1
	double temp = GyroOutputPeriod / tSample;
	for (int i = 0; i < temp; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			b_true1[j] += GyroNoise_K * ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * sqrt(tSample);
		}
	}

	/* 角速度结果按通讯协议输出 */
	double Wx = MeasGyro1[0] * 180.0 / 3.1415926535;  //角度制，单位°/s
	double Wy = MeasGyro1[1] * 180.0 / 3.1415926535;
	double Wz = MeasGyro1[2] * 180.0 / 3.1415926535;


	double Delta_angle_x = round(Tx * Kx * (Wx + Ox / 3600) / 1000);  //X轴角度增量脉冲数，单位为^
	double Delta_angle_y = round(Ty * Ky * (Wy + Oy / 3600) / 1000);  //Y轴角度增量脉冲数，单位为^
	double Delta_angle_z = round(Tz * Kz * (Wz + Oz / 3600) / 1000);  //Z轴角度增量脉冲数，单位为^

	double Integ_time_x = Tx;  //积分时间，单位为ms
	double Integ_time_y = Ty;
	double Integ_time_z = Tz;


	Delta_Gyro_angle_int[0] = (int32_t)Delta_angle_x;  //将double型变量转换为整数
	Delta_Gyro_angle_int[1] = (int32_t)Delta_angle_y;
	Delta_Gyro_angle_int[2] = (int32_t)Delta_angle_z;
	Integ_time_int[0] = (uint16_t)Integ_time_x;
	Integ_time_int[1] = (uint16_t)Integ_time_y;
	Integ_time_int[2] = (uint16_t)Integ_time_z;
}