/*
 * msp_DC_CAN.c
 *
 *  Created on: 2024年6月21日
 *      Author: SQ-X
 */
#include <msp_DC_CAN_SWING_Controller.h>
#include "bsp_FDCAN.h"
#include "BHBF_ROBOT.h"
static DHCANopenData0x2E4   dHCANopenData0x2E4;

DH_CAN_Remote* DH_CAN;

int32_t * Send_Left_Compensation;
int32_t * Send_Right_Compensation;
int32_t * Send_BackwardDistance;
int32_t * Send_LaneChangeDistance;
int32_t * Send_Swing_Speed;
int32_t * Send_Swing_Angle;
int32_t * Send_Move_Speed;
int32_t * Send_Robot_Angle;

int32_t* SetSpeed[10];

int32_t* Robot_Move_Speed;
FDCANHandler *DH_CAN_Remote_Controller_Handler;
HardWareController *hardController;
DispacherController *dispacherController;

void DH_CAN_Remote_Controller_intialize(FDCANHandler *Handler)
{

	DH_CAN_Remote_Controller_Handler = Handler;
	DH_CAN_Remote_Controller_Handler->CAN_Decode = DecodeCAN;


	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,
			"DH_CAN_Remote_Controller_1E4", 0,DH_CAN_Remote_Controller_1E4);
	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,
			"DH_CAN_Remote_Controller_2E4", 0,DH_CAN_Remote_Controller_2E4);
    //周期性运行
	dispacherController = Handler->dispacherController;
	dispacherController->Add_Dispatcher_List(dispacherController,
			SendRobtDataToController_0x364);
	dispacherController->Add_Dispatcher_List(dispacherController,
			SendRobtDataToController_0x264);


}

void DecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length)
{
	//buffer[0] is CAN ID;

	int DH_40022_Flag = (int) canID;
	switch (DH_40022_Flag)
	{
		case 0x701:   			//心跳700 + NodeID1
			//DC_Decoding_Func();
			break;
		case 0x702:   			//心跳700 + NodeID2
			//DC_Decoding_Func();
			break;
		case 0x1E4:   			//1E4 = 484
		{
			DH_CAN->Front_Back_Move_Value = CAN_Buf[0];
			DH_CAN->Left_Right_Turn_Value = CAN_Buf[1];
			DH_CAN->Swing_Speed_Value = CAN_Buf[2];
			DH_CAN->Swing_Angle_Value = CAN_Buf[3];
			DH_CAN->Left_Compensation_Value = CAN_Buf[4];
			DH_CAN->Right_Compensation_Value = CAN_Buf[5];
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
					"DH_CAN_Remote_Controller_1E4", 1);
		}
			break;
		case 0x2E4:				//2E4

		{
			 memcpy(&dHCANopenData0x2E4,buffer,8);
			DH_CAN->Emergency_Stop = dHCANopenData0x2E4.EmergencyStop;
			DH_CAN->Move_Forward = dHCANopenData0x2E4.Forwards;
			DH_CAN->Move_Backward = dHCANopenData0x2E4.BackWards;
			DH_CAN->Move_Turn_Left = dHCANopenData0x2E4.TurnLeft;
			DH_CAN->Move_Turn_Right = dHCANopenData0x2E4.TurnRight;

			*Robot_Move_Speed = GetSpeed(dHCANopenData0x2E4);

			DH_CAN->Mode_Selection = GetMode(dHCANopenData0x2E4);

			DH_CAN->Work_Start = dHCANopenData0x2E4.TaskStart;

			DH_CAN->Increase = dHCANopenData0x2E4.Incresss;
			DH_CAN->Decrease = dHCANopenData0x2E4.Decresss;

			DH_CAN->Lane_Change = dHCANopenData0x2E4.LaneChnge;//换道
			DH_CAN->UpWards = dHCANopenData0x2E4.UpWards;
			DH_CAN->DownWards = dHCANopenData0x2E4.DownWards;

			DH_CAN->Swing_Left = dHCANopenData0x2E4.SwingLeft;
			DH_CAN->Swing_Right = dHCANopenData0x2E4.SwingRight;
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
					"DH_CAN_Remote_Controller_2E4", 1);
		}

			break;
		default:
			break;
	}

}
uint8_t data_to_Send[8] =
{ 1, 2, 3, 4, 5, 6, 7, 8 };

