#include "TopDefine.h"
#include "shoot.hpp"
#include "userTask.h"
#include "main.h"
#include "remote_control.h"
#include "FreeRTOS.h"
#include <cmsis_os2.h>
#include "calc_lib.h"
#include "vofa.h"
#include "buzzer.h"
#include "bsp_buzzer.h"
#include <math.h>

extern RC_ctrl_t rc_ctrl;
NUC_t nuc_v;
float vofa[8];

double test_distance;

// sw[7]👆 1694  中 1000 👇306
// sw[2]👆 1694   👇306

// E键 sw[1]👆 200 shoot  中 1000 stop 👇1800  error
// F键 sw[0]👆 1800 开   👇200 关
// B键 sw[3]👆 200 开   👇1800 关
// 左旋钮 sw[4] 👈 200    👉1800

// 尽量别动这组pid
const fp32 Shoot::M2006_speed_PID[3] = {5, 0, 0.01};
const fp32 Shoot::M2006_angle_PID[3] = {15, 0.1, 0};

#define TO_TOP 10.0f
#define TO_BOTTOM 6.0f

#define INIT_POS -130
#define TOP_POS -211
#define BOTTOM_POS 0
#define GO_ERROR 1.0f
#define Tigger_DO 0
#define Tigger_ZERO 120
#define Tigger_ERROR 3

float knob_increment;
double last_distance = 4.0f; // 4米做测试吧

Shoot::Shoot()
{
    // 扳机初始化
    trigger_Motor = get_motor_point(0); // id 201
    trigger_Motor->type = M2006;
    PID_init(&speed_pid, PID_POSITION, M2006_speed_PID, 5000, 2000); // pid初始化
    PID_init(&angle_pid, PID_POSITION, M2006_angle_PID, 5000, 2000); // pid初始化
    t_speedSet = 0;
    result = 0;

    go1.id = 1,
    go1.mode = 1,
    go1.K_P = 1.0f,
    go1.K_W = 0.05,
    go1.Pos = 0, // 上电先到一个舒服的位置
    go1.W = 0,
    go1.T = 0,

    limit_speed = TO_TOP; // 快速上去
    fire_pos = INIT_POS;  // 发射位置

    BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART1_RxCompleteCallback); // 注册串口回调函数，bsp层

    go1_data = get_GO_measure_point(); // go1数据

    currentState = SHOOT_IDLE;

    LowPassFilter2p_Init(&distance_filter, 500.0f, 80.0f); // 给distance 做滤波
}

void Shoot::trigger_control()
{
    int delta = 0;
    delta = PID_calc(&angle_pid, trigger_Motor->total_angle, t_posSet); // 计算位置环PID
    result = PID_calc(&speed_pid, trigger_Motor->speed_rpm, delta);     // 计算速度环PID

    CAN_cmd_200(result, 0, 0, 0, &hcan1);
}

float shoot_fitting(float x)
{
    return 0.2006334f * x * x + 25.788123f * x + -169.32157 + 3.8f;
}

float pass_fitting(float x)
{
    return 1.1790172f * x * x + 15.983755f * x + -172.04664f + 1.6f;
}

void Shoot::distanceGet(const NUC_t &nuc_v)
{
    // 判断数据是否在合理范围
    if (nuc_v.vision.x >= 0.0f && nuc_v.vision.x <= 7.5f)
    {
        last_distance = LowPassFilter2p_Apply(&distance_filter, nuc_v.vision.x);
    }

    // 否则不更新，保持上一次的值
    distance = last_distance;
}

int Shoot::GO_SendData(float pos, float limit)
{

    //// static int is_calibration=0;
    //// static fp32 error=0; //误差量

    // 读取参数
    // float tff = go1.T;          // 前馈力矩
    // float kp = go1.K_P;         // 位置刚度
    // float kd = go1.K_W;         // 速度阻尼
    // float q_desired = go1.Pos;  // 期望位置（rad）
    float q_current = go1_data->Pos; // 当前角度位置（rad）
                                     // float dq_desired = go1.W;  // 期望角速度（rad/s）
                                     // float dq_current = go1_data->W;  // 当前角速度（rad/s）

    // 计算输出力矩 tau
    //// float tau = tff + kp * (q_desired - q_current) + kd * (dq_desired - dq_current);

    /*限制最大输入来限制最大输出*/
    if (pos - q_current > limit)
    {
        go1.Pos = q_current + limit; // 限制位置
    }
    else if (pos - q_current < -limit)
    {
        go1.Pos = q_current - limit; // 限制位置
    }
    else
    {
        go1.Pos = pos; // 允许位置
    }

    // 发送数据
    GO_M8010_send_data(&go1);

    return 0;
}

