// app
#include "balance.h"
#include "linkNleg.h"
#include "robot_def.h"
#include "general_def.h"
#include "ins_task.h"
#include "HT04.h"
#include "LK9025.h"
#include "controller.h"
#include "can_comm.h"
#include "super_cap.h"
#include "user_lib.h"
#include "remote_control.h"
#include "referee_task.h"
#include "stdint.h"
#include "arm_math.h" // 需要用到较多三角函数
#include "bsp_dwt.h"
#include "bsp_log.h"
#include "lqr_calc.h"

// 计时变量
static uint32_t balance_dwt_cnt;
static float del_t;

// 底盘拥有的实例模块
static INS_t *Chassis_IMU_data;
static RC_ctrl_t *rc_data;  // 底盘单独调试用
static Chassis_Ctrl_Cmd_s chassis_cmd_recv; 

// 四个关节电机和两个驱动轮电机
static HTMotorInstance *lf, *lb, *rf, *rb, *joint[4];   // 指针数组方便传参和调试
static LKMotorInstance *l_driven, *r_driven, *driven[2];

// 两个腿的参数,0为左腿,1为右腿
static LinkNPodParam l_side, r_side;
static ChassisParam chassis;

// 底盘状态
static Robot_Status_e chassis_status;


void BalanceInit()
{
    rc_data = RemoteControlInit(&huart3);
    Chassis_IMU_data = INS_Init();

    // 关节电机
    Motor_Init_Config_s joint_conf = {
        // 写一个,剩下的修改方向和id即可
        .can_init_config = {
            .can_handle = &hcan1},
        .controller_setting_init_config = {
            .close_loop_type = OPEN_LOOP,
            .outer_loop_type = OPEN_LOOP,
            .motor_reverse_flag = FEEDBACK_DIRECTION_NORMAL,
            .angle_feedback_source = MOTOR_FEED,
            .speed_feedback_source = MOTOR_FEED,
        },
        .motor_type = HT04};
    joint_conf.can_init_config.tx_id = 1;
    joint_conf.can_init_config.rx_id = 11;
    joint[LF] = lf = HTMotorInit(&joint_conf);
    joint_conf.can_init_config.tx_id = 2;
    joint_conf.can_init_config.rx_id = 12;
    joint[LB] = lb = HTMotorInit(&joint_conf);
    joint_conf.can_init_config.tx_id = 3;
    joint_conf.can_init_config.rx_id = 13;
    joint[RF] = rf = HTMotorInit(&joint_conf);
    joint_conf.can_init_config.tx_id = 4;
    joint_conf.can_init_config.rx_id = 14;
    joint[RB] = rb = HTMotorInit(&joint_conf);

    // 驱动轮电机
    Motor_Init_Config_s driven_conf = {
        // 写一个,剩下的修改方向和id即可
        .can_init_config.can_handle = &hcan2,
        .controller_setting_init_config = {
            .angle_feedback_source = MOTOR_FEED,
            .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type = OPEN_LOOP,
            .close_loop_type = OPEN_LOOP,
            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
        },
        .motor_type = LK9025,
    };
    driven_conf.can_init_config.tx_id = 1;
    driven[RD] = r_driven = LKMotorInit(&driven_conf);
    driven_conf.can_init_config.tx_id = 2;
    driven[LD] = l_driven = LKMotorInit(&driven_conf);

    // 状态初始化
    chassis_status = ROBOT_READY;
    DWT_GetDeltaT(&balance_dwt_cnt);
}

static void EnableAllMotor() /* 打开所有电机 */
{
    for (uint8_t i = 0; i < JOINT_CNT; i++) // 打开关节电机
        HTMotorEnable(joint[i]);
    for (uint8_t i = 0; i < DRIVEN_CNT; i++) // 打开驱动电机
        LKMotorEnable(driven[i]);
}

