#include "ball.hpp"
#include "bsp_delay.h"
#include "remote_control.h"
#include "detect.h"
#include "userTask.h"
#include "user_math.h"
#include "shoot.hpp"


#define HANGDING_TWO 0 // 是否使用两次运球

extern RC_ctrl_t rc_ctrl;
extern int ball_exit;


// 伸缩
//外死点132  外130 中119.12  内92  限位90 

#define I_ANGLE 92
#define O_ANGLE 130
#define WAIT_POS 119

// PE11 气缸

Ball ::Ball()
{
    detect_init();

    // 小米电机
    feedback = get_CyberGear_point();

    // 小米电机初始化
    xiaomi.position = I_ANGLE; //
    xiaomi.speed = 25;         //
    xiaomi.K_P = 80;           // 位置增益
    xiaomi.K_D = 20;           // 位置阻尼
    xiaomi.K_C = 12;           // 力矩
    xiaomi.Pmax = 1;           // 好像没啥用

    // //小米电机初始化
    // xiaomi.position = 0;    //
    // xiaomi.speed    = 0;   //
    // xiaomi.K_P      = 0;  //
    // xiaomi.K_D      =0;    //
    // xiaomi.K_C      = 0 ;
    // xiaomi.Pmax     =0;

    // 状态机状态初始化
    currentState1 = BALL_IDLE;
}

// E键 sw[1]  👆 200 shoot  中 1000 stop sw[2]👇1800
// G键 sw[6]👆 200  中 1000 👇1800
//     sw[5] 👆 200 👇1800
// H键 sw[6] 👆200 👇1800
// 左旋 sw[7] 200 --1800

int last_sw5 = 1800; // 上一次拨杆状态

void Ball::rc_mode()
{
    if (rc_ctrl.sw[6] == 200)
    {
        rc_key = UP2;
    }
    if (rc_ctrl.sw[6] == 1000)
    {
        rc_key = MIDDLE2;
    }
    if (rc_ctrl.sw[6] == 1800)
    {
        rc_key = DOWN2;
    }

    if (rc_ctrl.sw[5] == 200)
    {
        extern_key = IN;
    }
    if (rc_ctrl.sw[5] == 1800)
    {
        extern_key = OUT;
    }

    // if (rc_ctrl.sw[5] == 200 && last_sw5 == 1800) // 只在1800->200的瞬间触发
    // {
    //     rc_key = DOWN2;
    // }
    // else
    // {
    //     rc_key = 0; // 其他情况不触发
    // }
    // last_sw5 = rc_ctrl.sw[5];
}

void Ball::Send_control()
{

    CAN_XiaoMi(1, &xiaomi, &hcan2);
    osDelay(1);
}

#if ONE_CONTROL == 0

void Ball::ballDown(void)
{

    HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸爪子
    HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
}

void Ball::Move_Extend()
{
    if (extern_key == IN)
    {
        xiaomi.position = I_ANGLE;
    }
    if (extern_key == OUT)
    {
        xiaomi.position = O_ANGLE;
    }
}

int runCount = 0;        // 运球次数计数
int last_ball_state = 0; // 上一次的光电状态
int step = 0;            // 0:持球 1:击球 2:反弹 3:完全离开

#if HANGDING_TWO == 0

void Ball::ball_control()
{
    ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态(有球 0，无球 1)

    Move_Extend();

    switch (currentState1)
    {
    case BALL_IDLE:
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭
        if (rc_key == DOWN2)                                           // 检测按键是否被按下
        {
            currentState1 = BALL_FORWARD;
        }
        break;

    case BALL_FORWARD:
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸爪子
        osDelay(5);
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 打开下气缸
        currentState1 = BALL_DROP;                                 // 切换到球下落状态
        break;

    case BALL_DROP:
        osDelay(100);                                                // 延时 100ms
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 关闭下气缸
        // 一直检测有球（ball_state == 0），等球离开
        if (ball_state == 1 && last_ball_state == 0) // 球离开
        {
            currentState1 = BALL_FLAG;
        }
        last_ball_state = ball_state;
        break;

    case BALL_FLAG:
        osDelay(10); // 延时 50ms
        // 等待球弹回再次检测到球
        if (ball_state == 0 && last_ball_state == 1) // 球弹回
        {
            currentState1 = BALL_CLOSE;
        }
        last_ball_state = ball_state;
        break;

    case BALL_CLOSE:
        osDelay(100);
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
        currentState1 = BALL_FINISH;                                   // 切换到反转状态
        break;

    case BALL_FINISH:
        osDelay(50);                                                   // 延时 50ms
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭

        currentState1 = BALL_IDLE; // 回到空闲状态

        break;
    default:
        currentState1 = BALL_IDLE; // 默认回到空闲状态
        break;
    }

    Send_control();
}