uint16_t data_temp;
void SendRobtDataToController_0x364()				//Send 换道距离, 0x364
{

	memset(data_to_Send, 0, sizeof(data_to_Send));

	data_temp = (uint16_t)(*Send_Left_Compensation);
	memcpy(&data_to_Send[0], &data_temp, 2);				//左补偿
	data_temp = (uint16_t) (*Send_Right_Compensation);
	memcpy(&data_to_Send[2], &data_temp, 2);				//右补偿
	data_temp = (uint16_t) (*Send_BackwardDistance);
	memcpy(&data_to_Send[4], &data_temp, 2);				//后退距离
	data_temp = (uint16_t) (*Send_LaneChangeDistance);
	memcpy(&data_to_Send[6], &data_temp, 2);				//换道距离

	DH_CAN_Remote_Controller_Handler->CAN_Send(DH_CAN_Remote_Controller_Handler,
			0X364, 8, data_to_Send);
	//DH_CAN_Remote_Controller_Handler->CAN_Send(DH_CAN_Remote_Controller_Handler,10,8,data);
}
void SendRobtDataToController_0x264()
{
	memset(data_to_Send, 0, sizeof(data_to_Send));
	//data_temp=(uint16_t)GV.DH_CAN.Send_Angle;
	data_temp = (uint16_t) (*Send_Swing_Speed);
	memcpy(&data_to_Send[0], &data_temp, 2);	//摆速
	data_temp = (uint16_t) (*Send_Swing_Angle);
	memcpy(&data_to_Send[2], &data_temp, 2);	//摆角
	data_temp = (uint16_t) (*Send_Move_Speed);
	memcpy(&data_to_Send[4], &data_temp, 2);	//速度
	data_temp = (uint16_t) (*Send_Robot_Angle);//需要知道是不是这个东西
	memcpy(&data_to_Send[6], &data_temp, 2);	//角度

	//DH_40022_FDCANfunction(0X264, 8,data);
	DH_CAN_Remote_Controller_Handler->CAN_Send(DH_CAN_Remote_Controller_Handler,
			0X264, 8, data_to_Send);
}


int32_t GetSpeed(DHCANopenData0x2E4   dHCANopenData0x2E4)
{
	if(dHCANopenData0x2E4.Speed1==1) {return *SetSpeed[0];}
	if(dHCANopenData0x2E4.Speed2==1) {return *SetSpeed[1];}
	if(dHCANopenData0x2E4.Speed3==1) {return *SetSpeed[2];}
	if(dHCANopenData0x2E4.Speed4==1) {return *SetSpeed[3];}
	if(dHCANopenData0x2E4.Speed5==1) {return *SetSpeed[4];}
	if(dHCANopenData0x2E4.Speed6==1) {return *SetSpeed[5];}
	if(dHCANopenData0x2E4.Speed7==1) {return *SetSpeed[6];}
	if(dHCANopenData0x2E4.Speed8==1) {return *SetSpeed[7];}
	if(dHCANopenData0x2E4.Speed9==1) {return *SetSpeed[8];}
	if(dHCANopenData0x2E4.Speed10==1) {return *SetSpeed[9];}
	return 0;
}
int32_t GetMode(DHCANopenData0x2E4   dHCANopenData0x2E4)
{

		if(dHCANopenData0x2E4.Mode0==1) {return 0;}
		if(dHCANopenData0x2E4.Mode1==1) {return 1;}
		if(dHCANopenData0x2E4.Mode2==1) {return 2;}
		if(dHCANopenData0x2E4.Mode3==1) {return 3;}
		if(dHCANopenData0x2E4.Mode4==1) {return 4;}
		if(dHCANopenData0x2E4.Mode5==1) {return 5;}
		if(dHCANopenData0x2E4.Mode6==1) {return 6;}
		if(dHCANopenData0x2E4.Mode7==1) {return 7;}
		if(dHCANopenData0x2E4.Mode8==1) {return 8;}
		if(dHCANopenData0x2E4.Mode9==1) {return 9;}
		if(dHCANopenData0x2E4.Mode10==1) {return 10;}
		return -1;
}