#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"
#include "buzzer.h"
#include "bsp_buzzer.h"

extern RC_ctrl_t rc_ctrl;
NUC_t nuc_v;

//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 关
//左旋钮 sw[4] 👈 200    👉1800 

const fp32 Shoot:: M2006_speed_PID[3] = {5, 0, 0};
const fp32 Shoot:: M2006_angle_PID[3] = { 25, 0, 0.1};

#define INIT_POS -100
#define TOP_POS -210
#define Tigger_DO -5  
#define Tigger_ZERO 130  

float knob_increment;

Shoot::Shoot()
{
    //扳机初始化
    trigger_Motor= get_motor_point(4);//id 205
    trigger_Motor->type=M2006;
    PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,3000, 2000);//pid初始化
    PID_init(&angle_pid,PID_POSITION,M2006_angle_PID,3000, 2000);//pid初始化
    t_speedSet = 0; 
    result = 0;
   

    go1.id =1,
    go1.mode = 1,
    go1.K_P = 1.0f,
    go1.K_W = 0.05,
    go1.Pos = 0,//上电先到一个舒服的位置
    go1.W = 0,
    go1.T = 0,

    fire_pos = INIT_POS; // 发射位置

    BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART1_RxCompleteCallback);//注册串口回调函数，bsp层

    go1_data = get_GO_measure_point();//go1数据

    currentState= SHOOT_IDLE;


}

void Shoot::trigger_control()
{
    int delta = 0;
    delta = PID_calc(&angle_pid,trigger_Motor->total_angle,t_posSet); // 计算位置环PID
    result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,delta); // 计算速度环PID
   // result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,t_speedSet);
    CAN_cmd_1FF(result,0,0,0,&hcan1);
}


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

    //test_distance=nuc_v.vision.x; // 获取自瞄数据

}

int Shoot::GO_SendData(float pos,float limit)
{
   
    //// static int is_calibration=0;
	//// static fp32 error=0; //误差量

    // 读取参数
    // float tff = go1.T;          // 前馈力矩
    // float kp = go1.K_P;         // 位置刚度
    // float kd = go1.K_W;         // 速度阻尼
    // float q_desired = go1.Pos;  // 期望位置（rad）
    float q_current = go1_data->Pos; // 当前角度位置（rad）
    // float dq_desired = go1.W;  // 期望角速度（rad/s）
    // float dq_current = go1_data->W;  // 当前角速度（rad/s）

    // 计算输出力矩 tau
    //// float tau = tff + kp * (q_desired - q_current) + kd * (dq_desired - dq_current);

	  /*限制最大输入来限制最大输出*/
	if (pos - q_current > limit) {
		go1.Pos = q_current + limit; // 限制位置
	}else if (pos - q_current < -limit) {
		go1.Pos = q_current - limit; // 限制位置
	}else {
		go1.Pos = pos; // 允许位置
	}

    // 发送数据
    GO_M8010_send_data(&go1);

    return 0;
}

//E键 sw[1]👆 200 shoot  中 1000 stop 👇1800  error
void Shoot::rc_mode()
{
     if(rc_ctrl.sw[1]==200)
    {
        rc_key=UP1;
    }
     if(rc_ctrl.sw[1]==1000)
    {
        rc_key=MIDDLE1;
    }
     if(rc_ctrl.sw[1]==1800)
    {
        rc_key=DOWN1;
    }

    if(rc_ctrl.sw[7]==200)
    {
      trigger_key=WAIT;  
    }
    if(rc_ctrl.sw[7]==1800)
    {
      trigger_key=SHOOT;  
    }

    //knob_increment=rc_ctrl.ch[2]/150;

    //旋钮增量
    static int last_knob_value = 0; // 记录旋钮的上一次值
    int current_knob_value = rc_ctrl.sw[4]; // 获取当前旋钮值

    // 计算旋钮增量
    if (current_knob_value >= 200 && current_knob_value <= 1800) {
        knob_increment = (current_knob_value - last_knob_value) / 50.0f; // 每80单位对应一个增量
    } else {
        knob_increment = 0; // 如果旋钮值超出范围，不产生增量
    }

}

