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

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

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

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

//      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();
//         //Close();
//         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 DOWN2: 

//         ballHadling();


//         break;

//     }

//     Send_control(); 

// }

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

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

}

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

// }

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

