#include "sensor_mems.h"
#include "orbit_info.h"

#include "libconvert.h"

#include "liblog.h"

#include <math.h>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include "GNCFunction.h"

int32_t Delta_angle_int[3];
uint16_t Integ_Cnt_int[3];
double  Gyro2MtxInstall[3][3] = { {-0.000872664182949154, -0.999999237792072, 0.000873425726015331},
								  {0.999999238456644, -0.000873425726015331, -0.000871902307307135},
								  {0.000872664515235150, 0.000872664182949154, 0.999999238456644} }; 
double MemsOutputPeriod = 1.0;

void GetGyro2Meas(double MeasGyro2[3], double AttiVelECI[3], double Gyro2MtxInstall[3][3], double MemsOutputPeriod);

/// @brief 生成MEMS遥测帧
/// @param frame:生成的遥测帧数据 
/// @return 
uint8_t generate_mems_frame(uint8_t *frame, double AttiVelECI[3])
{
	MEMS_Frame mems;
	memset(frame,0,MEMS_FRAME_LENGTH);
	memset( (uint8_t *)&mems.header,0,MEMS_FRAME_LENGTH);


	double  MeasGyro2[3];
	GetGyro2Meas(MeasGyro2, AttiVelECI, Gyro2MtxInstall, MemsOutputPeriod);

	//printf("MeasGyro2 is %10.3f \t %10.3f \t %10.3f\r\n", MeasGyro2[0], MeasGyro2[1], MeasGyro2[2]);
	spdlog::info("函数{0}计算MEMS输出为:{1:10.3f},{2:10.3f},{3:10.3f}",__FUNCTION__,
					MeasGyro2[0], MeasGyro2[1], MeasGyro2[2]);

	mems.header = 0x97;

	mems.x_angle_delta[0] = (uint8_t)((Delta_angle_int[0] & 0x00FF0000) >> 16);
	mems.x_angle_delta[1] = (uint8_t)((Delta_angle_int[0] & 0x0000FF00) >> 8);
	mems.x_angle_delta[2] = (uint8_t)((Delta_angle_int[0] & 0x000000FF));
	mems.x_mems_integral_Cnt = htons(Integ_Cnt_int[0]);
	mems.y_angle_delta[0] = (uint8_t)((Delta_angle_int[1] & 0x00FF0000) >> 16);
	mems.y_angle_delta[1] = (uint8_t)((Delta_angle_int[1] & 0x0000FF00) >> 8);
	mems.y_angle_delta[2] = (uint8_t)((Delta_angle_int[1] & 0x000000FF));
	mems.y_mems_integral_Cnt = htons(Integ_Cnt_int[1]);
	mems.z_angle_delta[0] = (uint8_t)((Delta_angle_int[2] & 0x00FF0000) >> 16);
	mems.z_angle_delta[1] = (uint8_t)((Delta_angle_int[2] & 0x0000FF00) >> 8);
	mems.z_angle_delta[2] = (uint8_t)((Delta_angle_int[2] & 0x000000FF));
	mems.z_mems_integral_Cnt = htons(Integ_Cnt_int[2]);


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

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

	return 1;
}

//* 微机械惯组测量模拟 */
void GetGyro2Meas(double MeasGyro2[3], double AttiVelECI[3], double Gyro2MtxInstall[3][3], double MemsOutputPeriod) {

	/* 微机械惯组测量模拟模块 */
	/* 输入量 */
	/* AttiVelECI		角速度Wbi */
	/* Gyro2MtxInstall  微机械惯组总安装矩阵（从本体到真实） */
	/* MemsOutputPeriod 循环周期*/
	/* 输出量 */
	/* MeasGyro2		微机械惯组输出值Wgi_meas */

	double wgi_true[3];
	double wgi_meas[3];
	double Gyro2MeasErr[3] = { 1.39E-5 * 3.1415926535 / 180.0, 8.33E-5 * 3.1415926535 / 180.0, 1E-4 * 3.1415926535 / 180.0 };
	static double b_true2[3] = { 2.4260e-07, 2.4260e-07, 2.4260e-07 };
	static double b_true_last2[3] = { 2.4260e-07, 2.4260e-07, 2.4260e-07 };
	double tSample = 0.05;
	double GyroNoise_N = Gyro2MeasErr[1];
	double GyroNoise_K = Gyro2MeasErr[2];

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

	//保存b_true_last2
	b_true_last2[0] = b_true2[0];
	b_true_last2[1] = b_true2[1];
	b_true_last2[2] = b_true2[2];

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

	/* 角速度结果按通讯协议输出 */
	double Wx = MeasGyro2[0] * 180.0 / 3.1415926535;  //角度制，单位°/s
	double Wy = MeasGyro2[1] * 180.0 / 3.1415926535;
	double Wz = MeasGyro2[2] * 180.0 / 3.1415926535;
	
	double Ox = 1.465294; double Oy = 2.140157; double Oz = 1.470876;  //%常值漂移，单位为°/h
	double Tx = 20.0;  double Ty = 20.0;  double Tz = 20.0;   //积分次数
	double Kx = 319198.1;  double Ky = 319182.2;  double Kz = 319335.1;  //%标度因子，单位为^/°/s

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


	Delta_angle_int[0] = (int32_t)Delta_angle_x;  //将double型变量转换为整数
	Delta_angle_int[1] = (int32_t)Delta_angle_y;
	Delta_angle_int[2] = (int32_t)Delta_angle_z;
	Integ_Cnt_int[0] = (uint16_t)Integ_Cnt_x;
	Integ_Cnt_int[1] = (uint16_t)Integ_Cnt_y;
	Integ_Cnt_int[2] = (uint16_t)Integ_Cnt_z;
}