#include "TopDefine.h"
#include "shoot.hpp"
#include "main.h"
#include "remote_control.h"
#include "FreeRTOS.h"
#include <cmsis_os2.h>
#include "calc_lib.h"
#include "vofa.h"

extern RC_ctrl_t rc_ctrl;
float vofa[8];
#define SHOOT_SPEED 30500
#define SHOOT_SPEED_BACK -2500
#define Error 50

#define STOP 0

#define Trigger_Torque -5000

//sw[7]👆 1694  中 1000 👇306
//sw[2]👆 1694   👇306

//F键 sw[0]👆 1800  中 1000 👇200
//E键 sw[1]👆 1800   👇200

const fp32 Shoot:: M2006_speed_PID[3] = {5, 0, 0};
int t=0;

Shoot::Shoot()
{
    //扳机初始化
    trigger_Motor= get_motor_point(4);
    trigger_Motor->type=M2006;
    PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,6000, 1000);//pid初始化
    speedSet = 0; 
    result = 0;
   
    //发射摩擦轮
    motor_5065[0] = get_vesc_point(0);//获取电机数据指针
    motor_5065[1] = get_vesc_point(1);//获取电机数据指针
    speed_5065=0;
    currentState= SHOOT_IDLE;
}

void Shoot::trigger_control()
{
    ///speedSet=speed;
    //result = PID_calc(&speed_pid, trigger_Motor->speed_rpm, speedSet);
    //result = Trigger_Torque;
    CAN_cmd_1FF(result,0,0,0,&hcan1);

}

void Shoot::shootThree()
{
    if((rc_ctrl.sw[7]==1694))
    {

        speed_5065 = SHOOT_SPEED;

    }
    if((rc_ctrl.sw[7]==1000))
    {
		   speed_5065=STOP;
     //发一次会堵塞另一个      
//       CAN_VESC_HEAD(1); 
//       CAN_VESC_HEAD(2); 
    }
     if(rc_ctrl.sw[7]==306)
    {
		speed_5065=SHOOT_SPEED_BACK;
        
    }
        CAN_VESC_Control(1,speed_5065,&hcan2);
        // CAN_VESC_RPM(1, speed_5065); 
        // CAN_VESC_RPM(2, speed_5065);

        // vofa[0] = motor_5065[1]->rotor_speed;
        // vofa[1] = motor_5065[0]->rotor_speed; // 发送电机速度数据
        // vofa_tx_main(vofa); // 发送数据到虚拟串口
}

void Shoot::shootBack()
{
      speed_5065=SHOOT_SPEED_BACK;      
      CAN_VESC_Control(1,speed_5065,&hcan2);
    
}

void Shoot::shootStop()
{
      speed_5065=STOP;      
      CAN_VESC_Control(1,speed_5065,&hcan2);
  
}

void Shoot::shootStateMachine() {
    switch (currentState) {
        case SHOOT_IDLE: {
            speed_5065=STOP;
            result=STOP;
            // 空闲状态，等待遥控器输入
            if((rc_ctrl.sw[0]==1800)) {
                currentState = SHOOT_FIRE; // 切换到发射状态
            }
            break;
        }

        case SHOOT_FIRE: {
            // 发射状态，控制电机发射
            speed_5065 = SHOOT_SPEED;
        if(motor_5065[0]->rotor_speed>=SHOOT_SPEED-Error && motor_5065[0]->rotor_speed<=SHOOT_SPEED+Error &&
		   motor_5065[1]->rotor_speed>=SHOOT_SPEED-Error && motor_5065[1]->rotor_speed<=SHOOT_SPEED+Error)
		{
			result=Trigger_Torque;//恒力扳机
		}

            // 检测光电传感器是否触发
            if (IS_PHOTOELECTRIC_TRIGGERED()) {
                currentState = SHOOT_BACK; // 切换到光电触发状态
            }
            break;
        }

        case SHOOT_BACK: {
            // 光电触发状态，停止发射
             result=STOP;
             speed_5065=STOP;

            // 切换到返回状态
            currentState = SHOOT_RETURN;
            break;
        }

        case SHOOT_RETURN: {
            // 自动返回状态，控制电机反转
            speed_5065=SHOOT_SPEED_BACK;
            result=-Trigger_Torque;

            // 检测返回完成（可以通过时间或其他传感器判断）
            if (rc_ctrl.sw[0]==1000) { // 假设遥控器开关控制返回完成
                speed_5065=SHOOT_SPEED_BACK;
                result=STOP;
                currentState = SHOOT_IDLE; // 切换回空闲状态
            }
            break;
        }

        default: {
            // 默认状态，回到空闲
            currentState = SHOOT_IDLE;
            break;
        }
    }
     CAN_VESC_Control(1,speed_5065,&hcan2);
     
        vofa[0] = motor_5065[1]->rotor_speed;
        vofa[1] = motor_5065[0]->rotor_speed; // 发送电机速度数据
        vofa[2] = speed_5065; // 发送电机速度数据

        vofa_tx_main(vofa); // 发送数据到虚拟串口
     
}