#else

void Ball::ball_control()
{
    ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态(有球 0，无球 1)

    Move_Extend();

    switch (currentState1)
    {
    case BALL_IDLE:
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭
        if (rc_key == DOWN2)                                           // 检测按键是否被按下
        {
            runCount = 0; // 每次拨动重新计数
            currentState1 = BALL_FORWARD;
        }
        break;

    case BALL_FORWARD:
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸爪子
        osDelay(5);
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 打开下气缸
        currentState1 = BALL_DROP;                                 // 切换到球下落状态
        break;

    case BALL_DROP:
        osDelay(100);                                                // 延时 100ms
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 关闭下气缸
        // 一直检测有球（ball_state == 0），等球离开
        if (ball_state == 1 && last_ball_state == 0) // 球离开
        {
            currentState1 = BALL_FLAG;
        }
        last_ball_state = ball_state;
        break;

    case BALL_FLAG:
        osDelay(10); // 延时 50ms
        // 等待球弹回再次检测到球
        if (ball_state == 0 && last_ball_state == 1) // 球弹回
        {
            currentState1 = BALL_CLOSE;
        }
        last_ball_state = ball_state;
        break;

    case BALL_CLOSE:
        osDelay(100);
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
        currentState1 = BALL_FINISH;                                   // 切换到反转状态
        break;

    case BALL_FINISH:
        osDelay(200);                                                  // 延时 50ms
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭
        runCount++;
        osDelay(200); // 延时 50ms
        if (runCount < 2)
        {
            currentState1 = BALL_FORWARD; // 继续下一次运球
            osDelay(50);
        }
        else
        {
            currentState1 = BALL_IDLE; // 完成两次，回到空闲
        }
        break;

    default:
        currentState1 = BALL_IDLE; // 默认回到空闲状态
        break;
    }

    Send_control();
}

#endif

void Ball::Close(void)
{
    HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
    HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭
    currentState1 = BALL_IDLE;                                     // 回到空闲状态
}

void Ball::Hadling(void)
{
    switch (currentState1)
    {
    case BALL_IDLE:
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭
        if (rc_key == DOWN2)
        {
            ball_exit = 0; // 进入流程时清零
            currentState1 = BALL_FORWARD;
        }
        break;

    case BALL_FORWARD:
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸爪子
        osDelay(5);
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 打开下气缸
        currentState1 = BALL_DROP;                                 // 切换到球下落状态
        break;

    case BALL_DROP:
        osDelay(100);                                                // 延时 100ms
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 关闭下气缸
        // 只响应第一次边沿
        if (ball_exit == 1)
        {
            currentState1 = BALL_FLAG;
        }
        break;
    }
}

void Ball::exit_Handling(void)
{
    switch (currentState1)
    {
    case BALL_IDLE:
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭
        if (rc_key == DOWN2)
        {
            ball_exit = 0; // 进入流程时清零
            currentState1 = BALL_FORWARD;
        }
        break;

    case BALL_FORWARD:
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸爪子
        osDelay(5);
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 打开下气缸
        currentState1 = BALL_DROP;                                 // 切换到球下落状态
        break;

    case BALL_DROP:
        osDelay(100);                                                // 延时 100ms
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 关闭下气缸
        // 只响应第一次边沿
        if (ball_exit == 1)
        {
            currentState1 = BALL_FLAG;
        }
        break;

    case BALL_FLAG:
        // osDelay(10); // 延时 10ms
        //  只响应第二次边沿
        if (ball_exit == 3)
        {
            currentState1 = BALL_CLOSE;
        }
        break;

    case BALL_CLOSE:
        osDelay(25);
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
        currentState1 = BALL_FINISH;                                   // 切换到反转状态
        break;

    case BALL_FINISH:
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭
        ball_exit = 0;                                                 // 流程完成后清零
        break;

    default:
        currentState1 = BALL_IDLE; // 默认回到空闲状态
        break;
    }
}

