#include "up_utils.h"
#include "up.h"

int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle)
{
	  int8_t cnt=0;
	  fp32 angle ,delta;
 			
    angle = f->ecd;  
    if (f->init_cnt < 50) {
		f->orig_angle= angle;    
		f->last_angle = angle;
		f->init_cnt++;            
     return 0;			
    }
		
	
	  delta = angle - f->last_angle;
    if (delta > 4096) {
        f->round_cnt--;           
    } else if (delta < -4096) {
        f->round_cnt++;          
    }
	    f->last_angle = angle;
		  f->total_angle=(f->round_cnt*8191+(angle -f->orig_angle ))*ecd_to_angle;
	
}
/*go电机控制*/	 

int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, float limit)
{ 
 
	  *go_data = *get_GO_measure_point() ;


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

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

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

    // 发送数据
    GO_M8010_send_data(go_cmd);


    return 0;
}


// 计算58度曲线
static float curve_58(float d) {
    return 0.448f * d * d + 24.8604f * d - 177.99552f;
}

// 计算66度曲线
static float curve_66(float d) {
    return 8.84935f * d * d - 10.5424f * d - 126.791f;
}


/*
   曲线切换,用于距离和pos拟合
   迟滞区x-y
   曲线x重合区，根据当前函数和变化方向切换
*/
float CurveChange(float d, float x, float y, CurveType *cs)
{
    
    if (*cs == CURVE_66) {
        if (d > y) {
            *cs = CURVE_58;
        }
    } else { // CURVE_58
        if (d < x) {
            *cs = CURVE_66;
        }
    }

    // 根据当前曲线返回结果
    if (*cs == CURVE_58) {
        return curve_58(d);
    } else {
        return curve_66(d);
    }
}

int8_t Pass_Sequence_Check(UP_t *u, CMD_t *c) //按键顺序检测，传球，按需求改
{
    PassState_t *state = &u->PassContext.PassState;
    static enum {
        SEQ_IDLE,
        SEQ_MID1,
        SEQ_UP,
        SEQ_MID2,
        SEQ_DOWN
    } seq = SEQ_IDLE;

    switch (seq) {
        case SEQ_IDLE:
            if (c->CMD_mode == PB_MID) {
                seq = SEQ_MID1;
                *state = PASS_IDLE;
            }
            break;
        case SEQ_MID1:
            if (c->CMD_mode == PB_UP) {
                seq = SEQ_UP;
                *state = PASS_PREPARE;
            } else if (c->CMD_mode != PB_MID) {
                seq = SEQ_IDLE;
                *state = PASS_STOP;
            }
            break;
        case SEQ_UP:
            if (c->CMD_mode == PB_MID) {
                seq = SEQ_MID2;
                *state = PASS_START;
            } else if (c->CMD_mode != PB_UP) {
                seq = SEQ_IDLE;
                *state = PASS_STOP;
            }
            break;
        case SEQ_MID2:
            if (c->CMD_mode == PB_DOWN) {
                seq = SEQ_DOWN;
                *state = PASS_POS_PREPARE;
            } else if (c->CMD_mode != PB_MID) {
                seq = SEQ_IDLE;
                *state = PASS_STOP;
            }
            break;
        case SEQ_DOWN:
            if (c->CMD_mode == PB_MID) {
                seq = SEQ_IDLE;
                *state = PASS_IDLE; // 或 PASS_COMPLETE，看你需求
            } else if (c->CMD_mode != PB_DOWN) {
                seq = SEQ_IDLE;
                *state = PASS_STOP;
            }
            break;
    }
    return 0;
}