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

void GetMeasDISRemote(double Dis_Remote[2], double TarRVECI[6], double ChaRVECI[6], double MtxECI2VVLH[3][3], double LOS_Remote[9], int TargetFLAG);
void GetMeasDISClose(double Dis_Close[2], double TarRVECI[6], double ChaRVECI[6], double MtxECI2VVLH[3][3], double MtxVVLH2Body[3][3], double Euler[3], double R_Plane[2], int TargetFLAG);

/// @brief 生成小光电遥测帧
/// @param frame:生成的遥测帧数据 
/// @return 生成帧的长度,生成失败返回0
//输入: TarRVECI[6],目标轨道 从姿轨动力学获得
//		ChaRVECI[6],本体轨道 从姿轨动力学获得
//		MtxECI2VVLH[3][3], 转移矩阵  从姿轨动力学获得
//      LOS_Remote[9], 远距离相机测角，从TX2中获取
//		Euler[3],      姿态角 从姿轨动力学获得
//		R_Plane[2],    近距离相机，从TX2中获取
//		TargetFLAG     标记位 //XGD和TX2使用标记,1--左帆板, 0--本体
//输出:frame 生成帧
void generate_xgd_frame(uint8_t *frame, double TarRVECI[6], double ChaRVECI[6], double MtxECI2VVLH[3][3], double MtxVVLH2Body[3][3], double LOS_Remote[9], double Euler[3], double R_Plane[2], int TargetFLAG)
{
	static uint8_t frame_cnt = 0;
	OPTICAL_TLM_STR xgd;
	memset(frame,0,OPTIC_FRAME_LENGTH);
	memset( (uint8_t *)&xgd.FRAME_HEAD,0,OPTIC_FRAME_LENGTH);


	double  Dis_Remote[2], Dis_Close[2];
	GetMeasDISRemote(Dis_Remote, TarRVECI, ChaRVECI, MtxECI2VVLH, LOS_Remote, TargetFLAG);
	GetMeasDISClose(Dis_Close, TarRVECI, ChaRVECI, MtxECI2VVLH, MtxVVLH2Body, Euler, R_Plane, TargetFLAG);

	//printf("LOS_Remote is %10.3f \t %10.3f\r\n", Dis_Remote[0], Dis_Remote[1]);
	//printf("LOS_Close is %10.3f \t %10.3f\r\n", Dis_Remote[0], Dis_Remote[1]);
	spdlog::info("函数{0} Dis_Remote为:{1:.6f}, {2:.6f}, Dis_Close为:{3:.6f}, {4:.6f}",__FUNCTION__,
					Dis_Remote[0], Dis_Remote[1],Dis_Close[0], Dis_Close[1]);

	xgd.FRAME_HEAD = 0x90EB;
	xgd.length = OPTIC_FRAME_LENGTH - 3;
	xgd.AI01 = convert_endian((float)Dis_Remote[1]);
	xgd.AI02 = convert_endian((float)Dis_Close[1]);
	xgd.AI03 = htons(2);
	xgd.AI19 = htonl(150);
	xgd.AI_far = (uint8_t)Dis_Remote[0];
	xgd.AI_close = (uint8_t)Dis_Close[0];
	xgd.AI10 = frame_cnt++;


	uint8_t checksum = 0;
	uint8_t *buffer = (uint8_t *)&xgd.FRAME_HEAD;
	for(int kc = 2;kc<OPTIC_FRAME_LENGTH-1;kc++)
		checksum +=buffer[kc];
	xgd.Check_sum = checksum;

	spdlog::info("函数{0}OPTICAL数据帧长度为:{1:2d} 校验和为:{2:2x}",__FUNCTION__,
					sizeof(OPTICAL_TLM_STR),xgd.Check_sum);
					
	memcpy(frame,(uint8_t *)&xgd.FRAME_HEAD,OPTIC_FRAME_LENGTH);
}


void generate_photo_frame(uint8_t *frame)
{
	static uint16_t frame_cnt = 0;
	memset(frame,0,249);
	frame[0] = 0xEB;
	frame[1] = 0x91;
	frame[2] = 0;
	frame[3] = 245;
	frame[4] = (frame_cnt & 0xFF00)>>8;
	frame[5] = (frame_cnt & 0x00FF);

	for(int kc = 6;kc<248;kc++) //不给校验位赋值
	{
		frame[kc] = frame_cnt+kc;
	}

	if(frame_cnt++>=270) frame_cnt = 0;

	for(int kc= 2;kc<248;kc++)
		frame[248] +=  frame[kc];
}


