/******************************************************************************

                  版权所有 (C), 2018-2099, Radkil

 ******************************************************************************
  文 件 名   : msp_MK32.c
  版 本 号   : 初稿
  作    者   : radkil
  生成日期   : 2026年6月8日
  最近修改   :
  功能描述   : MK32遥控器
  
  修改历史   :
  1.日    期   : 2026年6月8日
    作    者   : radkil
    修改内容   : 创建文件

******************************************************************************/
#include "msp_MK32.h"
#include "BHBF_robot.h"
#include <math.h>


/*----------------------------------------------*
 * 外部变量说明                                 *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 外部函数原型说明                             *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 内部函数原型说明                             *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 全局变量                                     *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 模块级变量                                   *
 *----------------------------------------------*/
static SP_MSP_MK32_Button RB_MK32;

/*----------------------------------------------*
 * 常量定义                                     *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 宏定义                                       *
 *----------------------------------------------*/

static int check_MK32Data(char *_pBuffer, uint32_t _iSize)
{
    if (_pBuffer[0] != 0x0f) 
    {
        return -1; 
    }
    if (_pBuffer[24] != 0x0f)
    {
        return -1; 
    }
    else
    {
        return 25;
    }
}

static void decode_MK32Data(const char *buf, uint32_t _iSize)
{
    int16_t CH[16];
	int Start_byte = -1;

	CH[0] = ((buf[Start_byte + 1] | buf[Start_byte + 2] << 8) & 0x07FF);
	CH[1] = ((buf[Start_byte + 2] >> 3 | buf[Start_byte + 3] << 5) & 0x07FF);
	CH[2] = ((buf[Start_byte + 3] >> 6 | buf[Start_byte + 4] << 2
			| buf[Start_byte + 5] << 10) & 0x07FF);
	CH[3] = ((buf[Start_byte + 5] >> 1 | buf[Start_byte + 6] << 7) & 0x07FF);
	CH[4] = ((buf[Start_byte + 6] >> 4 | buf[Start_byte + 7] << 4) & 0x07FF);
	CH[5] = ((buf[Start_byte + 7] >> 7 | buf[Start_byte + 8] << 1
			| buf[Start_byte + 9] << 9) & 0x07FF);
	CH[6] = ((buf[Start_byte + 9] >> 2 | buf[Start_byte + 10] << 6) & 0x07FF);
	CH[7] = ((buf[Start_byte + 10] >> 5 | buf[Start_byte + 11] << 3) & 0x07FF);
	CH[8] = ((buf[Start_byte + 12] | buf[Start_byte + 13] << 8) & 0x07FF);
	CH[9] = ((buf[Start_byte + 13] >> 3 | buf[Start_byte + 14] << 5) & 0x07FF);
	CH[10] = ((buf[Start_byte + 14] >> 6 | buf[Start_byte + 15] << 2
			| buf[Start_byte + 16] << 10) & 0x07FF);
	CH[11] = ((buf[Start_byte + 16] >> 1 | buf[Start_byte + 17] << 7) & 0x07FF);
	CH[12] = ((buf[Start_byte + 17] >> 4 | buf[Start_byte + 18] << 4) & 0x07FF);
	CH[13] = ((buf[Start_byte + 18] >> 7 | buf[Start_byte + 19] << 1
			| buf[Start_byte + 20] << 9) & 0x07FF);
	CH[14] = ((buf[Start_byte + 20] >> 2 | buf[Start_byte + 21] << 6) & 0x07FF);
	CH[15] = ((buf[Start_byte + 21] >> 5 | buf[Start_byte + 22] << 3) & 0x07FF);

	int32_t* channel_ptrs[16] =
	{
		&RB_MK32.CH0_RY_H, &RB_MK32.CH1_RY_V, &RB_MK32.CH2_LY_V, &RB_MK32.CH3_LY_H,
		&RB_MK32.CH4_SA,   &RB_MK32.CH5_SB,   &RB_MK32.CH6_SC,   &RB_MK32.CH7_SD,
		&RB_MK32.CH8_SE,   &RB_MK32.CH9_SF,   &RB_MK32.CH10_LD1, &RB_MK32.CH11_RD1,
		&RB_MK32.CH12_S1,  &RB_MK32.CH13_S2,  &RB_MK32.CH14_LT,  &RB_MK32.CH15_RT
	};

	// 按键数值转换：1050为中间值，272-1712
	for (int i = 0; i < 16; i++)
	{
		// 通过指针解引用赋值，完美替代原来的 RB_MK32[i + 1]
		*channel_ptrs[i] = (int32_t)((CH[i] - 992) * 1.388889);
	}

	// 【核心修改】：通过具体的成员名访问 IsOnline，替代原来的 RB_MK32[17]
	if (buf[22] == 0)
	{
		RB_MK32.IsOnline = 1;
	}
	else
	{
		RB_MK32.IsOnline = 0;
	}
	RB_MK32.RxIndex++;
}

void MK32_Init(void)
{
    TUartUserData *ptUartUserData = UART_userdata_init(5, 115200, 512);    
    g_ptE28_SBUS = rd_ComCreate(check_MK32Data, decode_MK32Data, E28_SBUS_Send, ptUartUserData->m_buf_size, ptUartUserData);
    UART_IT_init(g_ptE28_SBUS);
}

GF_CMD MK32_Task(int mode)
{
    int angle;
	angle = atan2(RB_MK32.CH1_RY_V, RB_MK32.CH0_RY_H) * 180 / M_PI;

	if(RB_MK32.CH5_SB == 0)
	{
		// 前进
		if((angle >= 45) && (angle <= 135))
		{
		    if (0 == mode) return GF_CMD_MANUAL_FORWARD;
			else return GF_CMD_AUTO_FORWARD;
		}
		// 后退
		else if((angle >= -135) && (angle < -45))
		{
			return GF_CMD_MANUAL_BACKWARD;
		}
		else 
        {
            return GF_CMD_STOP_ALL;
		}
	}
	else
    {
        // 前进
		if((angle >= 45) && (angle <= 135))
		{
			if (0 == mode) return GF_CMD_MANUAL_FORWARD;
			else return GF_CMD_AUTO_FORWARD;
		}
		// 后退
		else if((angle >= -135) && (angle < -45))
		{
			return GF_CMD_MANUAL_BACKWARD;
		}
		else 
        {
			return GF_CMD_STOP_ALL;
		}
	}
}