#include "sensor_wheel.h"
#include "orbit_info.h"
#include "GNCFunction.h"

#include "libconvert.h"

#include "liblog.h"


double WheelOutputPeriod = 1.0;		//动量轮输出周期


//其中k为被转换发送的数据，exp取1000，limit4 取4.294967296e9，数组a[]为转换后发送的4字节数据。
void cal_cmder_para(double k, double exp, uint8_t *a);

/// @brief 生成遥测动量轮帧
/// @param frame:生成的遥测帧数据 
/// @return:0
uint8_t generate_wheel_frame(uint8_t *frame,double CmdWheel)
{
	FlyWheel_Frame wheel;
	memset(frame,0,WHEEL_FRAME_LENGTH);
	memset( (uint8_t *)&wheel.header,0,WHEEL_FRAME_LENGTH);

	wheel.header = 0xFF;
	//wheel.rotate_speed[3] = 10;
	cal_cmder_para(CmdWheel,WHEEL_EXP,wheel.rotate_speed);
	wheel.id = 0x11;

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

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

void cal_cmder_para(double k, double exp, uint8_t *a)
{
	//float limit4=4.294967296e9;
	unsigned long t;
	if(k<0) t=(unsigned long)(WHEEL_LIMIT4+k*exp);
	else t=(unsigned long)(k*exp);
	a[0]=(uint8_t)(t>>24)&0xff;
	a[1]=(uint8_t)(t>>16)&0xff;
	a[2]=(uint8_t)(t>>8)&0xff;
	a[3]=(uint8_t)t&0xff;
}


double cal_message_para(int s1, int s2, int s3, int s4, int exp)
{
	//float limit4=4.294967296e9;
	double result=0;
	unsigned long k = 0;

	k=(((unsigned long)s1<<24) & 0xff000000) | (((unsigned long)s2<<16) & 0xff0000) | (((unsigned long)s3<<8) & 0xff00) | ((unsigned long)s4 & 0xff);

	if(k>0x7fffffff)
	{
	result = (double)k;
	result = (result-WHEEL_LIMIT4);
	result = result/exp;
	}
	else
	{
	result = (double)k;
	result = result/exp; 
	}
	return result;
}


/* 动量轮输出模拟 */
void sensor_WheelMeas(double WheelTorqueBody[3], double WheelAM[3], double CmdWheel[3]) {

	/* 动量轮测量模拟模块 */
	/* 输入量 */
	/* WheelMtxI			动量轮转动惯量(主对角) */
	/* WheelMtxInstall		动量轮总安装矩阵 */
	/* CmdWheel				动量轮转速指令rpm*/
	/* WheelOutputPeriod	动量轮采样周期rpm*/
	/* 输出量 */
	/* MeasWheelRPM			动量轮输出值rpm转每分 */
	/* WheelTorqueBody		动量轮总力矩 */
	/* WheelAM				动量轮角动量 */

	double wgi_true[3], DeltaWheelVel[3], temp[3], WheelTorque[3], WheelAMInstall[3], MeasWheelRPM[3];
	static double WheelRPM[3] = { 0, 0, 0 }; 
	double WheelMtxI = 1.273 * 1e-4;
	double  WheelMtxInstall[3][3] = { {0, 1, 0},
									  {-1, 0, 0},
									  {0, 0, 1} };

	MeasWheelRPM[0] = CmdWheel[0] + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) / 0.5;
	MeasWheelRPM[1] = CmdWheel[1] + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) / 0.5;
	MeasWheelRPM[2] = CmdWheel[2] + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) / 0.5;

	DeltaWheelVel[0] = (CmdWheel[0] * 2.0 * 3.1415926535 / 60.0) - (WheelRPM[0] * 2.0 * 3.1415926535 / 60.0);
	DeltaWheelVel[1] = (CmdWheel[1] * 2.0 * 3.1415926535 / 60.0) - (WheelRPM[1] * 2.0 * 3.1415926535 / 60.0);
	DeltaWheelVel[2] = (CmdWheel[2] * 2.0 * 3.1415926535 / 60.0) - (WheelRPM[2] * 2.0 * 3.1415926535 / 60.0);

	temp[0] = DeltaWheelVel[0] / WheelOutputPeriod;
	temp[1] = DeltaWheelVel[1] / WheelOutputPeriod;
	temp[2] = DeltaWheelVel[2] / WheelOutputPeriod;

	WheelTorque[0] = -1.0 * WheelMtxI * temp[0];
	WheelTorque[1] = -1.0 * WheelMtxI * temp[1];
	WheelTorque[2] = -1.0 * WheelMtxI * temp[2];

	mtxMtp((double*)WheelTorqueBody, (double*)WheelMtxInstall, 3, 3, (double*)WheelTorque, 3, 1);

	WheelAMInstall[0] = WheelMtxI * (CmdWheel[0] * 2.0 * 3.1415926535 / 60.0);
	WheelAMInstall[1] = WheelMtxI * (CmdWheel[1] * 2.0 * 3.1415926535 / 60.0);
	WheelAMInstall[2] = WheelMtxI * (CmdWheel[2] * 2.0 * 3.1415926535 / 60.0);

	mtxMtp((double*)WheelAM, (double*)WheelMtxInstall, 3, 3, (double*)WheelAMInstall, 3, 1);

	WheelRPM[0] = CmdWheel[0];
	WheelRPM[1] = CmdWheel[1];
	WheelRPM[2] = CmdWheel[2];
}