/* 远距离激光测距 */
void GetMeasDISRemote(double Dis_Remote[2], double TarRVECI[6], double ChaRVECI[6], double MtxECI2VVLH[3][3], double LOS_Remote[9], int TargetFLAG) {

	/* 远距离激光测距模拟模块 */
	/* 输入量 */
	/* TarRVECI			目标惯性系下RV */
	/* ChaRVECI			追踪星惯性系下RV */
	/* MtxECI2VVLH		追踪星惯性系到轨道系的旋转矩阵Aoi */
	/* LOS_Remote		远程测距角度信息 */
	/* TargetFLAG		目标选择标志 */
	/* 输出量 */
	/* Dis_Remote		远距离激光测距信息：标志位和距离*/

	double Dis_Remote_Measure = 0.0;
	double LOSReal, Dis_Measure, temp[3];
	int LOS_Flag, DisRemoteFlag;
	static double qbo[4] = { 1, 0, 0, 0 };

	double Alpha_Left_remote = LOS_Remote[2];
	double Beta_Left_remote = LOS_Remote[1];
	int LeftLOSRemoteFlag = LOS_Remote[0];
	double Alpha_Body_remote = LOS_Remote[5];
	double Beta_Body_remote = LOS_Remote[4];
	int BodyLOSRemoteFlag = LOS_Remote[3];
	double Alpha_Right_remote = LOS_Remote[8];
	double Beta_Right_remote = LOS_Remote[7];
	int RightLOSRemoteFlag = LOS_Remote[6];

	// 真实值
	double RT[3] = { TarRVECI[0], TarRVECI[1], TarRVECI[2] };
	double RC[3] = { ChaRVECI[0], ChaRVECI[1], ChaRVECI[2] };
	double R_Co5[3] = { 0 };
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			R_Co5[i] += MtxECI2VVLH[i][j] * (RT[j] - RC[j]);
		}
	}
	double Dis_Body = norm(R_Co5, 3);
	double a[3] = { 0, -10.682, 0 };
	mtxAdd((double*)temp, (double*)R_Co5, (double*)a, 3, 1);
	double Dis_Fan_Left = norm(temp, 3);
	double b[3] = { 0, 10.682, 0 };
	mtxAdd((double*)temp, (double*)R_Co5, (double*)b, 3, 1);
	double Dis_Fan_Right = norm(temp, 3);

	// 目标选择，0~本体，1~左帆板，2~右帆板,在OBC里做切换

	/*if (TargetFLAG == 0)
	{
		Dis_Measure = Dis_Body;
		LOSReal = fabs(Beta_Body_remote - 90.0);
		LOS_Flag = BodyLOSRemoteFlag;
	}
	else if (TargetFLAG == 1)
	{
		Dis_Measure = Dis_Fan_Left;
		LOSReal = fabs(Beta_Left_remote - 90.0);
		LOS_Flag = LeftLOSRemoteFlag;
	}
	else if (TargetFLAG == 2)
	{
		Dis_Measure = Dis_Fan_Right;
		LOSReal = fabs(Beta_Right_remote - 90.0);
		LOS_Flag = RightLOSRemoteFlag;
	}*/
	Dis_Measure = Dis_Body;
	LOSReal = fabs(Beta_Body_remote - 90.0);
	LOS_Flag = BodyLOSRemoteFlag;

	// 在测角有效、角度在4°以内，且距离在一定范围之内，测距有效
	if (LOS_Flag == 0 || LOSReal > 4.0 || Dis_Measure > 2000)
	{
		DisRemoteFlag = 0;
	}
	else
	{
		DisRemoteFlag = 1;
	}

	if (DisRemoteFlag == 1)
	{
		if (Dis_Measure < 2000 && Dis_Measure >= 500)
		{
			Dis_Remote_Measure = Dis_Measure + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 1.0 / 3.0;
		}
		else if (Dis_Measure < 500 && Dis_Measure >= 50)
		{
			Dis_Remote_Measure = Dis_Measure + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.1 / 3.0;
		}
	}

	// 返回结果数组
	Dis_Remote[0] = DisRemoteFlag;
	Dis_Remote[1] = Dis_Remote_Measure;
}


