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

//伸缩
#define I_ANGLE 147
#define O_ANGLE 187

//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      = 50 ;  // 力矩
    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;  
    }

}

void Ball::Send_control()
{
  
    CAN_XiaoMi(1,&xiaomi,&hcan2);
    osDelay(1);

}

#if TESTUP

void Ball::ballDown(void)
{

    HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_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;
    }
}

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(PAW_GPIO_Port, PAW_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 triggerCount = 0; // 光电传感器触发计数
 int last_ball_state = 1; // 上一次的光电状态
//(有球 0,无球 1)
void Ball::ballHadling(void)
{
  
    switch (currentState1)
    {
    case BALL_IDLE:
       HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); //确保爪气缸关闭 
       HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭 
        if (rc_key == DOWN2) // 检测按键是否被按下
        {
            triggerCount = 0; // 重置触发计数
            currentState1 = BALL_FORWARD; 
        }
        break;

    case BALL_FORWARD:
        HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_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_state == 0 && last_ball_state == 1) // 检测到状态从无球变为有球
        {
            //osDelay(10); // 延时去抖动
            triggerCount++; // 增加触发计数
            if (triggerCount == 1) // 第一次触发
            {
                currentState1 = BALL_FLAG; // 切换到等待第二次触发状态
            }
        }
        last_ball_state = ball_state; // 更新上一次的状态
        break;

        case BALL_FLAG:
        if (triggerCount == 1 && ball_state == 0 && last_ball_state == 1) // 第二次检测到球
        {
            triggerCount++; // 增加触发计数
            currentState1 = BALL_CLOSE; // 切换到闭合气缸状态
        }
        last_ball_state = ball_state; // 更新上一次的状态
        break;

    case BALL_CLOSE:
        if (triggerCount == 2) // 确保是第二次触发
        {
            //osDelay(5); 
            HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
            currentState1 = BALL_FINISH; // 切换到反转状态
        }
        break;

    case BALL_FINISH:
        osDelay(50); // 延时 50ms
        HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
        triggerCount = 0; // 重置触发计数

        //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); // 读取光电状态(有球 1，无球 0)

    switch (rc_key){
        case MIDDLE2:
         Idle_control();
        break;

       case UP2: 
        ballDown();
        break;

       case DOWN2: 
         osThreadFlagsSet(task_struct.thread.shoot, PREPARE); // 通知发射任务快点去蓄力
         ballHadling();
        break;

    }

    Send_control(); 

}


//任务通知来作全过程

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

        case EXTEND_DOWN:
            HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET);
            HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);
            // 检测到球自由下落一次就切换状态
            if(ball_state == 0)
            {
                currentState1 = EXTEND_OUT;
            }
            break;

        case EXTEND_OUT:
            xiaomi.position = O_ANGLE; // 保持伸出
            if(feedback->position_deg >= O_ANGLE-0.2 && feedback->position_deg <= O_ANGLE+0.2)
            {
                osThreadFlagsSet(task_struct.thread.shoot, EXTEND_OK);
                currentState1 = EXTEND_FINISH; // 保持伸出，直到拨杆复位
            }
            break;

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

        default:
            currentState1 = BALL_IDLE;
            break;
    }
}

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

    osThreadFlagsClear(PREPARE);
    osThreadFlagsClear(EXTEND_OK);

    // 拨杆回到中间挡位时，回位并重置状态机
    xiaomi.position = I_ANGLE;
    currentState1 = BALL_IDLE;
}

 int ball_state = 0;
 int triggerCount = 0; // 光电传感器触发计数
 int last_ball_state = 1; // 上一次的光电状态
//(有球 0,无球 1)
void Ball::ballHadling(void)
{
  
    switch (currentState1)
    {
    case BALL_IDLE:
       HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); //确保爪气缸关闭 
       HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭 
        if (rc_key == DOWN2)
        {   
            xiaomi.position = O_ANGLE;//外伸       
            triggerCount = 0; // 重置触发计数
            if(feedback->position_deg>= O_ANGLE-1)// 确保伸缩电机到位
            {
                 currentState1 = BALL_FORWARD; 
            }
           
        }
        break;

    case BALL_FORWARD:
        HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_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_state == 0 && last_ball_state == 1) // 检测到状态从无球变为有球
        {
            triggerCount++; // 增加触发计数
            if (triggerCount == 1) // 第一次触发
            {
                currentState1 = BALL_FLAG; // 切换到等待第二次触发状态
            }
        }
        last_ball_state = ball_state; // 更新上一次的状态
        break;

        case BALL_FLAG:
       // osDelay(10); // 延时 50ms
        if (triggerCount == 1 && ball_state == 0 && last_ball_state == 1) // 第二次检测到球
        {
            triggerCount++; // 增加触发计数
            currentState1 = BALL_CLOSE; // 切换到闭合气缸状态
        }
        last_ball_state = ball_state; // 更新上一次的状态
        break;

    case BALL_CLOSE:
        if (triggerCount == 2) // 确保是第二次触发
        {
            osDelay(50); 
            HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
            currentState1 = BALL_FINISH; // 切换到反转状态
        }
        break;

    case BALL_FINISH:
        osDelay(50); // 延时 50ms
        HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
       
        triggerCount = 0; // 重置触发计数

        break;

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

    
}   

#endif

