#include "control_attitude.h"
#include "hpm_math.h"
#include "auto_pilot.h"
#include "common.h"
#include "control_throttle.h"
#include "flight_mode.h"
#include "helpler_funtions.h"
#include "lowpass_filter.h"
#include "my_math.h"
#include "params.h"
#include "pilot_navigation.h"
#include "soft_flash.h"
#include "soft_imu.h"
#include "soft_motor_output.h"
#include "soft_rc_input.h"


// 自动模式下机头方向
static float target_wpt_bearing = 0.0f;

float t_roll_last = 0.0f, t_pitch_last = 0.0f;

/**
 * @brief 设置自动模式下机头指向
 * @param yaw_deg 机头指向
 * @return true 设置成功
 * @return false 设置失败，机头指向被锁定为指向某点
 */
bool set_automode_target_yaw(float yaw_deg)
{
    target_wpt_bearing = constrain_float(yaw_deg, 0.0f, 360.0f);
    return true;
}

// convert pilot input to lean angles
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
void get_pilot_desired_lean_angles(short roll_in, short pitch_in)
{
    float t_roll = 0.0f, t_pitch = 0.0f, t_total = 0.0f;
    float ratio = 0.0f;
    float scaler = (float)params_get_value(ParamNum_APMaxTilteAngleDeg) / (float)(500 - RC_DEAD_ZONE);

    if (rc_in[RC_CH3] < 1050 && rc_in[RC_CH3] >= 1000)
    {
        roll_in = 1500;
        pitch_in = 1500;
    }

    // 将杆量转换为目标姿态角
    if ((roll_in - 1500) > RC_DEAD_ZONE)
    {
        t_roll = (roll_in - 1500 - RC_DEAD_ZONE) * scaler;
    }
    else if ((roll_in - 1500) < -RC_DEAD_ZONE)
    {
        t_roll = (roll_in - 1500 + RC_DEAD_ZONE) * scaler;
    }
    else
    {
        t_roll = 0.0f;
    }

    if ((pitch_in - 1500) > RC_DEAD_ZONE)
    {
        t_pitch = (pitch_in - 1500 - RC_DEAD_ZONE) * scaler;
    }
    else if ((pitch_in - 1500) < -RC_DEAD_ZONE)
    {
        t_pitch = (pitch_in - 1500 + RC_DEAD_ZONE) * scaler;
    }
    else
    {
        t_pitch = 0.0f;
    }

    t_total = get_norm(t_roll, t_pitch);

    // do circular limit
    if (t_total > params_get_value(ParamNum_APMaxTilteAngleDeg))
    {
        ratio = params_get_value(ParamNum_APMaxTilteAngleDeg) / t_total;

        t_roll *= ratio;
        t_pitch *= ratio;
    }

    pid_m_roll.angle_t = apply(t_roll, t_roll_last, 4.0f, fast_loop_dt);
    pid_m_pitch.angle_t = apply(t_pitch, t_pitch_last, 4.0f, fast_loop_dt);
    t_roll_last = pid_m_roll.angle_t;
    t_pitch_last = pid_m_pitch.angle_t;
}

// get_pilot_desired_heading - transform pilot's yaw input into a desired yaw
// rate
// returns desired yaw rate in centi-degrees per second
void get_pilot_desired_yaw_angle_fromrc(short yaw_in, float dt)
{
    float rc_yaw_rate = 0.0f;

    bool yaw_motion = false;
    static bool yaw_lock = true;

    float scaler = (float)YAW_RATE_MAX / (float)(500 - RC_DEAD_ZONE);

    if (rc_in[RC_CH3] < 1050 && rc_in[RC_CH3] >= 1000)
        yaw_in = 1500;
    //推杆启动慢，默认20启动，导致想小幅度转机头比较难实现
    if ((yaw_in - 1500) > RC_DEAD_ZONE)
    {
        rc_yaw_rate = (yaw_in - 1500 - RC_DEAD_ZONE) * scaler;
        yaw_motion = true;
    }
    else if ((yaw_in - 1500) < -RC_DEAD_ZONE)
    {
        rc_yaw_rate = (yaw_in - 1500 + RC_DEAD_ZONE) * scaler;
        yaw_motion = true;
    }
    else
    {
        yaw_motion = false;
        rc_yaw_rate = 0.0f;
    }

    // angle_p有值是才更新目标角度。避免调试时Kp为零，打杆飞机不动，但是angle_t在更新。
    if (pid_v_yaw.angle_p > 0)
    {
        if (yaw_motion == true)
        {
            yaw_lock = false;
            switch (flight_mode)
            {
            case ATTITUDE:
                // rc_yaw_rate 方向是右打为正，和角度相同
                pid_m_yaw.angle_t = wrap_360(attitude_head + rc_yaw_rate / pid_v_yaw.angle_p);
                break;
            default:
                pid_m_yaw.angle_t = wrap_360(pid_m_yaw.angle_c + rc_yaw_rate / pid_v_yaw.angle_p);
                break;
            }
        }
        else
        {
            if (yaw_lock == false)
            {
                yaw_lock = true;
                switch (flight_mode)
                {
                case ATTITUDE:
                    // gyro_c 的方向是北偏西正，和角度相反
                    pid_m_yaw.angle_t = wrap_360(attitude_head - 1.0f * pid_m_yaw.gyro_c / pid_v_yaw.angle_p);
                    break;
                default:
                    pid_m_yaw.angle_t = wrap_360(pid_m_yaw.angle_c - 1.0f * pid_m_yaw.gyro_c / pid_v_yaw.angle_p);
                    break;
                }
            }
        }
    }

    attitude_head += (-pid_m_yaw.gyro_c * dt);
    attitude_head = wrap_360(attitude_head);
    if (ground_air_status == ON_GROUND)
    {
        attitude_head = pid_m_yaw.angle_c;
        if (abs_int16(yaw_in - 1500) < RC_DEAD_ZONE)
        {
            pid_m_yaw.angle_t = pid_m_yaw.angle_c;
        }
    }
}