// F键 sw[0]👆 1800    👇200
// E键 sw[1]  👆 200 shoot  中 1000 stop  sw[2]200  sw[2]👇1800
// G键 sw[6]👆 200  中 1000 👇1800
// B键 sw[3]👆 200 开 中 1000  👇1800 关
//     sw[5] 👆 200 👇1800
// 左旋 sw[7] 200 --1800

void Shoot::rc_mode()
{
    // 底部光电检测（假设0为到位，1为未到位，根据实际硬件调整）
    int bottom_sensor = HAL_GPIO_ReadPin(BALL_GPIO_Port, BALL_Pin); // 0为到位
    static bool is_ready = false;

    // 只要检测到到位就解除保护
    if (!is_ready)
    {
        if (bottom_sensor == 0)
        {
            is_ready = true;
            BSP_Buzzer_Stop();
        }
        else
        {
            BSP_Buzzer_Start();
            BSP_Buzzer_Set(1, 5000);
            return;
        }
    }

    // 以下为原有遥控按键逻辑
    if (rc_ctrl.sw[1] == 200)
    {
        rc_key = UP1;
    }
    if (rc_ctrl.sw[1] == 1800 && rc_ctrl.sw[2] == 200)
    {
        rc_key = MIDDLE1;
    }
    if (rc_ctrl.sw[2] == 1800 && rc_ctrl.sw[1] == 1800)
    {
        rc_key = DOWN1;
    }
    if (rc_ctrl.sw[0] == 1800)
    {
        mode_key = PASS;
    }
    if (rc_ctrl.sw[0] == 200)
    {
        mode_key = VSION;
    }
    if (rc_ctrl.sw[5] == 1800)
    {
        ready_key = OFFENSIVE;
    }
    else if (rc_ctrl.sw[5] == 200)
    {
        ready_key = DEFENSE;
    }

    // 旋钮映射部分不变
    const int knob_min = 200;
    const int knob_max = 1800;
    const float map_min = 130.0f;
    const float map_max = -60.0f;
    int current_knob_value = rc_ctrl.sw[7];
    if (current_knob_value < knob_min)
        current_knob_value = knob_min;
    if (current_knob_value > knob_max)
        current_knob_value = knob_max;
    knob_increment = map_min + (map_max - map_min) * (current_knob_value - knob_min) / (knob_max - knob_min);
}

#if ONE_CONTROL == 0

void Shoot::shoot_control()
{

    // 方便调试
    feedback.fd_gopos = go1_data->Pos;
    feedback.fd_tpos = trigger_Motor->total_angle;
    switch (mode_key)
    {
    case VSION:
        fire_pos = distance; // 视觉拟合的力
        // fire_pos =shoot_fitting(test_distance);
        switch (rc_key)
        {
        case DOWN1:

            control_pos = INIT_POS; // 默认中间挡位位置
            go1.Pos = fire_pos;     // 设置蓄力电机位置
            // t_posSet = Tigger_ZERO; // 扳机松开

            if (currentState == SHOOT_READY)
            {
                // 如果当前状态是准备发射，松开钩子发射
                t_posSet = Tigger_ZERO; // 扳机扣动
                BSP_Buzzer_Stop();
                if (t_posSet >= Tigger_ZERO - 20)
                {                              // 假设120度为发射完成角度
                    currentState = SHOOT_IDLE; // 切换到空闲状态
                    is_hooked = false;         // 重置钩子状态
                }
            }
            else
            {
                currentState = SHOOT_IDLE; // 默认回到空闲状态
            }
            break;

        case MIDDLE1:
            shoot_Current();
            break;

        case UP1:
            RemoveError();
            break;

        default:
            break;
        }
        break;
    case PASS:
        // 实时可调蓄力位置
        fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置

        switch (rc_key)
        {
        case DOWN1:

            control_pos = INIT_POS; // 默认中间挡位位置
            go1.Pos = fire_pos;     // 设置蓄力电机位置
            // t_posSet = Tigger_ZERO; // 扳机松开

            if (currentState == SHOOT_READY)
            {
                // 如果当前状态是准备发射，松开钩子发射
                t_posSet = Tigger_ZERO; // 扳机扣动
                BSP_Buzzer_Stop();
                if (t_posSet >= Tigger_ZERO - 20)
                {                              // 假设120度为发射完成角度
                    currentState = SHOOT_IDLE; // 切换到空闲状态
                    is_hooked = false;         // 重置钩子状态
                }
            }
            else
            {
                currentState = SHOOT_IDLE; // 默认回到空闲状态
            }
            break;

        case MIDDLE1:
            shoot_Current();
            break;

        case UP1:
            RemoveError();
            break;

        default:
            break;
        }

        break;

    default:
        break;
    }

    abs_limit_min_max_fp(&go1.Pos, -210.0f, 2.0f);
    // 发送数据到蓄力电机
    GO_SendData(go1.Pos, limit_speed);

    // 控制扳机电机
    trigger_control();
}