/* 近距离激光测距 */
void GetMeasDISClose(double Dis_Close[2], double TarRVECI[6], double ChaRVECI[6], double MtxECI2VVLH[3][3], double MtxVVLH2Body[3][3], double Euler[3], double R_Plane[2], int TargetFLAG) {

	/* 近距离激光测距模拟模块 */
	/* 输入量 */
	/* TarRVECI			目标惯性系下RV */
	/* ChaRVECI			追踪星惯性系下RV */
	/* MtxECI2VVLH		追踪星惯性系到轨道系的旋转矩阵Aoi */
	/* Euler			姿态角（滚俯偏123）弧度制 */
	/* R_Plane		 */
	/* TargetFLAG		目标选择标志 */
	/* 输出量 */
	/* Dis_Close		近距离激光测距信息：标志位和距离 */

	double Dis_Close_Measure = 0.0;
	int DisCloseFlag;
	double RVLVLH[6];
	double RLVLH[3];
	double PointLVLH[3];
	double MtxVVLH2LVLH[3][3] = { {0, 0,-1},
								  {1, 0, 0},
								  {0,-1, 0} };
	// 真实值
	double RT[3] = { TarRVECI[0], TarRVECI[1], TarRVECI[2] };
	double RC[3] = { ChaRVECI[0], ChaRVECI[1], ChaRVECI[2] };
	FunECI2LVLH(RLVLH, RVLVLH, ChaRVECI, TarRVECI);
	double R_Co5[3] = { 0 };
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			R_Co5[i] += MtxECI2VVLH[i][j] * (RT[j] - RC[j]);
		}
	}

	//Euler是偏滚俯，这依次需要俯仰和偏航
	/*if (R_Co5[0] >= 70)
	{
		DisCloseFlag = 0;
	}
	else if (R_Co5[0] < 70 && R_Co5[0] >= 50)
	{
		Dis_Close_Measure = fabs(R_Co5[0] / (cos(Euler[2] * 3.1415926 / 180.0) * cos(Euler[0] * 3.1415926 / 180.0))) + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.02 / 3.0;
	}
	else if (R_Co5[0] < 50 && R_Co5[0] >= 0)
	{
		Dis_Close_Measure = fabs(R_Co5[0] / (cos(Euler[2] * 3.1415926 / 180.0) * cos(Euler[0] * 3.1415926 / 180.0))) + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.01 / 3.0;
	}
	else
	{
		DisCloseFlag = 0;
	}*/
	if (R_Co5[0] >= 70)
	{
		DisCloseFlag = 0;
	}
	else if (R_Co5[0] < 70 && R_Co5[0] >= 50)
	{
		DisCloseFlag = 1;
		Dis_Close_Measure = fabs(R_Co5[0]) + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.02 / 3.0;
	}
	else if (R_Co5[0] < 50 && R_Co5[0] >= 0)
	{
		DisCloseFlag = 1;
		Dis_Close_Measure = fabs(R_Co5[0]) + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) * 0.01 / 3.0;
	}
	else
	{
		DisCloseFlag = 0;
	}

	// 判断激光测距是否有效
	/*if (TargetFLAG == 1)
	{
		if (R_Plane[0] < 0)
		{
			DisCloseFlag = 1;
		}
	}
	else if (TargetFLAG == 2)
	{
		if (R_Plane[1] > 0)
		{
			DisCloseFlag = 1;
		}
	}
	else if (TargetFLAG == 0)
	{
		DisCloseFlag = 1;
	}
	else
	{
		DisCloseFlag = 0;
	}*/


	/************  2024.12.04 近距离激光是否有效与姿态相关 ************/
	double PointBody[3] = { -1, 0, 0 };
	double MtxBody2VVLH[3][3], PointVVLH[3], R_Co5_Norm, PointVVLHNorm[3], DeltaPointLVLH[3];
	// 计算激光向量在VVLH坐标系下指向
	mtxT((double*)MtxBody2VVLH, (double*)MtxVVLH2Body, 3, 3);
	mtxMtp((double*)PointVVLH, (double*)MtxBody2VVLH, 3, 3, (double*)PointBody, 3, 1);
	// 计算激光向量在LVLH面内投影
	R_Co5_Norm = norm(R_Co5, 3);
	PointVVLHNorm[0] = PointVVLH[0] * R_Co5_Norm;
	PointVVLHNorm[1] = PointVVLH[1] * R_Co5_Norm;
	PointVVLHNorm[2] = PointVVLH[2] * R_Co5_Norm;
	mtxMtp((double*)PointLVLH, (double*)MtxVVLH2LVLH, 3, 3, (double*)PointVVLHNorm, 3, 1);
	mtxSub(DeltaPointLVLH, RLVLH, PointLVLH, 3, 1);
	// 判断激光在LVLH面内投影是否落下卫星本体上
	// 卫星尺寸 本体1720 * 2400mm  三角长度1370mm  帆板1680 * 6000mm
	double a_Body = 1720 / 2 * 1e-3;
	double b_Body = 2400 / 2 * 1e-3;
	double c_Body = 2200 / 2 * 1e-3;
	double a_sanjiao = 1370 * 1e-3;
	double a_Left = 6000 / 2 * 1e-3;
	double b_Left = 1600 / 2 * 1e-3;
	int Flag_Body, Flag_Left;
	if ( DisCloseFlag == 1 ) {
		// 激光是否落在本体上
		if ((fabs(DeltaPointLVLH[2]) <= a_Body) && (fabs(DeltaPointLVLH[0]) <= b_Body)) {
			Flag_Body = 1;
			Dis_Close_Measure = Dis_Close_Measure - c_Body;
		}
		else {
			Flag_Body = 0;
		}
		// 激光落在帆板上
		if ((fabs(DeltaPointLVLH[2]) >= a_Body) && (fabs(DeltaPointLVLH[2]) <= (a_Body + a_sanjiao + a_Left)) && ((fabs(DeltaPointLVLH[0]) <= b_Left))) {
			Flag_Left = 1;
		}
		else {
			Flag_Left = 0;
		}
		if (Flag_Body == 1 || Flag_Left == 1) {
			DisCloseFlag = 1;
		}
		else {
			DisCloseFlag = 0;
		}
	}

	// 返回结果数组
	Dis_Close[0] = DisCloseFlag;
	Dis_Close[1] = Dis_Close_Measure;
}