#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 

//尽量别动这组pid
const fp32 Shoot:: M2006_speed_PID[3] = {5, 0, 0.01};
const fp32 Shoot:: M2006_angle_PID[3] = { 15, 0.1, 0};

#define TO_TOP 10.0f
#define TO_BOTTOM 5.0f

#define INIT_POS -130
#define TOP_POS -211
#define BOTTOM_POS 0
#define GO_ERROR 1.0f 
#define Tigger_DO 0  
#define Tigger_ZERO 100 
#define Tigger_ERROR 3


float knob_increment;

Shoot::Shoot()
{
    //扳机初始化
    trigger_Motor= get_motor_point(0);//id 201
    trigger_Motor->type=M2006;
    PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,5000, 2000);//pid初始化
    PID_init(&angle_pid,PID_POSITION,M2006_angle_PID,5000, 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,

    limit_speed=TO_TOP;//快速上去
    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;

    LowPassFilter2p_Init(&distance_filter,500.0f,80.0f); //给distance 做滤波

}

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


    CAN_cmd_200(result,0,0,0,&hcan1);
}


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

    distance= LowPassFilter2p_Apply(&distance_filter, nuc_v.vision.x); // 对视觉距离进行滤波处理
    //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;
}

// F键 sw[0]👆 1800    👇200 
// E键 sw[1]  👆 200 shoot  中 1000 stop  sw[2]200  sw[2]👇1800 
// G键 sw[6]👆 1800  中 1000 👇200
// B键 sw[3]👆 200 开 中 1000  👇1800 关
//     sw[5] 👆 200 👇1800
//左旋 sw[7] 200 --1800
void Shoot::rc_mode()
{
     if(rc_ctrl.sw[1]==200)
    {
        rc_key=UP1;
    }
     if(rc_ctrl.sw[1]==1800 && rc_ctrl.sw[2]==200)
    {
        rc_key=MIDDLE1;
    }
     if(rc_ctrl.sw[2]==1800 && rc_ctrl.sw[1]==1800)
    {
        rc_key=DOWN1;
    }
    if(rc_ctrl.sw[0]==1800)
    {
        mode_key=TEST;  
    }
    if(rc_ctrl.sw[0]==200)
    {
        mode_key=VSION;  
    }
    // if(rc_ctrl.sw[5]==200)
    // {
    //     mode_key=OUT;  
    // }
    if(rc_ctrl.sw[5]==1800)
    {
       ready_key=OFFENSIVE; 
    }
    else if(rc_ctrl.sw[5]==200)
    {
         ready_key=DEFENSE; 
    }

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

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

    // 旋钮物理范围
const int knob_min = 200;
const int knob_max = 1800;

// 目标映射范围
const float map_min = 130.0f;
const float map_max = -60.0f;

int current_knob_value = rc_ctrl.sw[7];

// 限制范围
if (current_knob_value < knob_min) current_knob_value = knob_min;
if (current_knob_value > knob_max) current_knob_value = knob_max;

// 线性映射
knob_increment = map_min + (map_max - map_min) * (current_knob_value - knob_min) / (knob_max - knob_min);


}



#if ONE_CONTROL==0

