/*
 * motor.c
 *
 *  Created on: 2026年4月20日
 *      Author: Lizongdi
 */
#ifdef BUILD_CANMOTOR

#include "motor.h"

TMotorCtrl *g_leftmotor;
TMotorCtrl *g_rightmotor;
TMotorCtrl *g_swingmotor;

char a[10];
int32_t fluat_flag=0;

static void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length)
{

	memcpy(a,buffer,8);
    fluat_flag = canID - 0x580;
	switch (fluat_flag)
	{
		case 1:
		{
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID1_LeftMotor", 1);
			MotorGetState(g_leftmotor, buffer);
		}
			break;
		case 2:
		{
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID2_RightMotor", 1);
			MotorGetState(g_rightmotor, buffer);
		}
			break;
		case 3:
		{
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "ZQ_CAN_ID3_SwingMotor", 1);
			MotorGetState(g_swingmotor, buffer);
		}
			break;
	}

}

static void MotorCommandsLoop()
{
    static char TT_Motor_Need_To_Activate = 1;
    WheelSetVelcity(g_leftmotor, GV.Left_Speed_M_min);
    WheelSetVelcity(g_rightmotor, GV.Right_Speed_M_min);

    if (TT_Motor_Need_To_Activate == 1)
	{
		WheelFuncInit(g_leftmotor, g_rightmotor);
	    TT_Motor_Need_To_Activate = 0;
	}
	else
	{
        WheelFuncProc(g_leftmotor, g_rightmotor);
	}	
}

static void MotorCommandsLoop_2_Position()
{
    static char TT_Motor_Need_To_Activate_1 = 1;
    SwingSetPosition(g_swingmotor, GV.Tar_Position_angle, GV.Tar_Position_Velcity_Degree_S);

	if (TT_Motor_Need_To_Activate_1 == 1)
	{
	    SwingFuncInit(g_swingmotor);
		TT_Motor_Need_To_Activate_1 = 0;
	}
	else
	{
	    SwingFuncProc(g_swingmotor);
	}
}

static TMotorCtrl *MotorControllerInit(int32_t MotorID, EMotorType MotorType, 
    int32_t HeartbeatID, FDCANHandler *fHandler)
{
    TMotorCtrl *ptMotorCtrl = malloc(sizeof(TMotorCtrl));

    if (ptMotorCtrl == NULL)
    {
        return NULL;
    }
    
    ptMotorCtrl->pMotorParam = malloc(sizeof(TT_MotorParameters));

    if (ptMotorCtrl->pMotorParam == NULL)
    {
        return NULL;
    }
    
    if (M_TYPE_MOVELEFT == MotorType)
    {
        ptMotorCtrl->pMotorParam = &GV.LeftMotor;  
        HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID1_LeftMotor",  1, ComError_ZQ_CAN_ID1_LeftMotor);
    }
    else if (M_TYPE_MOVERIGHT == MotorType)
    {
        ptMotorCtrl->pMotorParam = &GV.RightMotor;
        HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID2_RightMotor", 1, ComError_ZQ_CAN_ID2_RightMotor);
    }
    else if (M_TYPE_SWING == MotorType)
    {
        ptMotorCtrl->pMotorParam = &GV.SwingMotor; 
        HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID3_SwingMotor", 1, ComError_ZQ_CAN_ID3_SwingMotor);
    }
    
    ptMotorCtrl->pMotorParam->MotorID = MotorID;  
    ptMotorCtrl->MotorID = MotorID;
    ptMotorCtrl->MotorType = MotorType;
    ptMotorCtrl->HeartbeatID = HeartbeatID;
    ptMotorCtrl->fHandler = fHandler;
    
	ptMotorCtrl->fHandler->CAN_Decode = Roughening_MotorDecodeCAN;    
	ptMotorCtrl->fHandler->dispacherController->DispacherCallTime = 2;

    if (M_TYPE_SWING == MotorType)
    {
        ptMotorCtrl->fHandler->dispacherController->Add_Dispatcher_List(ptMotorCtrl->fHandler->dispacherController, MotorCommandsLoop_2_Position);
    }
    else if (M_TYPE_MOVELEFT == MotorType || M_TYPE_MOVERIGHT == MotorType)
    {
        ptMotorCtrl->fHandler->dispacherController->Add_Dispatcher_List(ptMotorCtrl->fHandler->dispacherController, MotorCommandsLoop);
    }
    
    return ptMotorCtrl;
}

void MotorInit(void)
{
    g_leftmotor = MotorControllerInit(1, M_TYPE_MOVELEFT, 0x707, &FD_CAN_1_Handler);
    g_rightmotor = MotorControllerInit(2, M_TYPE_MOVERIGHT, 0x708, &FD_CAN_1_Handler);
    g_swingmotor = MotorControllerInit(3, M_TYPE_SWING, 0x709, &FD_CAN_2_Handler);
}

#endif