/*
 * msp_DH_W_H.c
 *
 *  Created on: Dec 18, 2023
 *      Author: Administrator
 */


/*
 * msp_DH_W_H.c
 *
 *  Created on: Dec 14, 2023
 *      Author: Administrator
 */

#include "MSP/msp_DH_W_H.h"

UART_HandleTypeDef* Huart_DH_WH;
uint8_t DH_stas[20];
SP_MSP_DH_Handset_Parameters *DH_WH_BING;
int32_t DH_Falg=1;
void  GF_DH_Hangset_Unscramble_Callback();
void DH_Command_Comp(uint8_t *buf);
void DH_Command_Comp_Sub(uint8_t *buf_Sub);


unsigned short ModBusCRC16(unsigned char *puchMsg,unsigned short usDataLen);

uint8_t GF_MSP_DH_Wash_H_Init(UART_HandleTypeDef* huart,SP_MSP_DH_Handset_Parameters *_DH_Handset_W_H)
{


//	uint8_t buf_11[8] = {0x03,0x04,0x00,0x00,0x00,0x06};
//	DATATA=ModBusCRC16(buf_11, 6);  CRC校验

	DH_Falg=1;
	Huart_DH_WH=huart;
	GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_RS485_1_RxCpltCallback, GF_DH_Hangset_Unscramble_Callback);//这句话的理解有问题
	HAL_UART_Receive_IT(Huart_DH_WH,DH_stas,1);
	DH_WH_BING = _DH_Handset_W_H;
	return 1;
}

uint8_t Inquiry_Flag=0;
int ii=1;
void GF_MSP_DH_Stu_Inquiry(int16_t Velocity_Value,int16_t Angle_Val,int16_t Current_Val)
{
	uint8_t Inquiry_Order[8]={0X03, 0X04, 0X00, 0X00, 0X00, 0X08, 0XF0, 0X2E};
	uint8_t Status_Order[33]={0X03, 0X10, 0X00 , 0X00 , 0X00 , 0X0C , 0X18 , 0X00 , 0X02 ,
			0X00 , 0X02 , 0X00 , 0X06 , 0XCB , 0XB1 , 0XCF , 0XBA , 0XCB , 0XB1 , 0XBD , 0XB7 , 0XFA , 0XBB , 0XF7 , 0XC6 , 0XCB , 0XC8 , 0XAB , 0XB9 , 0XBE , 0XCB , 0XEA , 0XB0};
//	uint8_t Robot_Velcity[2]={0X00, 0X02};//速度赋值位置
//	uint8_t Robot_Angle[2]={0X00, 0X02};  // 角度赋值位置
//	uint8_t Robot_Current[2]={0X00, 0X01};// 电流幅值位置
	unsigned short CRC_Value;
	switch(Inquiry_Flag)
	{
	case 0:case 1: case 2: case 3: case 4:case 5: case 6: case 7: case 8:case 9: case 10:
	GF_BSP_UART_Transmit(1, Inquiry_Order, 8);//这句话的关联有问题
	Inquiry_Flag++;
		break;
	case 11:
//	 Status_Order[7]=Robot_Velcity[0];Status_Order[8]=Robot_Velcity[1];
//	 Status_Order[9]=Robot_Angle[0];Status_Order[10]=Robot_Angle[1];
//	 Status_Order[11]=Robot_Current[0];Status_Order[12]=Robot_Current[1];

	Status_Order[7]=(uint8_t)(Velocity_Value>>8); Status_Order[8]=(uint8_t)(Velocity_Value);
	Status_Order[9]=(uint8_t)(Angle_Val>>8); Status_Order[10]=(uint8_t)(Angle_Val);
	Status_Order[11]=(uint8_t)(Current_Val>>8); Status_Order[12]=(uint8_t)(Current_Val);

//	Status_Order[8]=(uint8_t)ii++;
//	 if (ii>100)
//	 {
//	 ii=0;
//	 }



	 CRC_Value=ModBusCRC16(Status_Order, 31); // CRC校验
	 Status_Order[31]= (uint8_t) CRC_Value;
	 Status_Order[32]= (uint8_t)( CRC_Value>>8);
	 GF_BSP_UART_Transmit(1, Status_Order, 33);//这句话的关联有问题
	 Inquiry_Flag=0;
		break;
	}
}

void  GF_DH_Hangset_Unscramble_Callback()
{
	switch(DH_Falg)
	{
	case 1:
		if(DH_stas[0]==0X03)
		{
		HAL_UART_Receive_IT(Huart_DH_WH,DH_stas,1);
		DH_Falg=2;
		}
		else
		{
		HAL_UART_Receive_IT(Huart_DH_WH,DH_stas,1);
		DH_Falg=1;
		}
		break;
	case 2:
		if(DH_stas[0]==0X04)
		{
		HAL_UART_Receive_IT(Huart_DH_WH,DH_stas,1);
		DH_Falg=3;
		}
		else
		{
		HAL_UART_Receive_IT(Huart_DH_WH,DH_stas,1);
		DH_Falg=1;
		}
		break;
	case 3:
		if(DH_stas[0]==0X10)
		{
		HAL_UART_Receive_IT(Huart_DH_WH,DH_stas,18);
		DH_Falg=4;
		}
		else
		{
		HAL_UART_Receive_IT(Huart_DH_WH,DH_stas,1);
		DH_Falg=1;
		}
		break;
	case 4:
		if(DH_stas[0]==0X00)
		{
		DH_Command_Comp(DH_stas);
		HAL_UART_Receive_IT(Huart_DH_WH,DH_stas,1);
		DH_Falg=1;
		}
		else
		{
		HAL_UART_Receive_IT(Huart_DH_WH,DH_stas,1);
		DH_Falg=1;
		}
		break;
//	case 5:
//		DH_Command_Comp(DH_stas);
//		HAL_UART_Receive_IT(Huart_DH_WH,DH_stas,1);
//		DH_Falg=1;
//		break;

	default:
		HAL_UART_Receive_IT(Huart_DH_WH,DH_stas,1);
		DH_Falg=1;
		break;
	}
}

