/*
 * msp_MK32_1.c
 *
 *  Created on: Oct 8, 2024
 *      Author: akeguo
 */
#include "msp_U7.h"
#include "DLT/DLTuc.h"
#include "BHBF_ROBOT.h"
#include "fsm.h"
#include "robot_state.h"

uint8_t Reset_Flag = 0;
SP_MSP_U7_Button *P_U7;


struct UARTHandler *U7_Sbus_Controller;
void decode_U7Data(uint8_t *buffer, uint16_t length)
{
    if(length != 25)
    {
        return;
    }

    if(buffer[0] == 0x0F && buffer[24] == 0x00)
    {
        Sbus_Data_Count_U7(&buffer[1], (int32_t*)(P_U7));

        // =================== 开机复位检测 ===================
        if (Reset_Flag == 0)   // 第一次收到数据
        {
            Reset_Flag = 1;

            if(!IsRemoteAtInitialPosition())   // 如果未复位
            {
                system_mode = SYSTEM_RESET;
                CurrentMoveState = Move_Reset;
                HALT_State_Do();   // 立即停车并清空状态
            }
            else
            {
                system_mode = SYSTEM_NORMAL;   // 正常运行
            }
        }
        else
        {
            // =================== 后续检测 ===================
            if(system_mode == SYSTEM_RESET)
            {
                if(IsRemoteAtInitialPosition())   // 按键已复位
                {
                    system_mode = SYSTEM_NORMAL;
                    CurrentMoveState = Move_Resetted;
                    LOG("U7 remote reset recovered, resume NORMAL mode");
                }
                else
                {
                    // 还没复位，保持 RESET
                    HALT_State_Do();
                }
            }
        }
    }

    HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "U7_sbus", 1);
}


void Sbus_Data_Count_U7(uint8_t *buf, int32_t *But_Value)
{

	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);

	CH[16] = ((buf[Start_byte + 22] >> 5 | buf[Start_byte + 23] << 3) & 0x07FF);



	//按键数值：1050为中间值，272-1712
	for (int i = 0; i < 20; i++)   //16
	{
		//But_Value[i+1]=  (int32_t)(((CH[i]-272)*0.625+1050-1500)*2.22222);

		But_Value[i + 1] = (int32_t) ((CH[i] - 992) * 1.388889);
	}

	//遥控器地面站在线状态
	if (buf[22] == 0)
		But_Value[17] = 1;
	else
		But_Value[17] = 0;

	But_Value[0]++;
	if(But_Value[0]>=500){
		But_Value[0] = 0;
	}
}


void U7_Sbus_UART_Handler_intialize(struct UARTHandler* Handler)
{
    U7_Sbus_Controller = Handler;
    U7_Sbus_Controller->UART_Decode = decode_U7Data;


    HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, "U7_sbus", 0, u7_sbus);
    LOG("U7_Sbus_intialize");
}

uint8_t IsRemoteAtInitialPosition(void)
{
    if(GV.U7_Key.CH0_LYB_V != 0) return 0;
    if(GV.U7_Key.CH1_LYB_H != 0) return 0;
    if(GV.U7_Key.LU_Single != -1000) return 0;
    if(GV.U7_Key.LU_Three != 0) return 0;
    if(GV.U7_Key.LU_pulley != 0) return 0;
    if(GV.U7_Key.M1 != -1000) return 0;
    if(GV.U7_Key.M2 != -1000) return 0;
    if(GV.U7_Key.M3 != -1000) return 0;
    if(GV.U7_Key.M5 != -1000) return 0;
    if(GV.U7_Key.M6 != -1000) return 0;
    if(GV.U7_Key.RU_Single != -1000) return 0;
    if(GV.U7_Key.RU_Three != 0) return 0;
    LOG("U7_At_Initial_Posotion");
    return 1; // 全部归零
}