/*
 * bsp_decode_command.c
 *
 *  Created on: Sep 24, 2024
 *      Author: akeguo
 */
#include "bsp_decode_command.h"
#include "bsp_Error.pb.h"
#include "main.h"

#include "pb.h"
#include "pb_encode.h"
#include "pb_common.h"
#include "fsm_state.h"
#include "motors_power_action.h"
#include "robot_move_actions.h"

#include "fsm_state_control.h"
#include "bsp_UDP.h"

#define success 1
#define fail 0

Cmd decoded_Cmd = Cmd_init_default;
ReCmd send_Cmd = ReCmd_init_default;
pb_istream_t i_stream =
{ 0 };
char StartDownLoadFlag = 0;
static uint32_t downloadCount = 0;
static uint8_t ReceivedData[1024];
static uint16_t calbriation;

void decode_command_and_feedback(uint8_t *buffer, uint16_t length, char sendWay,
        struct UARTHandler *send_Handler)
{

//	char array[1000];
//	memset(array, 0, sizeof(array));
//	memcpy(array, buffer, length);
//
//	if (length >= 1)
//	{
//
//		i_stream = pb_istream_from_buffer(buffer, length);
//
//		pb_decode(&i_stream, Cmd_fields, &decoded_Cmd);
//
//		memcpy(&decoded_Cmd.Buff_Data,
//		        &buffer[length - decoded_Cmd.Buff_Data_Length],
//		        decoded_Cmd.Buff_Data_Length);
//
//		switch (decoded_Cmd.CommadNum)
//		{
//
//			case 17: // 上位机获取控制权
//			{
//
//				switch (decoded_Cmd.Parameter0)
//				{
//					case 0:
//					{
//
//						//read CV
//						CV = GF_BSP_EEPROM_Get_CV(); //重新配置CV
//						is_upper_computer_take_over_control = 0;
//
//						WrapInCmdAndSendMessage(send_Cmd, 10, success,
//						        "return contorl  to Remote Controller success",
//						        sendWay, send_Handler);
//						fsm_state_set(&current_robot_move_state,
//						        &robot_halt_state);
//						//设置电机供电为 On
//						//fsm_state_set(&current_motor_power_state, &motor_power_off_state);
//						break;
//					}
//					case 1:
//					{
//
//						is_upper_computer_take_over_control = 1;
//						WrapInCmdAndSendMessage(send_Cmd, 17, success,
//						        "Take control success", sendWay, send_Handler);
//						fsm_state_set(&current_motor_power_state,
//						        &motor_power_on_state);
//						fsm_state_set(&current_robot_move_state,
//						        &robot_halt_state);
//						GV.Robot_Move_Speed = 1000;
//						CV.LeftTurnSpeed = 1;
//						CV.RightTurnSpeed = 1;
//						break;
//					}
//
//					default:
//						break;
//
//				}
//
//				break;
//			}
//			case 16: // 上位机手动控制机器人运动
//			{
//
//				switch (decoded_Cmd.Parameter0)
//				{
//					case 0:
//					{
//
//						WrapInCmdAndSendMessage(send_Cmd, 16, success,
//						        "manual control halt success", sendWay,
//						        send_Handler);
//						fsm_state_set(&current_robot_move_state,
//						        &robot_halt_state);
//						break;
//					}
//					case 1:
//					{
//						WrapInCmdAndSendMessage(send_Cmd, 16, success,
//						        "manual control forward success", sendWay,
//						        send_Handler);
//						fsm_state_set(&current_robot_move_state,
//						        &robot_forwards_state);
//						break;
//					}
//					case 2:
//					{
//						WrapInCmdAndSendMessage(send_Cmd, 16, success,
//						        "manual control backward success", sendWay,
//						        send_Handler);
//						fsm_state_set(&current_robot_move_state,
//						        &robot_backwards_state);
//						break;
//					}
//					case 3:
//					{
//						WrapInCmdAndSendMessage(send_Cmd, 16, success,
//						        "manual control turn left success", sendWay,
//						        send_Handler);
//						fsm_state_set(&current_robot_move_state,
//						        &robot_turn_left_state);
//						break;
//					}
//					case 4:
//					{
//						WrapInCmdAndSendMessage(send_Cmd, 16, success,
//						        "manual control turn right success", sendWay,
//						        send_Handler);
//						fsm_state_set(&current_robot_move_state,
//						        &robot_turn_right_state);
//						break;
//					}
//					default:
//						break;
//
//				}
//
//				break;
//			}
//
//			case 15: // 返回错误信息
//			{
//
//				pb_ostream_t Error_o_stream =
//				{ 0 };
//				char buf[1024];
//				Error_o_stream = pb_ostream_from_buffer(buf, sizeof(buf));
//				pb_encode(&Error_o_stream, ErrorData_fields,
//				        &GV.SystemErrorData);
//
//				//将数据拷贝到Command中
//				send_Cmd.CommadNum = 16;
//				send_Cmd.Buff_Data_Length = Error_o_stream.bytes_written;
//
//				WrapInCmdAndSend(send_Cmd, buf, sendWay, send_Handler);
//
//				break;
//			}
//
//			case 14: //这里是初始化Flash 好重新写入
//			{
//				NeedToFeedBackToComputer = 1;
//
//				WrapInCmdAndSendMessage(send_Cmd, 14, success,
//				        "disable periodical call of serialports and can",
//				        sendWay, send_Handler);
//
//				Error_Detect_Enable = 0; //disable ErrorDetect Function;
//
//				DLT_LOG_ENABLE_LEVEL = 0; //7 send all information 	  //0 send nothing
//
//				//将所有串口的周期性调用暂停
//				RS_485_1_UART_Handler.dispacherController->Dispacher_Enable = 0;
//				RS_485_2_UART_Handler.dispacherController->Dispacher_Enable = 0;
//				RS_485_3_UART_Handler.dispacherController->Dispacher_Enable = 0;
//				RS_485_4_UART_Handler.dispacherController->Dispacher_Enable = 0;
//				E28_SBUS_UART_Handler.dispacherController->Dispacher_Enable = 0;
//				LPUART1_UART_Handler.dispacherController->Dispacher_Enable = 0;
//				InterCall_DEBUG_UART_Handler.dispacherController->Dispacher_Enable =
//				        0;
//				LTE_7S0_Serial_UART_Handler.dispacherController->Dispacher_Enable =
//				        0;
//				RS_485_4_UART_Handler.dispacherController->Dispacher_Enable = 0;
//				is_udp_GV_update_loop_enalbed = 0;
//
//				//将CAN总线的周期性调用停掉
//				//因为电机有可能处于运动状态，所有CAN线不能停，得发送下去
//				//FD_CAN_1_Handler.dispacherController->Dispacher_Enable = 0;
//				//FD_CAN_2_Handler.dispacherController->Dispacher_Enable = 0;
//				fsm_state_set(&current_robot_move_state, &robot_halt_state);
//
//				break;
//			}
//			case 13: // 使能 GV 周期性性发送数据
//			{
//				if (decoded_Cmd.Parameter0 == 0) //关闭上传
//				{
//					NeedToFeedBackToComputer = 1;
//					LTE_7S0_Serial_UART_Handler.dispacherController->Dispacher_Enable =
//					        0;
//					WrapInCmdAndSendMessage(send_Cmd, 14, success,
//					        "disable send GV", sendWay, send_Handler);
//
//					is_udp_GV_update_loop_enalbed = 0;
//				}
//
//				if (decoded_Cmd.Parameter0 == 1) //打开上传
//				{
//					NeedToFeedBackToComputer = 0;
//					LTE_7S0_Serial_UART_Handler.dispacherController->Dispacher_Enable =
//					        1;
//					WrapInCmdAndSendMessage(send_Cmd, 14, success,
//					        "enable send GV", sendWay, send_Handler);
//
//					is_udp_GV_update_loop_enalbed = 1;
//				}
//
//				break;
//			}
//			case 12:  //重启系统，进入bootloader
//			{
//
//				WrapInCmdAndSendMessage(send_Cmd, 12, success, "STM32 reboot",
//				        sendWay, send_Handler);
//				NVIC_SystemReset();
//
//				break;
//			}
//			case 11: //配置无线网络模块，此处为透传指令
//			{
//				Send_WH_LTE_7S0_Data(decoded_Cmd.Buff_Data,
//				        decoded_Cmd.Parameter0);
//				break;
//			}
//			case 10:				//擦除flash
//			{
//				WrapInCmdAndSendMessage(send_Cmd, 10, success,
//				        "start erasing flash ", sendWay, send_Handler);
//				QSPI_W25Qx_EraseDownLoadFlash();
//				downloadCount = 0;
//				WrapInCmdAndSendMessage(send_Cmd, 10, success,
//				        "erase flash success", sendWay, send_Handler);
//				break;
//			}
//			case 9: //上位机下发保存软件代码指令
//			{
//				memcpy(ReceivedData,
//				        &buffer[length - decoded_Cmd.Buff_Data_Length],
//				        decoded_Cmd.Buff_Data_Length);
//
//				calbriation = MB_CRC16(&ReceivedData,
//				        decoded_Cmd.Buff_Data_Length);
//
//				if (calbriation == decoded_Cmd.Parameter4)		//Parameter4是校验
//				{
//
//					if (downloadCount == decoded_Cmd.Parameter3)
//					{
//						QSPI_W25Qx_Write_Buffer(&decoded_Cmd.Buff_Data,
//						        CODE_DOWNLOAD_FLASH_BEGIN_ADDRESS
//						                + decoded_Cmd.Parameter3,
//						        decoded_Cmd.Buff_Data_Length);
//						downloadCount += decoded_Cmd.Buff_Data_Length;
//						send_Cmd.Parameter3 = length;
//						WrapInCmdAndSendMessage(send_Cmd, 9, success,
//						        "download success", sendWay, send_Handler);
//
//					}
//					else
//					{
//						decoded_Cmd.Parameter2 = downloadCount;	//返回download count;
//						send_Cmd.Parameter3 = length;
//						WrapInCmdAndSendMessage(send_Cmd, 9, fail,
//						        "downloadCount!=Parameter3", sendWay,
//						        send_Handler);
//					}
//
//					if (decoded_Cmd.Parameter1 == 1)
//					{
//						WrapInCmdAndSendMessage(send_Cmd, 9, success,
//						        "download new software sucess", sendWay,
//						        send_Handler);
//						IAP_struct_define iap = GF_BSP_EEPROM_Get_IAP();//Get the version of the code
//						iap.UpgradeSucceeded = 1;
//						iap.Total_Bytes = decoded_Cmd.Parameter2;
//						StartDownLoadFlag = 0;
//						downloadCount = 0;
//						GF_BSP_EEPROM_Set_IAP(iap);
//
//					}
//				}
//				else
//				{
//
//					decoded_Cmd.Parameter2 = downloadCount;	//返回download count;
//					send_Cmd.Parameter3 = length;
//					WrapInCmdAndSendMessage(send_Cmd, 9, fail,
//					        "MB CRC failed and the length ", sendWay,
//					        send_Handler);
//
//				}
//
//				break;
//			}
//
////			定义 7 上位机获取拟合点
////			定义 8 上位机获取位置点 */
//			case 8:
//			{
////				send_data_to_upper_computer(*Desulfurizer_Angle,
////				        *Desulfurizer_Wire_Length, *Desulfurizer_Thickness, 0,
////				        0, desulfurizer_message_UART_Handler);
//				break;
//			}
//			case 7:
//			{
////				send_data_to_upper_computer(*Desulfurizer_Angle,
////				        *Desulfurizer_Wire_Length, *Desulfurizer_Thickness, 1,
////				        0, desulfurizer_message_UART_Handler);
//				break;
//			}
//				//定义 6 上位机设定编码器角度值为0
//			case 6:
//			{
//				//reset_current_value_0();
//				break;
//			}
//				//定义 4 上位机设定Trace等级值，无返回
//			case 4:
//			{
//
//				break;
//			}
//				//定义 3 上位机设定CV值
//			case 3:
//			{
//				CV_struct_define decoded_CV = CV_struct_define_init_default;
//				pb_istream_t i_cv_stream =
//				{ 0 };
//
//				i_cv_stream = pb_istream_from_buffer(decoded_Cmd.Buff_Data,
//				        decoded_Cmd.Buff_Data_Length);
//				pb_decode(&i_cv_stream, CV_struct_define_fields, &decoded_CV);
//				//将CV写入EEPROM
//				GF_BSP_EEPROM_Set_CV(decoded_CV);
//				CV = decoded_CV;
//				send_Cmd.Parameter3 = length;
//				WrapInCmdAndSendMessage(send_Cmd, 3, success,
//				        "Set CV succeeded", sendWay, send_Handler);
//				CV_GV_Init();
//				break;
//			}
//				//定义 1 上位机获取默认CV值, 下位机返回CV的值，
//				//定义 2 下位机返回CV值
//			case 1:
//			{
//				CV = GF_BSP_EEPROM_Get_CV();
//				pb_ostream_t CV_o_stream =
//				{ 0 };
//				char buf[1024];
//				CV_o_stream = pb_ostream_from_buffer(buf, sizeof(buf));
//				pb_encode(&CV_o_stream, CV_struct_define_fields, &CV);
//
//				//将数据拷贝到Command中
//				send_Cmd.CommadNum = 2;
//				send_Cmd.Buff_Data_Length = CV_o_stream.bytes_written;
//
//				WrapInCmdAndSend(send_Cmd, buf, sendWay, send_Handler);
//
//				break;
//			}
//			default:
//				WrapInCmdAndSendMessage(send_Cmd, -1, fail,
//				        "Not the right command", sendWay, send_Handler);
//				break;
//
//		}
//
//	}
//	else
//	{
//
//	}

}
void WrapInCmdAndSend(ReCmd send_Cmd, uint8_t *buf, int8_t sendWay,
        struct UARTHandler *send_Handler)
{

