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

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

int triggerspeed = -5000; // 扳机速度
int test_speed =34700;
float test_distance =3.6;
float  xxx;

#define STOP 0

int Torque = -5000; // 扳机恒力
#define Trigger_Torque -5000
#define Trigger_Slow 2000//扳机慢速 不用加负号

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

//E键 sw[1]👆 200 shoot  中 1000 stop 👇1800  error
//F键 sw[0]👆 1800 开   👇200 关
//B键 sw[3]👆 200 开   👇1800 关

const fp32 Shoot:: M2006_speed_PID[3] = {50, 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,8000, 1000);//pid初始化
    t_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()
{
   // result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,t_speedSet);
    CAN_cmd_1FF(result,0,0,0,&hcan1);
}

void Shoot::shootStop()
{
      speed_5065=STOP;   
      result=STOP;//扳机慢速归位   
    

}

void Shoot::errorControl()
{
        result=Trigger_Slow;//扳机慢速归位
        speed_5065=SHOOT_SPEED_BACK; //摩擦归位     
       
    
}


void Shoot :: distanceGet(const NUC_t &nuc_v)
{

    test_distance=nuc_v.vision.z;

}

void Shoot::vesc_send()
{
    CAN_VESC_Control(1, speed_5065, &hcan2);
}

void Shoot::vofaWatch()
{
    vofa[0] = motor_5065[1]->rotor_speed;
    vofa[1] = motor_5065[0]->rotor_speed; // 发送电机速度数据
    vofa[2] = speed_5065; // 发送电机速度数据
    vofa[4] = trigger_Motor->speed_rpm; // 发送扳机电机速度数据
    vofa_tx_main(vofa); // 发送数据到虚拟串口
}


void Shoot::shootStateMachine() {
    
     xxx=polynomial(test_distance);
     distance =test_distance;
    switch (currentState) {
        case SHOOT_IDLE: {
            speed_5065=STOP;
            t_speedSet=STOP;
            // 空闲状态，等待遥控器输入
            if((rc_ctrl.sw[1]==200)) {
                currentState = SHOOT_READY; // 切换到发射状态
            }
            break;
        }

        case SHOOT_READY:
        {
           // 发射状态，控制电机发射
           speed_5065 = test_speed;
           // speed_5065 =polynomial(distance);
        if(motor_5065[0]->rotor_speed>=speed_5065-Error && motor_5065[0]->rotor_speed<=speed_5065+Error &&
		   motor_5065[1]->rotor_speed>=speed_5065-Error && motor_5065[1]->rotor_speed<=speed_5065+Error)
		{
             if((rc_ctrl.sw[3]==1800)) {
                currentState = SHOOT_FIRE; // 切换到发射状态
            }
		
		}
        break;

        }

        case SHOOT_FIRE: {

            t_speedSet=triggerspeed;// 扳机速度影响？
            // 检测光电传感器是否触发
            if (IS_PHOTOELECTRIC_TRIGGERED()) {
                currentState = SHOOT_BACK; // 切换到光电触发状态
            }
            break;
        }



        case SHOOT_BACK: {
            // 光电触发状态，停止发射
             t_speedSet=STOP;
             speed_5065=STOP;
            
            // 切换到返回状态
            currentState = SHOOT_RETURN;
            break;
        }

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

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

        default: {
            // 默认状态，回到空闲
            currentState = SHOOT_IDLE;
            break;
        }
    }
      result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,t_speedSet);
   //  CAN_VESC_Control(1,speed_5065,&hcan2);
          
}

//MATLAB拟合函数
float polynomial(float x) {
    return 1185.3918f * powf(x, 5.0f)
    + -45876.846f * powf(x, 4.0f)
    + 517061.22f * powf(x, 3.0f)
    + -2567947.4f * powf(x, 2.0f)
    + 5954537.4f * x
    + -5258769.0f;
}


float polynomial2(float x) {
    return 49162.7402f * powf(x, 5.0f)
        + -917344.6015f * powf(x, 4.0f)
        + 6840516.5228f * powf(x, 3.0f)
        + -25479457.6038f * powf(x, 2.0f)
        + 47406744.9129f * x
        + -35217628.5565f;
}

// float polynomial(float x) {
//     return 1185.3918*pow(x,5) + -45876.846*pow(x,4) + 517061.22*pow(x,3) + -2567947.4*pow(x,2) + 5954537.4*x + -5258769;
// }