#include "auto_pilot.h"
#include "common.h"
#include "control_rate.h"
#include "control_throttle.h"
#include "data_save.h"
#include "flight_mode.h"
#include "my_math.h"
#include "params.h"
#include "pilot_navigation.h"
#include "soft_flash.h"
#include "soft_gs.h"
#include "soft_imu.h"
#include "soft_motor_output.h"
#include "soft_port_uart4.h"
#include "soft_rc_input.h"
#include "soft_sdcard.h"
#include "soft_time.h"
#include "soft_timer.h"
#include "soft_voltage.h"
#include "soft_warn.h"
#include "ver_config.h"
#include "helpler_funtions.h"
#include <math.h>
#include <stdlib.h>

char pilot_mode = PILOT_NORMAL;

char warn_reason = WARN_NO;

uint8_t home_pos_isrecord = HOME_POS_NOT_SET;
uint8_t takeoff_pos_isrecord = 0;

// 本次上电飞行时间
static float have_fly_time = 0.0f;
// 本架次飞行时间
static float have_fly_time_this_sort = 0.0f;

// @brief 5hz计算解锁飞行的时间
static void calculate_have_fly_time_5hz(void)
{
    static unsigned int record_fly_time = 0;

    if (thr_lock_status == LOCKED)
    {
        record_fly_time = micros();
        have_fly_time_this_sort = 0.0f;
    }
    else if (thr_lock_status == UNLOCKED)
    {
        float dt = (float)(micros() - record_fly_time) / 1000000.0f;
        if (dt < 0.0f)
        {
            dt = 0.2f;
        }
        have_fly_time += dt;
        have_fly_time_this_sort += dt;
        record_fly_time = micros();
    }
}

// @brief 获取总飞行时间 s
float Get_HaveFlyTime(void) { return have_fly_time; }

// @brief 获取本架次飞行时间 s
float Get_HaveFlyTimeThisSort(void) { return have_fly_time_this_sort; }

static float have_fly_miles_this_sort = 0;
static float have_fly_miles = 0;

// 已飞航线里程
static float have_fly_wp_miles;

// @brief 判断是否使用遥控器进入磁校准
void judge_mag_calibrateion(void)
{
    static short preinput_ch5_mag = 1000;
    static uint8_t magcal_change_counts = 0;
    static uint32_t magcal_time = 0;

    if ((rc_in[RC_CH5] >= MAX_S && rc_in[RC_CH5] <= MAX_E) &&
        (preinput_ch5_mag >= MIN_S && preinput_ch5_mag <= MIN_E))
    {
        magcal_change_counts++;
        preinput_ch5_mag = rc_in[RC_CH5];
        magcal_time = micros();
    }
    else if ((rc_in[RC_CH5] >= MIN_S && rc_in[RC_CH5] <= MIN_E) &&
             (preinput_ch5_mag >= MAX_S && preinput_ch5_mag <= MAX_E))
    {
        magcal_change_counts++;
        preinput_ch5_mag = rc_in[RC_CH5];
        magcal_time = micros();
    }

    // 5通道连续切换8次，且两次之间间隔小于1.5s，则识别为需要磁校准
    // 实际能识别7次就可以了。
    if (magcal_change_counts >= 8)
    {
        // 上锁状态并且未进入磁校准下才能执行，防止连续点击多次发送
        if (thr_lock_status == LOCKED && mag_cal_status == MAG_CAL_NO &&
            pilot_mode == PILOT_NORMAL)
        {
            ci_jiao_zhun = true;
            magcal_change_counts = 0;
        }
    }

    // 如果1.5s之内没有切换，则清零计数
    if (micros() - magcal_time >= 1500000)
    {
        magcal_change_counts = 0;
        magcal_time = micros();
    }
}

// @brief 判断是否使用遥控器进入遥控器校准
void judge_rc_calibration(void)
{
    static short preinput_CH7_rc = 1000;
    static unsigned char rccal_change_counts = 0;
    static unsigned int rccal_time = 0;

    // 解锁时不允许校准遥控器
    if (thr_lock_status == UNLOCKED)
        return;

    if (((raw_rc_in[RC_CH7] >= MAX_S && raw_rc_in[RC_CH7] <= MAX_E) &&
         (preinput_CH7_rc >= MIN_S && preinput_CH7_rc <= MIN_E)) ||
        ((raw_rc_in[RC_CH7] >= MIN_S && raw_rc_in[RC_CH7] <= MIN_E) &&
         (preinput_CH7_rc >= MAX_S && preinput_CH7_rc <= MAX_E)))
    {
        rccal_change_counts++;
        preinput_CH7_rc = raw_rc_in[RC_CH7];
        rccal_time = micros();
    }

    // 6通道切换输出8次
    if (rccal_change_counts >= 8)
    {
        rccal_change_counts = 0;
        if (rc_cal_flag == RC_CALIB_NO)
        {
            // 赋值进入遥控器校准模式
            pilot_mode = PILOT_RC_CLB;
            rc_cal_flag = RC_CALIB_START;
            rc_offset_capture_flag = true;
            // 点击遥控器校准后需要延时一段时间，，不然会记录一个原先的值。填充滤波新数据额需要时间，，，
            rc_cal_time = micros();
        }
        else if (rc_cal_flag == RC_CALIB_START)
        {
            rc_cal_flag = RC_CALIB_NO;
            write_rcfactor_flag = true;
        }
    }

    if (micros() - rccal_time >= 1500000)
    {
        rccal_change_counts = 0;
        rccal_time = micros();
    }
}