void Shoot::shoot_control() {

    //方便调试
    feedback.fd_gopos = go1_data->Pos; 
    feedback.fd_tpos = trigger_Motor->total_angle;
      
    switch (rc_key) {
    case DOWN1:
        // 中间挡位：调节拉簧蓄力电机位置
        control_pos = INIT_POS; // 默认中间挡位位置
        //fire_pos = control_pos + 10; // 发射位置（可调节）
        fire_pos = control_pos + knob_increment; // 根据旋钮值调整发射位置
        //fire_pos +=knob_increment;
        go1.Pos = fire_pos; // 设置蓄力电机位置

        if (currentState == SHOOT_READY) {
            // 如果当前状态是准备发射，松开钩子发射
            t_posSet = Tigger_ZERO; // 扳机扣动
            if (t_posSet >= 120) { // 假设120度为发射完成角度
                currentState = SHOOT_IDLE; // 切换到空闲状态
                is_hooked = false; // 重置钩子状态
            }
        } else {
            currentState = SHOOT_IDLE; // 默认回到空闲状态
        }
        break;

    case MIDDLE1:
        shoot_Current();
        break;

    case UP1:
       RemoveError();
        break;

    default:
        break;
    }

    // 发送数据到蓄力电机
    GO_SendData(go1.Pos, 5.0f);

    // 控制扳机电机
    trigger_control();
}

void Shoot :: shoot_Current() 
{

     switch (currentState) {
        case SHOOT_IDLE:
            // 初始状态：钩子移动到顶部，钩住拉簧
            control_pos = TOP_POS; // 顶部位置
            go1.Pos = control_pos; 
            t_posSet = Tigger_ZERO; // 扳机松开
            if (feedback.fd_gopos <-209) {
                currentState = SHOOT_TOP; // 切换到准备发射状态
            }
            break;

        case SHOOT_TOP:
            t_posSet = Tigger_DO; // 扳机扣动钩住
            osDelay(500); // 等待一段时间，确保扳机动作完成
            if (fd_tpos <1) 
            { 
                //判定为钩住
                is_hooked = true; // 标记钩子已钩住
                currentState = SHOOT_READY; // 切换到准备发射状态
            }
            break;

        case SHOOT_READY:
              if (is_hooked) 
            {
                go1.Pos = fire_pos; // 下拉到中间挡位位置
                if (feedback.fd_gopos >= fire_pos - 0.3f && feedback.fd_gopos <= fire_pos + 0.3f) {
                    //currentState = SHOOT_WAIT; // 等待发射信号
                    //在拨杆切换时触发了
                }
            }
            break;          
        // case SHOOT_WAIT:
        //     // 等待发射信号
        //     if (trigger_key == SHOOT) {
        //         currentState = SHOOT_FIRE; // 切换到发射状态
        //     }
        //     break;

        // case SHOOT_FIRE:
        //     // 发射逻辑
        //     t_posSet = Tigger_ZERO; // 扳机扣动
        //     if (t_posSet <= 50) { // 假设250度为发射完成角度
        //         currentState = SHOOT_IDLE; // 切换到空闲状态
        //         is_hooked = false; // 重置钩子状态
        //     }
        //     break;

        default:
            currentState = SHOOT_IDLE; // 默认回到空闲状态
            break;
        }
}

void Shoot::RemoveError() {
    currentState = SHOOT_IDLE;
    control_pos = TOP_POS;
    go1.Pos = control_pos;
    if(feedback.fd_gopos >= control_pos - 0.5f && feedback.fd_gopos <= control_pos + 0.5f) 
    {
        t_posSet = Tigger_ZERO; 
        is_hooked = false; 
    }
}