#include "msp_Angle_Encoder_YL159.h"
#include "BHBF_ROBOT.h"
#include "bsp_decode_command.h"
#include "gpio.h"
#include "bsp_Log.h"
#include "BSP/bsp_UART.h"

#include <msp_WH_LTE_7S0.h>


char NeedToFeedBackToComputer = 0;

struct UARTHandler *wh_LTE_7S0_Handler;
DispacherController* wh_LTE_7S0_dispacher;


void Reset()
{
	HAL_GPIO_WritePin(S0_RESET_GPIO_Port, S0_RESET_Pin, GPIO_PIN_RESET);
}
void NormalState()
{
	HAL_GPIO_WritePin(S0_RESET_GPIO_Port, S0_RESET_Pin, GPIO_PIN_SET);
}
void WH_LTE_7S0_intialize(struct UARTHandler *Handler)
{
	//NormalState();
	HAL_GPIO_WritePin(S0_RESET_GPIO_Port, GPIO_PIN_10, GPIO_PIN_SET);
	wh_LTE_7S0_Handler = Handler;
	wh_LTE_7S0_Handler->Wait_time = 30; //  最低不要低于4；
	wh_LTE_7S0_dispacher=Handler->dispacherController;
	wh_LTE_7S0_dispacher->Add_Dispatcher_List(wh_LTE_7S0_dispacher,UpdateGV);
	wh_LTE_7S0_dispacher->DispacherCallTime=600;
	//DispacherController* disp=wh_LTE_7S0_Handler->dispacherController;

	//disp->Add_Dispatcher_List(disp,UpdateGV);//周期性update GV数据

	LOG("WH_LTE_7S0_intialize");

	 wh_LTE_7S0_Handler->UART_Decode = decode_received_data_from_computer; //indicate that there is no need to listen

}

void Send_WH_LTE_7S0_Data(uint8_t *data, int length)
{

	char datass[100];
	memcpy(datass, data, length);
	wh_LTE_7S0_Handler->UART_Decode = decode_received_data_from_computer;
	memcpy(wh_LTE_7S0_Handler->Tx_Buf, data, length);
	wh_LTE_7S0_Handler->TxCount = length;
	wh_LTE_7S0_Handler->UART_Tx(wh_LTE_7S0_Handler);

}



void UpdateGV()
{

	//wh_LTE_7S0_Handler->UART_Decode = decode_received_data_from_computer;
	pb_ostream_t GV_o_stream =
	{ 0 };
	char buf[1024];
	GV_o_stream = pb_ostream_from_buffer(buf, sizeof(buf));
	pb_encode(&GV_o_stream, GV_struct_define_fields, &GV);


	memcpy(&wh_LTE_7S0_Handler->Tx_Buf[2], buf, GV_o_stream.bytes_written);
	wh_LTE_7S0_Handler->Tx_Buf[0]='1';
	wh_LTE_7S0_Handler->Tx_Buf[1]=',';
	//wh_LTE_7S0_Handler->Tx_Buf[2]='1';
	wh_LTE_7S0_Handler->TxCount = GV_o_stream.bytes_written+2;
	wh_LTE_7S0_Handler->UART_Tx(wh_LTE_7S0_Handler);
}

void decode_received_data_from_computer(uint8_t *buffer, uint16_t length)
{
	char buf[1024];
	memcpy(buf, buffer, length);
	if(buffer[0]=='2'&&buffer[1]==',')
	{
		//Decode protobuf
		//decode_command_from_computer(buffer+2,length-2);
		decode_command_and_feedback(buffer+2,length-2,1,wh_LTE_7S0_Handler);

	}
	if(NeedToFeedBackToComputer==1)
	{
		send_received_data_to_upper_computer(buffer, length);
	}

}