void Ball::test_up(void)
{

    switch (currentState1)
    {
    case BALL_IDLE:
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭
        if (rc_key == DOWN2)                                           // 检测按键是否被按下
        {
            step = 0;
            currentState1 = BALL_FORWARD;
        }
        break;

    case BALL_FORWARD:
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸爪子
        osDelay(5);
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 打开下气缸
        currentState1 = BALL_DROP;                                 // 切换到球下落状态
        break;

    case BALL_DROP:
        osDelay(50);                                                 // 延时 100ms
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 关闭下气缸

        // 步骤0：等待球离开（持球->无球）
        if (step == 0 && ball_state == 1 && last_ball_state == 0)
        {
            step = 1;
        }
        // 步骤1：等待球反弹回来（无球->持球）
        else if (step == 1 && ball_state == 0 && last_ball_state == 1)
        {
            step = 2;
        }
        // 步骤2：等待球完全上升离开（持球->无球）
        else if (step == 2 && ball_state == 1 && last_ball_state == 0)
        {
            step = 3;
            currentState1 = BALL_CLOSE; // 进入闭合气缸
        }
        last_ball_state = ball_state;
        break;

    case BALL_CLOSE:
        // osDelay(25);
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
        currentState1 = BALL_FINISH;                                   // 切换到完成状态
        break;

    case BALL_FINISH:
        osDelay(50);                                                   // 延时 50ms
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭
        // currentState1 = BALL_IDLE; // 可选：回到空闲状态
        break;

    default:
        currentState1 = BALL_IDLE; // 默认回到空闲状态
        break;
    }
}

void Ball::ballHadling(void)
{

    switch (currentState1)
    {
    case BALL_IDLE:
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭
        if (rc_key == DOWN2)                                           // 检测按键是否被按下
        {
            currentState1 = BALL_FORWARD;
        }
        break;

    case BALL_FORWARD:
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸爪子
        osDelay(5);
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 打开下气缸
        currentState1 = BALL_DROP;                                 // 切换到球下落状态
        break;

    case BALL_DROP:
        osDelay(100);                                                // 延时 100ms
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 关闭下气缸
        // 一直检测有球（ball_state == 0），等球离开
        if (ball_state == 1 && last_ball_state == 0) // 球离开
        {
            currentState1 = BALL_FLAG;
        }
        last_ball_state = ball_state;
        break;

    case BALL_FLAG:
        osDelay(10); // 延时 50ms
        // 等待球弹回再次检测到球
        if (ball_state == 0 && last_ball_state == 1) // 球弹回
        {
            currentState1 = BALL_CLOSE;
        }
        last_ball_state = ball_state;
        break;

    case BALL_CLOSE:
        osDelay(100);
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
        currentState1 = BALL_FINISH;                                   // 切换到反转状态
        break;

    case BALL_FINISH:
        osDelay(50);                                                   // 延时 50ms
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭

        // currentState1 = BALL_IDLE; // 回到空闲状态

        break;

    default:
        currentState1 = BALL_IDLE; // 默认回到空闲状态
        break;
    }
}

void Ball::ballHadling_two(void)
{
    switch (currentState1)
    {
    case BALL_IDLE:
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭
        if (rc_key == DOWN2)
        {
            runCount = 0; // 每次拨动重新计数
            currentState1 = BALL_FORWARD;
        }
        break;

    case BALL_FORWARD:
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸爪子
        osDelay(5);
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 打开下气缸
        currentState1 = BALL_DROP;
        break;

    case BALL_DROP:
        osDelay(100);
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 关闭下气缸
        if (ball_state == 1 && last_ball_state == 0)                 // 球离开
        {
            currentState1 = BALL_FLAG;
        }
        last_ball_state = ball_state;
        break;

    case BALL_FLAG:
        osDelay(10);
        if (ball_state == 0 && last_ball_state == 1) // 球弹回
        {
            currentState1 = BALL_CLOSE;
        }
        last_ball_state = ball_state;
        break;

    case BALL_CLOSE:
        osDelay(25);
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
        currentState1 = BALL_FINISH;
        break;

    case BALL_FINISH:
        osDelay(50);
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭
        runCount++;
        if (runCount < 2)
        {
            currentState1 = BALL_FORWARD; // 继续下一次运球
        }
        else
        {
            currentState1 = BALL_IDLE; // 完成两次，回到空闲
        }
        break;

    default:
        currentState1 = BALL_IDLE;
        break;
    }
}

#endif