#include "sensor_gnss.h"
#include "orbit_info.h"

#include "libconvert.h"

#include "liblog.h"

#include <arpa/inet.h>//linux下大小端转换

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

using namespace Dynamics;

GNSS_Base_t gnss_info;
uint8_t telemetry_cnt = 0;

void GetGNSSMeas(double PosVelECIMeas[6], double PosVelWGS84[6], double* GNSSTime, double PosVelECIReal[6], double mjd, double Epoch_J2000);

/// @brief 生成GNSS遥测帧
/// @param frame:生成的遥测帧数据 
/// @return: 无
uint8_t generate_gnss_frame(uint8_t *frame, double PosVelECIReal[6], double mjd, double Epoch_J2000)
{

	uint8_t gnss_frame[GPS_Hk_FRAME_LENGTH] = {0};
	double PosVelECIMeas[6], PosVelWGS84[6], GNSSTime;
	GetGNSSMeas(PosVelECIMeas, PosVelWGS84, &GNSSTime, PosVelECIReal, mjd, Epoch_J2000);


	//printf("PosVelECIMeas is %10.3f \t %10.3f \t %10.3f\r\n", PosVelECIMeas[0], PosVelECIMeas[1], PosVelECIMeas[2]);
	//printf("PosVelWGS84 is %10.3f \t %10.3f \t %10.3f\r\n", PosVelWGS84[0], PosVelWGS84[1], PosVelWGS84[1]);
	spdlog::info("函数{0}计算位置为:{1:.5f},{2:.5f},{3:.5f},速度为:{4:.2f},{5:.2f},{6:.2f},GNSSTime为:{7:.5f},Epoch_J2000为{8:.5f}",__FUNCTION__,
		 PosVelECIMeas[0], PosVelECIMeas[1], PosVelECIMeas[2],
		 PosVelECIMeas[3], PosVelECIMeas[4], PosVelECIMeas[5],
		GNSSTime, Epoch_J2000);

	uint16_t checksum = 0;
	gnss_info.header[0] = 0xEB;
	gnss_info.header[1] = 0x90;
	gnss_info.length = 0x6C01;
	gnss_info.datatype = 0x12;
	gnss_info.telemetry_cnt = telemetry_cnt++;
	gnss_info.GNSS_X_POS_inJ2000 = htonl((int32_t)(PosVelECIMeas[0]*10.0));
	gnss_info.GNSS_Y_POS_inJ2000 = htonl((int32_t)(PosVelECIMeas[1]*10.0));
	gnss_info.GNSS_Z_POS_inJ2000 = htonl((int32_t)(PosVelECIMeas[2]*10.0));
	gnss_info.GNSS_X_SPEED_inJ2000 = htonl((int32_t)(PosVelECIMeas[3]*100.0));
	gnss_info.GNSS_Y_SPEED_inJ2000 = htonl((int32_t)(PosVelECIMeas[4]*100.0));
	gnss_info.GNSS_Z_SPEED_inJ2000 = htonl((int32_t)(PosVelECIMeas[5]*100.0));
	gnss_info.GNSS_X_POS_inWGS84   = htonl((int32_t)(PosVelWGS84[0]*10.0));
	gnss_info.GNSS_Y_POS_inWGS84   = htonl((int32_t)(PosVelWGS84[1]*10.0));
	gnss_info.GNSS_Z_POS_inWGS84   = htonl((int32_t)(PosVelWGS84[2]*10.0));
	gnss_info.GNSS_X_SPEED_inWGS84 = htonl((int32_t)(PosVelWGS84[3]*100.0));
	gnss_info.GNSS_Y_SPEED_inWGS84 = htonl((int32_t)(PosVelWGS84[4]*100.0));
	gnss_info.GNSS_Z_SPEED_inWGS84 = htonl((int32_t)(PosVelWGS84[5]*100.0));

	gnss_info.posi_status = 0xE4;

	gnss_info.utc_time = htonl(GNSSTime);


	memcpy(gnss_frame,(uint8_t *)&gnss_info.header[0],GPS_Hk_FRAME_LENGTH);

	for(int kc= 2;kc<GPS_Hk_FRAME_LENGTH-4;kc++)
	{
		checksum+=gnss_frame[kc];
	}

	gnss_info.checksum = htons(checksum);
	gnss_info.tail = 0xD709;

	memcpy(frame,(uint8_t *)&gnss_info,GPS_Hk_FRAME_LENGTH);		
    return 1;

}



/* GNSS模拟器 */
void GetGNSSMeas(double PosVelECIMeas[6], double PosVelWGS84[6],double* GNSSTime, double PosVelECIReal[6], double mjd, double Epoch_J2000) {

	/* 近距离激光测距模拟模块 */
	/* 输入量 */
	/* PosVelECIReal			J2000坐标系下位置速度真实值 */
	/* mjd						简约儒略日，单位：天 */
	/* 输出量 */
	/* PosVelECIMeas			J2000坐标系下位置速度测量值 */
	/* PosVelWGS84				WGS84坐标系下位置速度测量值 */

	double ECIPos[3], ECIVel[3];

	ECIPos[0] = PosVelECIReal[0] + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 50;
	ECIPos[1] = PosVelECIReal[1] + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 50;
	ECIPos[2] = PosVelECIReal[2] + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 50;
	ECIVel[0] = PosVelECIReal[3] + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1;
	ECIVel[1] = PosVelECIReal[4] + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1;
	ECIVel[2] = PosVelECIReal[5] + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1;
	PosVelECIMeas[0] = ECIPos[0]; PosVelECIMeas[1] = ECIPos[1]; PosVelECIMeas[2] = ECIPos[2];
	PosVelECIMeas[3] = ECIVel[0]; PosVelECIMeas[4] = ECIVel[1]; PosVelECIMeas[5] = ECIVel[2];

	J2000_WGS84(mjd, PosVelECIMeas, PosVelWGS84);

	*GNSSTime = Epoch_J2000 - (86400 * 365 * 2);
}