/* 切换底盘遥控器控制和云台双板控制 */
static void ControlSwitch()
{ 
    // 右侧拨杆向下,进入遥控器底盘控制,此时不响应云台控制指令
    if (switch_is_down(rc_data->rc.switch_right) && RemoteControlIsOnline())
    {
        if (rc_data->rc.rocker_l1 < -600)
        {
            chassis_cmd_recv.chassis_mode = CHASSIS_RESET;
            chassis_cmd_recv.vx = 0.005 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
        }
        else
        {
            chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG;                        // 自由转动&前后
            chassis_cmd_recv.vx = 0.002 * (float)rc_data[TEMP].rc.rocker_r1;           // speed x, unit m/s
        }
    }
    else
        chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE; // 皆离线,急停
}


// 工作状态设定
static void WokingStateSet()
{
    if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) // 未收到遥控器和云台指令底盘进入急停
    {
        for (uint8_t i = 0; i < JOINT_CNT; i++)
            HTMotorStop(joint[i]);
        for (uint8_t i = 0; i < DRIVEN_CNT; i++)
            LKMotorStop(driven[i]);
        return; // 关闭所有电机,发送的指令为零
    }

    // 运动模式
    EnableAllMotor();
}


/**
 * @brief 将电机和imu的数据组装为LinkNPodParam结构体和chassisParam结构体
 *
 * @note HT04电机上电的编码器位置为零(校准过),请看Link2Pod()的note,以及HT04.c中的电机解码部分
 * @note 海泰04电机顺时针旋转为正; LK9025电机逆时针旋转为正,此处皆需要转换为模型中给定的正方向
 *
 */
static void ParamAssemble()
{ 
    // 机体参数,视为平面刚体
    chassis.pitch = Chassis_IMU_data->Pitch * DEGREE_2_RAD;
    chassis.pitch_w = Chassis_IMU_data->Gyro[0];
    chassis.yaw = Chassis_IMU_data->YawTotalAngle * DEGREE_2_RAD;
    chassis.wz = Chassis_IMU_data->Gyro[2];
    chassis.roll = Chassis_IMU_data->Roll * DEGREE_2_RAD;
    chassis.roll_w = Chassis_IMU_data->Gyro[1];

    // HT04电机的角度是顺时针为正,LK9025电机的角度是逆时针为正
    l_side.phi1 = PI + LIMIT_LINK_RAD - lb->measure.total_angle;
    l_side.phi1_w = -lb->measure.speed_rads;
    l_side.phi4 = -lf->measure.total_angle - LIMIT_LINK_RAD;
    l_side.phi4_w = -lf->measure.speed_rads;
    l_side.w_ecd = l_driven->measure.speed_rads;

    r_side.phi1 = PI + LIMIT_LINK_RAD + rb->measure.total_angle;
    r_side.phi1_w = rb->measure.speed_rads;
    r_side.phi4 = rf->measure.total_angle - LIMIT_LINK_RAD;
    r_side.phi4_w = rf->measure.speed_rads;
    r_side.w_ecd = -r_driven->measure.speed_rads;
}

static void WattLimitSet() /* 设定运动模态的输出 */
{
    LKMotorSetRef(l_driven, 195.3125 * l_side.T_wheel);
    LKMotorSetRef(r_driven, 195.3125 * -r_side.T_wheel);
}

void BalanceTask()
{
    del_t = DWT_GetDeltaT(&balance_dwt_cnt);

    // 切换遥控器控制or云台板控制
    ControlSwitch();
    // 设置目标参数和工作模式
    WokingStateSet();
    // 参数组装
    ParamAssemble();
    // 将五连杆映射成单杆
    Link2Leg(&l_side, &chassis);
    Link2Leg(&r_side, &chassis);
    // 根据单杆计算处的角度和杆长,计算反馈增益
    CalcLQR(&l_side, &chassis);
    CalcLQR(&r_side, &chassis);

    // 运动模态,电机输出映射和限幅
    WattLimitSet();
}