void Shoot ::shoot_Current()
{

    switch (currentState)
    {
    case SHOOT_IDLE:
        // 初始状态：钩子移动到顶部，钩住拉簧
        control_pos = TOP_POS; // 顶部位置
        limit_speed = TO_TOP;  // 快速上去
        go1.Pos = control_pos;
        t_posSet = Tigger_ZERO; // 扳机松开
        if (feedback.fd_gopos < -209)
        {
            currentState = SHOOT_TOP; // 切换到准备发射状态
        }
        break;

    case SHOOT_TOP:
        t_posSet = Tigger_DO; // 扳机扣动钩住
        if (trigger_Motor->total_angle < Tigger_DO + 1.0f && trigger_Motor->total_angle > Tigger_DO - 1.0f)
        {
            // 判定为钩住
            is_hooked = true;           // 标记钩子已钩住
            currentState = SHOOT_READY; // 切换到准备发射状态
        }
        break;

    case SHOOT_READY:
        if (is_hooked)
        {
            go1.Pos = fire_pos;      // 下拉到中间挡位位置
            limit_speed = TO_BOTTOM; // 慢速下来
            if (feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR)
            {
                BSP_Buzzer_Start();
                BSP_Buzzer_Set(1, 5000);
                // currentState = SHOOT_WAIT; // 等待发射信号
                // 在拨杆切换时触发了
            }
        }
        break;

    default:
        currentState = SHOOT_IDLE; // 默认回到空闲状态
        break;
    }
}

void Shoot::RemoveError()
{
    currentState = SHOOT_IDLE;
    control_pos = TOP_POS + 1.0f;
    if (feedback.fd_gopos >= control_pos - 0.5f && feedback.fd_gopos <= control_pos + 0.5f)
    {
        t_posSet = Tigger_ZERO;
        is_hooked = false;
    }
    if (trigger_Motor->total_angle >= Tigger_ZERO - 10)
    {
        // 认为完全松开
        control_pos = INIT_POS;
        BSP_Buzzer_Stop();
    }
    limit_speed = 3.0f; // 慢慢送上去
    go1.Pos = control_pos;
}

#endif

#if ONE_CONTROL