void Shoot::shoot_control() {

    //方便调试
    feedback.fd_gopos = go1_data->Pos; 
    feedback.fd_tpos = trigger_Motor->total_angle;
 switch (mode_key)
    {
    case VSION:
      fire_pos = distance; // 视觉拟合的力
      switch (rc_key) {
       case DOWN1:
       
        control_pos = INIT_POS; // 默认中间挡位位置     
        go1.Pos = fire_pos; // 设置蓄力电机位置
        //t_posSet = Tigger_ZERO; // 扳机松开

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

    case MIDDLE1:
        shoot_Current();
        break;

    case UP1:
       RemoveError();
        break;

    default:
        break;
    }
break;
   case TEST:
    //实时可调蓄力位置
    fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置

    switch (rc_key) {
       case DOWN1:
       
        control_pos = INIT_POS; // 默认中间挡位位置
        go1.Pos = fire_pos; // 设置蓄力电机位置
        //t_posSet = Tigger_ZERO; // 扳机松开

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

    case MIDDLE1:
        shoot_Current();
        break;

    case UP1:
       RemoveError();
        break;

    default:
        break;
    }
    
break;       
    
    default:
        break;
    }     
    

    	
    abs_limit_min_max_fp(&go1.Pos ,-210.0f,2.0f);
    // 发送数据到蓄力电机
    GO_SendData(go1.Pos,limit_speed);

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

void Shoot :: shoot_Current() 
{

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

        case SHOOT_TOP:
            t_posSet = Tigger_DO; // 扳机扣动钩住
            if (trigger_Motor->total_angle<Tigger_DO+1.0f && trigger_Motor->total_angle>Tigger_DO-1.0f) 
            { 
                //判定为钩住
                is_hooked = true; // 标记钩子已钩住
                currentState = SHOOT_READY; // 切换到准备发射状态
            }
            break;

        case SHOOT_READY:
              if (is_hooked) 
            {
                go1.Pos = fire_pos; // 下拉到中间挡位位置
                limit_speed=TO_BOTTOM;//慢速下来
                if (feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR) {
                  BSP_Buzzer_Start();			
		          BSP_Buzzer_Set(1,5000);
                    //currentState = SHOOT_WAIT; // 等待发射信号
                    //在拨杆切换时触发了
                }
            }
            break;          


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

void Shoot::RemoveError() {
    currentState = SHOOT_IDLE;
    control_pos = TOP_POS+1.0f;
    if (feedback.fd_gopos >= control_pos - 0.5f&& feedback.fd_gopos<= control_pos + 0.5f)
    {
        t_posSet = Tigger_ZERO; 
        is_hooked = false; 
    }
    if(trigger_Motor->total_angle>=Tigger_ZERO-10)
    {
        //认为完全松开
         control_pos=INIT_POS;
         BSP_Buzzer_Stop();

    }
     limit_speed=3.0f;//慢慢送上去
     go1.Pos = control_pos;
}

#endif

#if ONE_CONTROL


void Shoot::shoot_control() {

//方便调试 反馈信息
feedback.fd_gopos = go1_data->Pos; 
feedback.fd_tpos = trigger_Motor->total_angle;


shoot_thread = osThreadFlagsGet(); // 获取任务通知标志位

if(ready_key==OFFENSIVE){
switch (mode_key)
    {
    case VSION:
      fire_pos = distance; // 视觉拟合的力
      switch (rc_key) {
       case MIDDLE1:
            ball_receive(); // 执行接收球的操作
                
        if(shoot_thread & EXTEND_OK) // 如果收到伸缩完成的通知
        {
            go1.Pos = fire_pos; // 设置蓄力电机位置
            if(feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR)
            {
             BSP_Buzzer_Start();			
             BSP_Buzzer_Set(1,5000);
            }
        }
        break;

    case DOWN1:
    if(shoot_thread & EXTEND_OK)
       {
        BSP_Buzzer_Stop();
        t_posSet = Tigger_ZERO; // 扳机扣动 射出
        currentState = SHOOT_IDLE; // 默认回到空闲状态
        osThreadFlagsClear(EXTEND_OK);
       }

        break;

    case UP1:
       RemoveError();
        break;

    default:
        break;
    }
break;

//无自瞄拟合测试档
   case TEST:
    switch (rc_key) {
       case MIDDLE1:

        if(shoot_thread & EXTEND_OK) // 如果收到伸缩完成的通知
        {
            fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置
            go1.Pos = fire_pos; // 设置蓄力电机位置
            if(feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR)
            {
             BSP_Buzzer_Start();			
             BSP_Buzzer_Set(1,5000);
            }
        }
        if(shoot_thread & READY_TELL) // 如果收到等待配合的通知
        {
           ball_receive(); // 执行接收球的操作
        }
        else
        {
          go1.Pos = TOP_POS-3.0f; //-208
          limit_speed=TO_TOP;//快速上去
          t_posSet = Tigger_ZERO; // 扳机松开
        }
        break;

    case DOWN1:
    if(shoot_thread & EXTEND_OK)
       {
        BSP_Buzzer_Stop();
        t_posSet = Tigger_ZERO; // 扳机扣动 射出
        currentState = SHOOT_IDLE; // 默认回到空闲状态
        osThreadFlagsClear(EXTEND_OK);     
       }

        break;

    case UP1:
       RemoveError();
        break;

    default:
        break;
    }
    
break;       
    
    default:
        break;
    }     

}
else if(ready_key==DEFENSE)//攻守档总控
{
          go1.Pos = TOP_POS-3.0f; //-208
          limit_speed=TO_TOP;//快速上去
          t_posSet = Tigger_ZERO; // 扳机松开

}

    abs_limit_min_max_fp(&go1.Pos ,-210.0f,2.0f);
    // 发送数据到蓄力电机
    GO_SendData(go1.Pos, limit_speed);

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

//配合运球到发射
void Shoot ::ball_receive()
{
    switch (currentState) 
    {
        case SHOOT_IDLE:
            // 初始状态：钩子移动到顶部，钩住拉簧
            control_pos = TOP_POS;   
            limit_speed=TO_TOP;//快速上去      
            t_posSet = Tigger_ZERO;
            if(feedback.fd_gopos <-209)
            {
                currentState = GO_TOP; // 切换到准备发射状态
            }
            break;

       case GO_TOP:
            t_posSet = Tigger_DO;
            if (trigger_Motor->total_angle<Tigger_DO+1.0f && trigger_Motor->total_angle>Tigger_DO-1.0f) 
            { 
              
                currentState = BAKC;    // 切换到准备发射状态
            }
            break;  
       case BAKC:
            control_pos = BOTTOM_POS; 
            limit_speed=TO_BOTTOM;//慢速下来  
            if(feedback.fd_gopos >= BOTTOM_POS - 1.0f && feedback.fd_gopos <= BOTTOM_POS + 1.0f)
            {

                osThreadFlagsClear(READY_TELL); // 清除任务通知标志位
            }
            
            break; 
    }
    
   //调整go电机位置
     go1.Pos = control_pos;
}

void Shoot::RemoveError() {
    currentState = SHOOT_IDLE;
    control_pos = TOP_POS;
    if (feedback.fd_gopos >= control_pos - 0.5f&& feedback.fd_gopos<= control_pos + 0.5f)
    {
        t_posSet = Tigger_ZERO; 
        is_hooked = false; 
    }
    if(trigger_Motor->total_angle>=Tigger_ZERO-5)
    {
        //认为完全松开
         control_pos=INIT_POS;
         BSP_Buzzer_Stop();

    }
     limit_speed=3.0f;//慢慢送上去
     go1.Pos = control_pos;
}


#endif