	memcpy(send_Cmd.Buff_Data, buf, send_Cmd.Buff_Data_Length);
	pb_ostream_t ReCmd_out_stream =
	{ 0 };

	if (sendWay == 1)				//MQTT
	{
		ReCmd_out_stream = pb_ostream_from_buffer(&send_Handler->Tx_Buf[4],
		        sizeof(send_Handler->Tx_Buf) - 4);
		pb_encode(&ReCmd_out_stream, ReCmd_fields, &send_Cmd); //encode to buffer

		send_Handler->Tx_Buf[0] = '3';
		send_Handler->Tx_Buf[1] = ',';
		send_Handler->Tx_Buf[2] = 0xfe;
		send_Handler->Tx_Buf[3] = 0xfe;
		send_Handler->TxCount = ReCmd_out_stream.bytes_written + 4;
		send_Handler->UART_Tx(send_Handler);
	}
	else if (sendWay == 2) //UDP 发送
	{
		uint8_t Tx_Buf[2048];
		ReCmd_out_stream = pb_ostream_from_buffer(&Tx_Buf[2],
		        sizeof(Tx_Buf) - 2);
		pb_encode(&ReCmd_out_stream, ReCmd_fields, &send_Cmd); //encode to buffer
		Tx_Buf[0] = 0xfe;
		Tx_Buf[1] = 0xfe;
		udp_cmd_send_back(Tx_Buf, ReCmd_out_stream.bytes_written + 2);

	}
	else //485发送
	{
		ReCmd_out_stream = pb_ostream_from_buffer(&send_Handler->Tx_Buf[2],
		        sizeof(send_Handler->Tx_Buf) - 2);
		pb_encode(&ReCmd_out_stream, ReCmd_fields, &send_Cmd); //encode to buffer
		send_Handler->Tx_Buf[0] = 0xfe;
		send_Handler->Tx_Buf[1] = 0xfe;
		//send_Handler->AddSendList(send_Handler,send_Handler->Tx_Buf,send_Handler->TxCount,NULL);
		send_Handler->TxCount = ReCmd_out_stream.bytes_written + 2;
		//send_Handler->TxCount = 200;
		send_Handler->UART_Tx(send_Handler);
	}

}

//send_Cmd.CommadNum=0xff;//代表返回信息
//send_Cmd.Parameter0 代表执行的指令，发过来的命令号;
//send_Cmd.Parameter1 1 代表成功，0 代表失败;
//send_Cmd.databuf 存的是相关信息，可以通过ACII转成string;

void WrapInCmdAndSendMessage(ReCmd send_Cmd, int8_t functionNum,
        int8_t isSuccess, uint8_t *buf, int8_t sendWay,
        struct UARTHandler *send_Handler)
{

	send_Cmd.CommadNum = 0xff; //代表返回信息
	send_Cmd.Parameter0 = functionNum; //代表返回信息
	send_Cmd.Parameter1 = isSuccess; //1 代表成功，0 代表失败;
	send_Cmd.Buff_Data_Length = strlen(buf);
	WrapInCmdAndSend(send_Cmd, buf, sendWay, send_Handler);
}

void send_received_data_to_upper_computer(uint8_t *buffer, uint16_t length)
{
	send_Cmd.CommadNum = 12; //定义12为设定Mqtt透传向上返回的数据
	send_Cmd.Buff_Data_Length = length;

	WrapInCmdAndSend(send_Cmd, buffer, 0, desulfurizer_message_UART_Handler);

}