/*
姿态控制部分，角度差装换为期望角速度
*/
float attitude_pid_ctl_rp(struct pid_method_rpy *method,
                          struct pid_value_rpy *value)
{
    method->angle_e = method->angle_t - method->angle_c;

    // ================ 计算P比例量 ==================
    method->angle_p_item = method->angle_e * value->angle_p; //当前值为4.5

    method->angle_pid_out = method->angle_p_item;

    method->gyro_t = constrain_float(method->angle_pid_out, -80.0f, +80.0f);

    /*
    测试快速打杆超调问题，加入目标角速度的单次变化限幅。有明显改善。
    测试期间查看rate_p<±100左右，变化迅速>，rate_i<±30左右，变化缓慢>，rate_d<±100左右，变化迅速>
    测试rate_d的低通系数，系数过低时<5HZ>延时过大，导致很容易超调。
    */
    method->gyro_t = method->gyro_t_last + constrain_float(method->gyro_t - method->gyro_t_last,
                        -pid_v_pos.brake_gyro, pid_v_pos.brake_gyro);
    method->gyro_t_last = method->gyro_t;

    return method->gyro_t;
}

/**************************实现函数********************************************
 *函数原型:		float Get_Yaw_Error(float set,float currt)
 *功　　能:	    计算航向差
 *******************************************************************************/
float get_yaw_error(float target, float currt)
{
    float temp;
    temp = target - currt;
    if (temp < 0)
        temp += 360.0f;

    if ((temp >= 0.0f) && (temp <= 180.0f))
        return temp;
    else
        return (temp - 360.0f);
}
/*
姿态控制部分，角度差装换为期望角速度
*/
float attitude_pid_ctl_yaw(struct pid_method_rpy *method,
                           struct pid_value_rpy *value)
{

    if (flight_mode == ATTITUDE)
    {
        // 如果电机输出饱和，触发了航向限制，则赋值一下angle_t
        if (MotorOutput_GetYawRestrictionStatus())
            pid_m_yaw.angle_t = attitude_head;

        method->angle_e = get_yaw_error(method->angle_t, attitude_head);

        if (pid_v_yaw.angle_p > 0 &&
            fabsf(method->angle_e) > YAW_RATE_MAX / pid_v_yaw.angle_p)
        {
            method->angle_e = method->angle_e / fabsf(method->angle_e) *
                              YAW_RATE_MAX / pid_v_yaw.angle_p;

            method->angle_t = wrap_360(attitude_head + method->angle_e);
        }
    }
    else
    {
        // 如果电机输出饱和，触发了航向限制，则赋值一下angle_t
        if (MotorOutput_GetYawRestrictionStatus())
            pid_m_yaw.angle_t = pid_m_yaw.angle_c;

        method->angle_e = get_yaw_error(method->angle_t, method->angle_c);

        if (pid_v_yaw.angle_p > 0 &&
            fabsf(method->angle_e) > YAW_RATE_MAX / pid_v_yaw.angle_p)
        {
            method->angle_e = method->angle_e / fabsf(method->angle_e) *
                              YAW_RATE_MAX / pid_v_yaw.angle_p;

            method->angle_t = wrap_360(method->angle_c + method->angle_e);
        }
    }

    method->angle_p_item =
        method->angle_e * pid_v_yaw.angle_p * -1.0f; //当前值为4.5

    method->gyro_t = method->angle_p_item;

    return method->gyro_t;
}

/**
 * @brief 在不同的模式平滑计算目标杆量
 * 由于SD卡存在延迟现象，此函数应
 */
void get_target_yaw_by_flight_mode(unsigned char flight_mode, float dt)
{
    if (pilot_mode == PILOT_NORMAL)
    {
        switch (flight_mode)
        {
        case ATTITUDE:
            get_pilot_desired_yaw_angle_fromrc(rc_in[RC_YAW], dt);
            set_automode_target_yaw(pid_m_yaw.angle_t);
            break;
        case GCS_VEHICLE_LAUNCH:
            get_smooth_target_yaw(target_wpt_bearing, dt);
            break;
        default:
            get_smooth_target_yaw(target_wpt_bearing, dt);
            break;
        }
    }
}

/*********************************************************************
参  数  ： argTargetYaw - 航线的方向
返回值  ： 传给航向内环的目标航向
功  能  ： 通过航线的方向计算出传递给内环的目标航向
*********************************************************************/
void get_smooth_target_yaw(float argTargetYaw, float dt)
{
    float yawError = wrap_180(argTargetYaw - pid_m_yaw.angle_t);

    if (yawError > parinf._par_maxyawrate * dt)
    {
        pid_m_yaw.angle_t += (parinf._par_maxyawrate * dt);
    }
    else if (yawError < -1.0f * parinf._par_maxyawrate * dt)
    {
        pid_m_yaw.angle_t += (-1.0f * parinf._par_maxyawrate * dt);
    }
    else
    {
        pid_m_yaw.angle_t = argTargetYaw;
    }

    pid_m_yaw.angle_t = wrap_360(pid_m_yaw.angle_t);
}