#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 key;
extern int16_t M2006_result;

// C键 sw[5]👆 1800   中1000  👇200
// D键 sw[6]👆 200   👇1800

//伸缩
#define M_SPEED 4000
#define I_ANGLE1 180
#define I_ANGLE2 -75
#define O_ANGLE1 340
#define O_ANGLE2 -240

// 三摩擦轮电机
#define M_SPEED1 3000
#define M_BACK -4000

#if HANDLING3 == 1
//三摩擦轮运球！！！
//添加平移3508 得跑位置吧

const fp32 Ball:: M3508_speed_PID[3] = {30, 0.03, 0};
const fp32 Ball:: Extend_speed_PID[3] = { 40, 0.0, 0.1};
const fp32 Ball:: Extend_angle_PID[3]=  { 25, 0.1, 0};

//摩擦轮转速
int speedm=0;

//PE11 气缸

Ball ::Ball()
{  
   detect_init();

 //三摩擦轮
   for(int i = 0;i < MOTOR_NUM;i ++)
 {
    hand_Motor[i] = get_motor_point(i);  
    if(i <=3)
    {
        hand_Motor[i]->type = M3508;//设置电机类型
        PID_init(&speed_pid[i],PID_POSITION,M3508_speed_PID,16000, 10000);//pid初始化
    }  
    result[i] = 0;
    speedSet[i] = 0;    
 }

   //两个伸缩6020 左207 右208
   Extern_Motor[0] = get_motor_point(6);
   Extern_Motor[1] = get_motor_point(7);

   Extern_Motor[0]->type = GM6020;
   Extern_Motor[1]->type = GM6020;
   PID_init(&extend_speed_pid[0],PID_POSITION,Extend_speed_PID,10000, 2000);
   PID_init(&extend_angle_pid[0],PID_POSITION,Extend_angle_PID,20000, 1000);

   PID_init(&extend_speed_pid[1],PID_POSITION,Extend_speed_PID,10000, 2000);
   PID_init(&extend_angle_pid[1],PID_POSITION,Extend_angle_PID,20000, 1000);

   e_result[0] = 0;
   e_result[1] = 0;
   angleSet[0] = 0;
   angleSet[1] = 0;

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


}


void Ball ::Extend_mcontrol(int angle1,int angle2)
{
    int16_t delta[2];
    angleSet[0] = angle1/2;
    angleSet[1] = angle2/2;

    fp32 angle_get[2];
    angle_get[0] = motor_ecd_to_angle_change(Extern_Motor[0]->ecd, angleSet[0]);
    angle_get[1] = motor_ecd_to_angle_change(Extern_Motor[1]->ecd, angleSet[1]);

    delta[0] = PID_calc(&extend_angle_pid[0], angle_get[0] , angleSet[0]);
    e_result[0] = PID_calc(&extend_speed_pid[0], Extern_Motor[0]->speed_rpm, delta[0]);

    delta[1] = PID_calc(&extend_angle_pid[1], angle_get[1] , angleSet[1]);
    e_result[1] = PID_calc(&extend_speed_pid[1], Extern_Motor[1]->speed_rpm, delta[1]);

}


void Ball ::Spin()
{

    speedSet[MOTOR_1] = speed_set;
    result[MOTOR_1] = PID_calc(&speed_pid[MOTOR_1],hand_Motor[MOTOR_1]->speed_rpm,speedSet[MOTOR_1]);

    speedSet[MOTOR_2] = speed_set;
    result[MOTOR_2] = PID_calc(&speed_pid[MOTOR_2],hand_Motor[MOTOR_2]->speed_rpm,speedSet[MOTOR_2]);

    speedSet[MOTOR_3] = speed_set;
    result[MOTOR_3] = PID_calc(&speed_pid[MOTOR_3],hand_Motor[MOTOR_3]->speed_rpm,speedSet[MOTOR_3]);

}

void Ball::ballDown(void)
{
     HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);//确保闭合气缸
     speed_set=500;

}

