#include "pilot_navigation.h"
#include "hpm_math.h"
#include "auto_pilot.h"
#include "common.h"
#include "control_attitude.h"
#include "control_throttle.h"
#include "flight_mode.h"
#include "geomatry.h"
#include "helpler_funtions.h"
#include "lowpass_filter.h"
#include "my_math.h"
#include "params.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_time.h"
#include "ver_config.h"
#include <stdlib.h>

struct rec_pos home_position;
struct rec_pos takeoff_position;
struct rec_pos poshold_position;
struct rec_pos wpphoto_position;
struct rec_pos circlecenter_position;
struct rec_pos target_yaw_lock_position;

// 航线圈数
uint16_t wp_cycle_times = 1;
uint16_t wp_have_cycle_times = 0;

// 机头到达位置的时间
uint32_t wp_reach_yaw_time;

void record_position(int *r_lng, int *r_lat, int lng, int lat)
{
    *r_lng = lng;
    *r_lat = lat;
}

// @brief 根究纬度计算余弦因子
float earth_longitude_scale(int lat)
{
    float earth_lngscale = cosf(fabsf(
        (lat / 10000000.0f) * DEG_TO_RAD)); // 1角度 = PI/180 = 0.0174532925弧度

    return earth_lngscale;
}

// @brief 计算 wgs48 经纬度两点的距离
// @note 使用纬度余弦因子近似弥补误差
int point_to_point_distance(int lon1, int lat1, int lon2, int lat2)
{
    float dist = 0;
    double cmLat = 0.0f;
    double cmLon = 0.0f;

    cmLat = wrap_double((double)(lat2) - (double)(lat1), -180 * 1e7, 180 * 1e7);
    cmLon = wrap_double((double)(lon2) - (double)(lon1), -180 * 1e7, 180 * 1e7) * earth_longitude_scale(lat2);

    dist= hpm_dsp_sqrt_f32(sq(cmLat) + sq(cmLon));
    dist *= LONLAT_TO_CM;

    return dist;
}





float cal_tar_vel_z(int t_alt, int c_alt, int acc_up, int acc_down)
{
    float tar_vel = pid_v_alt.dist_p * (t_alt - c_alt);

    tar_vel = constrain_float(
        tar_vel, -1.0f * parinf._par_max_approach_rate_automode * 10.0f,
        parinf._par_max_climb_rate_automode * 10.0f);

    float delt_vel_up_max = acc_up / 200.0f;
    float delt_vel_down_max = acc_down / 200.0f;

    if (althold_state == NO_ALT_HOLD)
    {
        // 如果飞机低于目标高度 1m 外，则慢加快减
        if (t_alt - c_alt > 100)
        {
            if (tar_vel - pid_m_alt.vel_t > 1.0f)
            {
                tar_vel = pid_m_alt.vel_t + 1.0f;
            }
            else if (tar_vel - pid_m_alt.vel_t < -delt_vel_down_max)
            {
                tar_vel = pid_m_alt.vel_t - delt_vel_down_max;
            }
        }
        // 如果飞机高于目标高度 1m 外，则快加慢减
        else if (t_alt - c_alt < -100)
        {
            if (tar_vel - pid_m_alt.vel_t > delt_vel_up_max)
            {
                tar_vel = pid_m_alt.vel_t + delt_vel_up_max;
            }
            else if (tar_vel - pid_m_alt.vel_t < -1.0f)
            {
                tar_vel = pid_m_alt.vel_t - 1.0f;
            }
        }
        // 如果飞机高度在目标高度 1m 内，则按照正常的逻辑计算目标垂直速度

        // 如果距离目标高度在 10 m 内，则注意速度不要超过 0.6 * 高度误差
        if (abs(t_alt - c_alt) < 1000)
        {
            float tmp_velz = fabsf((t_alt - c_alt) * 0.6f);

            float min_target_velz = (tmp_velz < 100 ? 100 : tmp_velz);

            if (fabsf(tar_vel) > min_target_velz)
            {
                tar_vel = min_target_velz * tar_vel / fabsf(tar_vel);
            }
        }
    }

    return tar_vel;
}


bool reset_wp_start_time_flag = false;
uint32_t start_vel_time = 0, time_period = 0;

#define WP_ACC 100.0f
/**
 * @brief 根据时间计算起停速度
 */
int cal_tar_vel_xy_unac(int h_dist, int r_dist, int min_vel, int max_vel)
{
    // 测试方法三：按照固定加速度执行。
    float start_vel, stop_vel;
    static int init_vel = 0.0f;

    if (r_dist < 0)
        r_dist = 0;

    if (max_vel < 50)
        max_vel = 50;

    if (reset_wp_start_time_flag == true)
    {
        start_vel_time = micros();
        time_period = 0;
        init_vel = ins.horz_vel;
        reset_wp_start_time_flag = false;
    }

    time_period += micros() - start_vel_time;
    start_vel_time = micros();

    start_vel = init_vel + WP_ACC * time_period / 1000000.0f;
    start_vel = constrain_int32(start_vel, 0, max_vel);

    if (r_dist > 500)
    {
        stop_vel = hpm_dsp_sqrt_f32(2 * 0.8f * WP_ACC * (r_dist - 500));
        stop_vel = constrain_int32(stop_vel, min_vel, max_vel);
    }
    else
        stop_vel = min_vel;

    return min_int32(start_vel, stop_vel);
}

// 偏航距的PID，输出一个速度的角度差
float drift_crosstrack_p = 0.0f, 
      drift_crosstrack_d = 0.0f, drift_crosstrack_d_last = 0.0f;



float drift_crosstrack_vel_integ = 0.0f;

// 航点总数
unsigned short waypoint_totalnum = 0;

// 目标航点序号
unsigned short tar_wp_no = 0;


// 前一个点到目标点的距离 cm
int wp_prevtotar_distance = 0;
// 前一个点到目标点的方位角 deg
float wp_prevtotar_bearing = 0.0f;

// 前一个点到当前点的距离 cm
int wp_prevtocur_distance = 0;
// 前一个点到当前点的方位角 deg
float wp_prevtocur_bearing = 0.0f;

// 当前点到目标点的的距离 cm
int wp_curtotar_distance = 0;
// 当前点到目标点的的方位角 deg
float wp_curtotar_bearing = 0.0f;

// 开始点到当前点的垂线距离
int wp_prevtocur_verdistance = 0;
// 当前点到目标点的垂线距离，距离有正负
int wp_curtotar_verdistance = 0;

// 当前点到下条航线的垂直距离
int wp_cur_to_next_wpline_verdistance = 0;

bool fly_point_flag = false, update_point_flag = false,
     execute_command_flag = false;


/**************************实现函数********************************************
 *函数原型:		void update_nav_point(void);
 *功　　能:	    更新航点的信息
 *******************************************************************************/

// 到达目标航点高度标志位
bool tarpoint_alt_isarrive = false;
// 到达目标航点位置标志位
bool tarpoint_pos_isarrive = false;

// U 型转弯航向转动标志
bool coordinatemode_yaw_turn = false;


float wp_curtotar_bearing_last = 0.0f;
float wp_prevtotar_bearing_last = 0.0f;

unsigned int arrive_point_time = 0;



// 航线目标水平速度
static int desire_vel_xy = 0; // CM/S
// 航线目标垂直速度
static int desire_vel_z = 0; // CM/S