//===================================================================
//
//================获取警告信息=====================================
//
//=====================================================================
void get_warn_info(void)
{
    int rcunlock_allow_flag = judge_pilot_rcunlock_allow();

    if (imu_link_status != COMP_NORMAL)
    {
        set_warn_flag_bit(WARN_NO_IMU);
    }
    else
    {
        reset_warn_flag_bit(WARN_NO_IMU);
    }

    if (ins.temprature > 80)
    {
        set_warn_flag_bit(WARN_HIGH_TEMPERATURE);
    }
    else
    {
        reset_warn_flag_bit(WARN_HIGH_TEMPERATURE);
    }

    if (gs_get_link_status(&gcs_link) == COMP_LOST)
    {
        set_warn_flag_bit(WARN_LINK_LOST);
    }
    else
    {
        reset_warn_flag_bit(WARN_LINK_LOST);
    }

    if (imu_link_status != COMP_NORMAL)
    {
        warn_reason = WARN_NOIMU;
    }
    else if (dma_iserror == true)
    {
        warn_reason = WARN_DMA;
    }
    else if (ins.temprature > 80)
    {
        warn_reason = WARN_OVER_TEMPRATURE;
    }
    else if (battary_volt_islow == true)
    {
        warn_reason = WARN_VOLT;
    }
    else if (COMP_LOST == gs_get_link_status(&gcs_link))
    {
        warn_reason = WARN_GS;
    }
    else if (rcunlock_allow_flag == UNLOCK_RCERR)
    {
        warn_reason = WARN_RC;
    }
    else if (ins.insgps_ok == F_AHRS)
    {
        warn_reason = WARN_AHRS;
    }
    else if (unlock_fail == true)
    {
        warn_reason = WARN_ULOCK_FAIL;
    }
    else if (sdcard_initok() != 1)
    {
        warn_reason = WARN_SD_ERROR;
    }
    else
    {
        warn_reason = WARN_NO;
    }
}

/**************************实现函数********************************************
 *函数原型:		void rate_control(void)
 *功　　能:	  底层的陀螺角速度控制 调用频率400hz，一次运行时间11us
 *******************************************************************************/

void rate_control(void)
{
    static unsigned int pid_time = 0;

    float pid_dt = 0.0f;

    pid_dt = time_interval(micros(), &pid_time) / 1000000.0f;
    pid_time = micros();

    pid_roll = rate_pid_ctl_rp(&pid_m_roll, &pid_v_roll, pid_dt); // delta_t 是时间间隔，单位是秒
    pid_pitch = -rate_pid_ctl_rp(&pid_m_pitch, &pid_v_pitch, pid_dt);
    pid_yaw = rate_pid_ctl_yaw(&pid_m_yaw, &pid_v_yaw, pid_dt);

    rate_pid_acctothrottle_alt(pid_dt);
    pid_thr = rate_pid_ctl_thr();
}

/*
1hz 低频率计算一些数据
*/
void loop_1_hz(void)
{
    if (hz_1_flag == true)
    {
        hz_1_flag = false;

        // 检测 imu 连接状态
        get_ins_status();

        // 获取警告信息
        get_warn_info();
    }
}

void loop_2_hz(void)
{
    if (hz_2_flag == true)
    {
        hz_2_flag = false;
    }
}

/*
正常控制模式的5hz循环
*/
void normal_loop_5_hz(void)
{
    if (hz_5_flag == true)
    {
        hz_5_flag = false;

        // 计算已飞时间
        calculate_have_fly_time_5hz();

        // get_fly_data_type_data_5hz();

        // 更新存储数据的buff
        // 只有在解锁的情况下才来记录数据，不解锁就记录会有问题：
        //  1，资源浪费.2，数据量怎多时同时200ms内FLASH无法写完会造成无法结束写入。
#ifdef REC_DATA_POS
        // if (thr_lock_status == UNLOCKED)
#endif
    }
}