void Ball::Move_Extend(void)
{
    if(rc_ctrl.sw[6] == 200) // 左拨杆上
    {
        Extend_mcontrol(I_ANGLE1, I_ANGLE2); // 内伸
    }
    else if(rc_ctrl.sw[6] == 1800) // 左拨杆下
    {
        Extend_mcontrol(O_ANGLE1, O_ANGLE2); // 外伸
    }
    else
    {
        Extend_mcontrol(O_ANGLE1, O_ANGLE2); // 外伸
    }
}

// C键 sw[5]👆 200   中1000  👇1800
// D键 sw[6]👆 200   👇1800
void Ball::rc_mode()
{
    if(rc_ctrl.sw[5]==200)
    {
        rc_key=UP2;
    }
     if(rc_ctrl.sw[5]==1000)
    {
        rc_key=MIDDLE2;
    }
     if(rc_ctrl.sw[5]==1800)
    {
        rc_key=DOWN2;
    }

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

}


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:
        speed_set=0;
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//确保闭合气缸          
        if (currentState1 == BALL_FINISH)
        {
            currentState1 = BALL_IDLE;
        }
        else {
             currentState1 = BALL_IDLE; // 默认回到空闲状态
        }
        break;

       case UP2: 
        Three_Handing();
        break;

        case DOWN2: 
        ballDown();

        break;

    }

    Spin(); 

    Send_control(); 

}

void Ball::Three_Handing()
{
    switch (currentState1){
    case BALL_IDLE:
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//确保闭合气缸          
        currentState1 = BALL_FORWARD; // 切换到正转状态

        break;
    case BALL_FORWARD:
        speed_set=M_SPEED1;
        if(is_reached_multiple(hand_Motor[MOTOR_1]->speed_rpm,
                            hand_Motor[MOTOR_2]->speed_rpm,
                            hand_Motor[MOTOR_3]->speed_rpm,
                            speed_set,
                            speed_set,
                            speed_set,
                            50.f,50))
        {
          currentState1 = BALL_DROP; // 切换到球下落状态  
        }
       break;

    case BALL_DROP:
      HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸
            if (ball_state == 0) // 读光电 有球 1  无球 0
        {
            osDelay(200); // 延时200ms
            speed_set = M_BACK;
            currentState1 = BALL_REVERSE; // 切换到反转状态
        }
        break;
    case BALL_REVERSE:
        if (ball_state == 1)
        {
            speed_set = 0;
            currentState1 = BALL_CLOSE; // 切换到完成状态
        }
        break;
    case BALL_CLOSE:
        if(ball_state == 1)
        {
            HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
            currentState1 = BALL_FINISH; // 切换到完成状态

        } 
    case BALL_FINISH:
        key = 0; // 重置按键状态
        speed_set = 0;
        osThreadFlagsSet(task_struct.thread.shoot, BALL_OK);
        //currentState1 = BALL_IDLE; // 直接回到空闲状态
        break;       
    default:
          currentState1 = BALL_IDLE; // 默认回到空闲状态
        break;    

    }
}

void Ball::Send_control()
{
    CAN_cmd_200(result[MOTOR_1],result[MOTOR_2],result[MOTOR_3],0,&hcan1);
    osDelay(1);

    CAN_cmd_1FF(M2006_result,0,e_result[0],e_result[1],&hcan1);
    osDelay(1);

}

#else

 int ball_state = 0;
 int triggerCount = 0; // 光电传感器触发计数
 int last_ball_state = 1; // 上一次的光电状态
void Ball::ballHadling(void)
{
  
    ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态（有球 0，无球 1）

    switch (currentState1)
    {
    case BALL_IDLE:
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保气缸闭合
        if (key > 0) // 检测按键是否被按下
        {
            key = 0; // 重置按键状态
            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:
        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(100); // 延时去抖
            HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
            currentState1 = BALL_REVERSE; // 切换到反转状态
        }
        break;

    case BALL_REVERSE:
        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); // 确保下气缸关闭
        key = 0; // 重置按键状态
        triggerCount = 0; // 重置触发计数
        currentState1 = BALL_IDLE; // 回到空闲状态
        break;

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