#include "ball.hpp"
#include "bsp_delay.h"
#include "remote_control.h"
#include "detect.h"

extern RC_ctrl_t rc_ctrl;
#define KP 0.8
#define KD 0.008
//PE9  刹车光电
//PE11 接球气缸
//PE13 爪子气缸
//PE14 下球气缸
//PC6  接球光电
//PI6  运球光电

int aaa=0;
// 定义状态机变量
BallState_t currentBallState = BALL_IDLE;

Ball ::Ball()
{
    aaa=10;
    //ballState =HAL_GPIO_ReadPin(in_ball_GPIO_Port, in_ball_Pin);
detect_init();
   
}

void Ball ::ballCatch(void)
{

    if(rc_ctrl.ch[1]>=300)
    {
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);//收网    
    }
    else if(rc_ctrl.ch[1]<300)
    {
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//放网
    }

}

void Ball ::ballFlow(void)
{
    GO_M8010_send_data(&huart6, 1,0,0,angleSet[1],0.5,KP,KD);
}

void Ball ::ballZero(void)
{
    GO_M8010_send_data(&huart6, 1,0,0,0,0,0,0);
}

void Ball ::ballHadling(void)
{
    if(rc_ctrl.sw[0]==306)
    {
        HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET);//爪子气缸 开爪
        osDelay(200);
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);//下球气缸 打出
        osDelay(500);
        angleSet[1] = 2;
        ballFlow();
    }
    else
    {
        HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET);//爪子气缸 关爪
        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET);//下球气缸 打入
        angleSet[1] = 0;
        ballFlow();
    }

}

void Ball::ballStateMachine() {
    switch (currentBallState) {
        case BALL_IDLE: {
            // 空闲状态，等待遥控器输入
            if (rc_ctrl.sw[0] == 306) { // 假设遥控器开关控制开始运球
              //  HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 收网
              //  osDelay(200); // 等待收网完成
                currentBallState = BALL_RELEASE; // 切换到下球气缸弹出状态
               
            }
            break;
        }

        case BALL_RELEASE: {
            // 下球气缸弹出，爪子张开
            HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 下球气缸弹出
            HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET);    // 爪子张开
            osDelay(200); // 等待动作完成
            currentBallState = BALL_JOINT_REVERSE; // 切换到关节电机反转状态
            break;
        }

        case BALL_JOINT_REVERSE: {
            // 关节电机反转置水平位置
            angleSet[1] = 2; // 设置关节电机目标角度为水平位置
            ballFlow();      // 控制关节电机动作
            osDelay(500);    // 等待电机动作完成
            currentBallState = BALL_FALLING; // 切换到球加速落下状态
            break;
        }

        case BALL_FALLING: {
            // 球加速落下，等待光电检测
            if (IS_PHOTOELECTRIC_BALL()) {
                currentBallState = BALL_WAIT_BOUNCE; // 切换到等待反弹状态
            }
            break;
        }

        case BALL_WAIT_BOUNCE: {
            // 等待光电检测反弹
            osDelay(50); // 简单延时，避免误触发
            if (IS_PHOTOELECTRIC_BALL()) {
                currentBallState = BALL_NET_CLOSE; // 切换到收网状态
            }
            break;
        }

        case BALL_NET_CLOSE: {
            // 收网状态
            // HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 收网
            // osDelay(200); // 等待收网完成
            HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET);  // 关闭爪子
            HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);  // 下球气缸复位
            angleSet[1] = 1; // 关节电机复位
            ballFlow();
            currentBallState = BALL_IDLE; // 切换回空闲状态
            break;
        }

        default: {

            currentBallState = BALL_IDLE; // 默认回到空闲状态
            
            break;
        }
    }
}