void Shoot::shoot_control()
{

    // 方便调试 反馈信息
    feedback.fd_gopos = go1_data->Pos;
    feedback.fd_tpos = trigger_Motor->total_angle;

    shoot_thread = osThreadFlagsGet(); // 获取任务通知标志位

    if (ready_key == OFFENSIVE)
    {
        switch (mode_key)
        {
        case VSION:
            fire_pos = shoot_fitting(distance); // 视觉拟合的力
            switch (rc_key)
            {
            case MIDDLE1:
                if ((shoot_thread & READY_TELL) && !(shoot_thread & EXTEND_OK))
                {
                    // 只收到READY_TELL且未收到EXTEND_OK时，顶部蓄力流程
                    ball_receive();
                }
                else if (shoot_thread & EXTEND_OK)
                {
                    go1.Pos = fire_pos;
                    limit_speed = TO_BOTTOM;
                    if (feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR)
                    {
                        shoot_wait = 1;
                        BSP_Buzzer_Start();
                        BSP_Buzzer_Set(1, 500);
                    }
                }

                break;

            case DOWN1:
                if (shoot_thread & EXTEND_OK && shoot_wait == 1)
                {
                    t_posSet = Tigger_ZERO;
                    if (feedback.fd_tpos >= Tigger_ZERO - 20)
                    {

                        BSP_Buzzer_Stop();
                        currentState = SHOOT_IDLE;
                        osThreadFlagsClear(EXTEND_OK);
                        osThreadFlagsClear(READY_TELL); // 蓄力标志位
                        shoot_wait = 0;
                    }
                }
                break;

            case UP1:
                RemoveError();
                break;

            default:
                break;
            }

            break;

        // 传球档
        case PASS:
            switch (rc_key)
            {
            case MIDDLE1:
                fire_pos = pass_fitting(distance);
                if ((shoot_thread & READY_TELL) && !(shoot_thread & EXTEND_OK))
                {
                    // 只收到READY_TELL且未收到EXTEND_OK时，顶部蓄力流程
                    ball_receive(); // ball_receive内部写go1.Pos
                }
                else if (shoot_thread & EXTEND_OK)
                {
                    // 只收到EXTEND_OK时，允许调节蓄力位置
                    // fire_pos = INIT_POS + knob_increment;
                    go1.Pos = fire_pos;
                    limit_speed = TO_BOTTOM;
                    if (feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR)
                    {
                        shoot_wait = 1;
                        BSP_Buzzer_Start();
                        BSP_Buzzer_Set(1, 500);
                    }
                }
                // 没收到READY_TELL不做任何蓄力
                break;

            case DOWN1:
                if (shoot_thread & EXTEND_OK)
                {
                    if (shoot_wait == 1)
                    {
                        t_posSet = Tigger_ZERO;
                        if (feedback.fd_tpos >= Tigger_ZERO - 20)
                        {

                            BSP_Buzzer_Stop();
                            currentState = SHOOT_IDLE;
                            osThreadFlagsClear(EXTEND_OK);
                            osThreadFlagsClear(READY_TELL); // 蓄力标志位
                            shoot_wait = 0;
                        }
                    }
                }
                break;

            case UP1:
                RemoveError();
                break;

            default:
                break;
            }

            break;

        default:
            break;
        }
    }
    else if (ready_key == DEFENSE)
    {
        control_pos = TOP_POS - 2.0f; //-209
        go1.Pos = control_pos;
        limit_speed = TO_TOP;   // 快速上去
        t_posSet = Tigger_ZERO; // 扳机松开
    }
    else
    {
        t_posSet = Tigger_DO; // 防止debug时重复
    }

    abs_limit_min_max_fp(&go1.Pos, -210.0f, 2.0f);
    // 发送数据到蓄力电机
    GO_SendData(go1.Pos, limit_speed);

    // 控制扳机电机
    trigger_control();
}

// 配合运球到发射
void Shoot ::ball_receive()
{
    switch (currentState)
    {
    case SHOOT_IDLE:
        // 初始状态：钩子移动到顶部，钩住拉簧
        if (shoot_thread & READY_TELL) // 如果收到等待通知
        {
            control_pos = TOP_POS;
            limit_speed = TO_TOP; // 快速上去
            t_posSet = Tigger_ZERO;
            if (feedback.fd_gopos < -209)
            {
                currentState = GO_TOP; // 切换到准备发射状态
            }
        }
        break;

    case GO_TOP:
        t_posSet = Tigger_DO;
        if (trigger_Motor->total_angle < Tigger_DO + 1.0f && trigger_Motor->total_angle > Tigger_DO - 1.0f)
        {

            currentState = BAKC; // 切换到准备发射状态
        }
        break;
    case BAKC:
        control_pos = BOTTOM_POS;
        limit_speed = TO_BOTTOM; // 慢速下来
        if (feedback.fd_gopos >= BOTTOM_POS - 1.0f && feedback.fd_gopos <= BOTTOM_POS + 1.0f)
        {

            osThreadFlagsClear(READY_TELL); // 清除任务通知标志位
        }

        break;
    }

    // 调整go电机位置
    go1.Pos = control_pos;
}

void Shoot::RemoveError()
{
    currentState = SHOOT_IDLE;
    control_pos = TOP_POS;
    if (feedback.fd_gopos < -209)
    {
        t_posSet = Tigger_ZERO;
        is_hooked = false;
    }
    if (trigger_Motor->total_angle >= Tigger_ZERO - 5)
    {
        // 认为完全松开
        control_pos = INIT_POS;
        BSP_Buzzer_Stop();
    }
    limit_speed = 3.0f; // 慢慢送上去
    go1.Pos = control_pos;
}

#endif