/*
正常控制模式的20hz循环
*/
void normal_loop_10_hz(void)
{
    if (hz_10_flag == true)
    {
        hz_10_flag = false;

        // 检测SBUS接收机连接状态。
        check_sbus_link_status(0.1f);

        // 判断是否进入磁校准程序
        judge_mag_calibrateion();

        // 判断是否进入遥控器校准程序
        judge_rc_calibration();

        // 解锁识别
        pilot_unlock_decide();
        // 起飞识别
        pilot_takeoff_decide();
        // 上锁识别
        pilot_lock_decide();

        get_fly_data_type_data_5hz();
    }
}

/*
正常控制模式的50hz循环
*/
void normal_loop_50_hz(void)
{
    if (hz_50_flag == true)
    {
        hz_50_flag = false;

        const float dt = 1 / 50.0f;

        // 更新电压采集
        Voltage_Update();

        // 获取遥控器的值及当前状态
        get_rc_value();

        // 飞行模式判断
        flight_mode_switch();


        // 在锁定情况下如果是要检测电机，在输出电机值
        locked_motor_output();

        // 2 biaozhi 1ge chuan dimianzhan   1ge chuan yaokongqi   flight
        // mode flag == 条件 （1、地面站模式 flag1 2、遥控器模式 flag2）
    }
}

/*
正常控制模式的200hz循环
*/
unsigned int fast_loop_time = 0;
const float fast_loop_dt = 1.f / 200.f;

void normal_loop_200_hz(void)
{
    // 200hZ调用，设置飞机的工作模式
    if (hz_200_flag == true)
    {
        hz_200_flag = false;

        // 更新并执行飞行模式，模式识别在50hz循环中
        if (thr_lock_status == UNLOCKED || flight_mode == GCS_VEHICLE_LAUNCH)
        {
            update_flight_mode();
        }
    }
}

/**
 * 标准模式 400hz
 */
void loop_400_hz(void)
{
    if (hz_400_flag)
    {
        hz_400_flag = false;

    }
}

/*
磁校准模式 50hz的loop
*/
void compasscal_loop_50_hz(void)
{
    if (hz_50_flag == true)
    {
        hz_50_flag = false;

        if (mag_cal_status == MAG_CAL_NO)
        {
            // 经常出现无法进入磁校准的状况，所以，进不去后，可以重传
            ci_jiao_zhun = true;
        }
        /* 如果磁校准通过则退出磁校准模式进入普通模式 */
        if (mag_cal_status == MAG_CAL_OK)
        {
            /*V8M 在此退出磁校准， V7Pro 退出磁校准模式在它的电灯循环中进行 */
            if (ver_par.hardware_id == HW_V8M_YY)
            {
                pilot_mode = PILOT_NORMAL;
                mag_cal_status = MAG_CAL_NO;
                ci_jiao_zhun = false;
            }
        }
    }
}

/*
遥控器校准模式50hz的loop
*/
void rccal_loop_50_hz(void)
{

    if (hz_10_flag == true)
    {
        hz_10_flag = false;

        judge_rc_calibration();
    }

    if (hz_50_flag == true)
    {
        hz_50_flag = false;

        // 点击遥控器校准后需要延时一段时间，，不然会记录一个原先的值。填充滤波新数据额需要时间，，
        if ((rc_cal_flag == RC_CALIB_START) &&
            (micros() - rc_cal_time > 100000))
        {
            rc_input_calibration();
        }

        // 遥控器校准时需要校准的四个通道输出原始遥控器值,便于地面站观察
        for (uint8_t i = 0; i < RC_CALIB_CH_NUM; i++)
        {
            // 赋值给tmp_rc_in来在地面站显示。
            tmp_rc_in[i] = raw_rc_in[i];
        }
    }
}

/*
电调校准模式50hz的loop
*/
void esccal_loop_50_hz(void)
{
    char i = 0;

    if (hz_50_flag == true)
    {
        hz_50_flag = false;

        // 获取遥控器的值
        get_rc_value();

        // 根据机型，全部输出CH3油门量。
        for (i = 1; i <= conf_par.jixing / 10; i++)
        {
            set_motor_pwm(i, rc_in[RC_CH3]);
        }
    }
}

/*
飞行器的主要控制循环函数
*/
void pilot_main_loop(void)
{
    loop_1_hz();

    loop_2_hz();

    loop_400_hz();

    // 正常控制模式
    if (pilot_mode == PILOT_NORMAL)
    {
        normal_loop_5_hz();
        normal_loop_10_hz();
        normal_loop_50_hz();
        normal_loop_200_hz();
    }
    // 磁校准模式
    else if (pilot_mode == PILOT_MAG_CLB)
    {
        compasscal_loop_50_hz();
    }
    // 遥控器校准模式
    else if (pilot_mode == PILOT_RC_CLB)
    {
        rccal_loop_50_hz();
    }
    // 电调校准模式
    else if (pilot_mode == PILOT_ESC_CLB)
    {
        esccal_loop_50_hz();
    }
}