void DH_Command_Comp(uint8_t *buf)
{
	int Start_Bit=0;

	if(((buf[Start_Bit+13])&(0X01))>0)
	{
		DH_WH_BING->EMERGENCY_STOP=1;
	}
	else
	{
		DH_WH_BING->EMERGENCY_STOP=0;
	}

	if(((buf[Start_Bit+13])&(0X02))>0)
	{
		DH_WH_BING->Wireless_Status=1;
	}
	else
	{
		DH_WH_BING->Wireless_Status=0;
	}


	DH_WH_BING->Mob_Velocity=(int32_t)(buf[9]);
	DH_WH_BING->Cleaning_swing=(int32_t)(buf[11]);


	if(((buf[Start_Bit+13])&(0X04))>0)
	{
		DH_WH_BING->For_Back_Value_X=(int32_t)(buf[1]);
	}
	else if(((buf[Start_Bit+13])&(0X08))>0)
	{
		DH_WH_BING->For_Back_Value_X=(int32_t)(buf[1])*-1;

	}
	else
	{
		DH_WH_BING->For_Back_Value_X=0;
	}

	if(((buf[Start_Bit+13])&(0X10))>0)
	{
		DH_WH_BING->Left_Right_Value_Y=(int32_t)(buf[3]);
	}
	else if(((buf[Start_Bit+13])&(0X20))>0)
	{
		DH_WH_BING->Left_Right_Value_Y=(int32_t)(buf[3])*-1;
	}
	else
	{
		DH_WH_BING->Left_Right_Value_Y=0;
	}


	if(((buf[Start_Bit+13])&(0X40))>0)
	{
		DH_WH_BING->Rise_down_X=(int32_t)(buf[5])*1;
	}
	else if(((buf[Start_Bit+13])&(0X80))>0)
	{
		DH_WH_BING->Rise_down_X=(int32_t)(buf[5])*-1;
	}
	else
	{
		DH_WH_BING->Rise_down_X=0;
	}

	if(((buf[Start_Bit+12])&(0X01))>0)
	{
		DH_WH_BING->Turn_Left_Right_Y=(int32_t)(buf[7]);
	}
	else if(((buf[Start_Bit+12])&(0X02))>0)
	{
		DH_WH_BING->Turn_Left_Right_Y=(int32_t)(buf[7])*-1;
	}
	else
	{
		DH_WH_BING->Turn_Left_Right_Y=0;
	}


	if(((buf[Start_Bit+12])&(0X04))>0)
	{
		DH_WH_BING->Clean_Fire1_HighPres2=1;
	}
	else if(((buf[Start_Bit+12])&(0X08))>0)
	{
		DH_WH_BING->Clean_Fire1_HighPres2=2;
	}
	else
	{
		DH_WH_BING->Clean_Fire1_HighPres2=0;
	}


	if(((buf[Start_Bit+12])&(0X10))>0)
	{
		DH_WH_BING->Mov_Horiz1_vert2=1;
	}
	else if(((buf[Start_Bit+12])&(0X20))>0)
	{
		DH_WH_BING->Mov_Horiz1_vert2=2;
	}
	else
	{
		DH_WH_BING->Mov_Horiz1_vert2=0;
	}


	if(((buf[Start_Bit+12])&(0X40))>0)
	{
		DH_WH_BING->Mode_A1_B2=1;
	}
	else if(((buf[Start_Bit+12])&(0X80))>0)
	{
		DH_WH_BING->Mode_A1_B2=2;
	}
	else
	{
		DH_WH_BING->Mode_A1_B2=0;
	}


	if(((buf[Start_Bit+15])&(0X01))>0)
	{
		DH_WH_BING->Mode_C1_D2=1;
	}
	else if(((buf[Start_Bit+15])&(0X02))>0)
	{
		DH_WH_BING->Mode_C1_D2=2;
	}
	else
	{
		DH_WH_BING->Mode_C1_D2=0;
	}


	if(((buf[Start_Bit+15])&(0X04))>0)
	{
		DH_WH_BING->Scan_horiz1_vert2=1;
	}
	else if(((buf[Start_Bit+15])&(0X08))>0)
	{
		DH_WH_BING->Scan_horiz1_vert2=2;
	}
	else
	{
		DH_WH_BING->Scan_horiz1_vert2=0;
	}

}





const unsigned char auchCRCHi[] = {
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01,
0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81,
0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01,
0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01,
0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01,
0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
0x40
};


const unsigned char auchCRCLo[] = {
0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4,
0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,
0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD,
0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7,
0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,
0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE,
0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2,
0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,
0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB,
0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 0x50, 0x90, 0x91,
0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88,
0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80,
0x40
};


unsigned short ModBusCRC16(unsigned char *puchMsg,unsigned short usDataLen)
{
    unsigned char uchCRCHi = 0xFF ; /* high byte of CRC initialized */
    unsigned char uchCRCLo = 0xFF ; /* low byte of CRC initialized */
    unsigned char uIndex ;               /* will index into CRC lookup table */
    while (usDataLen--)             /* pass through message buffer */
    {
        uIndex = uchCRCHi ^ *puchMsg++ ; /* calculate the CRC */
        uchCRCHi = uchCRCLo ^ auchCRCHi[uIndex] ;
        uchCRCLo = auchCRCLo[uIndex] ;
    }
    return (uchCRCLo << 8 | uchCRCHi) ;
}





