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

extern RC_ctrl_t rc_ctrl;
extern int ball_exit;

// 外死点168  外163 中150  内127  限位124.8
// 伸缩
//外死点89  外85 中75  内49  限位46 

#define I_ANGLE 49
#define O_ANGLE 85
#define WAIT_POS 75
#define HANGDING_POS 89

// PE11 气缸git stash apply

Ball ::Ball()
{
    // 小米电机
    feedback = get_CyberGear_point();

    // 小米电机初始化
    xiaomi.position = I_ANGLE; //
    xiaomi.speed = 35;         //
    xiaomi.K_P = 40;           // 位置增益
    xiaomi.K_D = 100;           // 位置阻尼
    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
// 左旋 sw[7] 200 --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] == 1800)
    {
        ready_key = SIDE;
    }
    if (rc_ctrl.sw[5] == 200)
    {
        ready_key = DEF; // 默认不准备
    }
}

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 = HANGDING_POS;
    }
}

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

    Move_Extend();

    switch (rc_key)
    {
    case MIDDLE2:
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭
        if (currentState1 == BALL_FINISH)
        {
            currentState1 = BALL_IDLE;
        }
        else
        {
            currentState1 = BALL_IDLE; // 默认回到空闲状态
        }
        break;

    case UP2:
        ballDown();
        break;

    case DOWN2:
        ballHadling();

        break;
    }

    Send_control();
}

int ball_state = 0;
int last_ball_state = 0; // 上一次的光电状态

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

#endif

#if ONE_CONTROL

void Ball::ball_control()
{
    hand_thread = osThreadFlagsGet();                              // 获取任务通知标志位
    ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态(有球 0，无球 1)
    

    // 进攻
    if (ready_key == SIDE)
    {
        osThreadFlagsClear(DEF_READY);
        switch (rc_key)
        {
        case MIDDLE2:
            Idle_control();
            break;

        case UP2:
            ballDown();
            break;

        case DOWN2:
            ballHadling();
            break;
        }

        Send_control();
    }
    // 防守
    else if(ready_key == DEF)
    {
        if(hand_thread & DEF_READY)
        {
         xiaomi.position = I_ANGLE; 

        }
                                    // 保持收回
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭

        osThreadFlagsClear(EXTEND_OK);
        osThreadFlagsClear(READY_TELL); // 蓄力标志位
        osThreadFlagsClear(HANDING_FINISH);

        haveball=0;//变为空球状态

        currentState1 = BALL_IDLE;

        Send_control();
    }

}

// 任务通知来作全过程

void Ball::ballDown(void)
{
     osThreadFlagsClear(HANDING_FINISH);
    switch (currentState1)
    {
    case BALL_IDLE:
        xiaomi.position = I_ANGLE; // 保持收回
        if (feedback->position_deg >= I_ANGLE - 1 && feedback->position_deg <= I_ANGLE + 1)
        {
            currentState1 = EXTEND_DOWN;
        }
        break;

    case EXTEND_DOWN:
        osDelay(500); // 不放太快
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);
        // 检测到球自由下落一次就切换状态
        if (ball_state == 1)
        {
            currentState1 = EXTEND_OUT;
        }
        break;

    case EXTEND_OUT:
        xiaomi.position = O_ANGLE; // 保持伸出
        if (feedback->position_deg >= O_ANGLE - 1 && feedback->position_deg <= O_ANGLE + 1)
        {

            osThreadFlagsSet(task_struct.thread.shoot, EXTEND_OK);
            currentState1 = EXTEND_FINISH; // 保持伸出，直到拨杆复位
        }
        break;

    case EXTEND_FINISH:
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
        xiaomi.position = O_ANGLE; // 一直保持伸出
        // 等待拨杆复位（如切到MIDDLE2），Idle_control会负责回位
        break;

    default:
        currentState1 = BALL_IDLE;
        break;
    }
}

// void Ball::Idle_control()
// {
//     HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
//     HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭

//     osThreadFlagsClear(EXTEND_OK);

//     if (ready_key == SIDE) // 检测是否准备好
//     {
//         xiaomi.position = WAIT_POS;
//         if (feedback->position_deg >= WAIT_POS - 3)
//         {
//             // 只在READY_TELL未置位时发送，防止重复
//             if ((osThreadFlagsGet() & READY_TELL) == 0)
//             {
//                 osThreadFlagsSet(task_struct.thread.shoot, READY_TELL);
//             }
//         }
//     }
//     else
//     {
//         xiaomi.position = I_ANGLE; // 默认回到收回位置
//     }

//     // 拨杆回到中间挡位时，回位并重置状态机
//     if (currentState1 == EXTEND_FINISH) // 转移后
//     {
//         xiaomi.position = I_ANGLE;
//         currentState1 = BALL_IDLE;
//     }
//     if (currentState1 == BALL_FINISH) // 运球完成
//     {
//         xiaomi.position = O_ANGLE;
//         currentState1 = BALL_IDLE;
//     }
//     else
//     {
//         currentState1 = BALL_IDLE;
//     }
//     // xiaomi.position = I_ANGLE;
// }

void Ball::Idle_control()
{
    if(ball_state==1 && haveball==0)// 读取光电状态(有球 0，无球 1)
    {
      HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 确保爪气缸张开  

    }
    if(ball_state==0)
    {
        haveball=1;//变为持球状态
        osDelay(500);
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸闭合
    }


    // HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
    HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭

    osThreadFlagsClear(EXTEND_OK);

    if (ready_key == SIDE) // 检测是否准备好
    {
        if(hand_thread & HANDING_FINISH)
        {
            xiaomi.position=HANGDING_POS;//继续保持外伸

        }
        else
        {
          xiaomi.position = WAIT_POS;
          if (feedback->position_deg >= WAIT_POS - 3)
         {
            // 只在READY_TELL未置位时发送，防止重复
            if ((osThreadFlagsGet() & READY_TELL) == 0)
            {
                osThreadFlagsSet(task_struct.thread.shoot, READY_TELL);
            }
         }
        }


    }
    else
    {
        xiaomi.position = I_ANGLE; // 默认回到收回位置
    }

    // 拨杆回到中间挡位时，回位并重置状态机
    if (currentState1 == EXTEND_FINISH) // 转移后
    {
        xiaomi.position = I_ANGLE;
        currentState1 = BALL_IDLE;
    }
    if (currentState1 == BALL_FINISH) // 运球完成
    {
        xiaomi.position = O_ANGLE;
        currentState1 = BALL_IDLE;
    }
    else
    {
        currentState1 = BALL_IDLE;
    }

}



int ball_state = 0;
int last_ball_state = 0; // 上一次的光电状态

void Ball::ballHadling(void)
{

    switch (currentState1)
    {
    case BALL_IDLE:
        if (rc_key == DOWN2)
        {
            xiaomi.position = HANGDING_POS;                 // 外伸
            if (feedback->position_deg >= HANGDING_POS - 0.5f) // 确保伸缩电机到位
            {
                currentState1 = BALL_FORWARD;
            }
        }
        break;

    case BALL_FORWARD:
        osDelay(300); // 不放太快
        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); // 关闭下气缸
        // 一直检测有球（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
        osThreadFlagsSet(task_struct.thread.ball, HANDING_FINISH);
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭
        break;

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

#endif