#if 1
#include "Copter.h"


// bool Copter::close_to_EKF_origin(const Location& loc)
// {
//     // check distance to EKF origin
//     const struct Location &ekf_origin = inertial_nav.get_origin();
//     if (get_distance(ekf_origin, loc) < 15 && ekf_origin.alt < 5000) {
//         // gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: dist:%.2f,alt:%d",get_distance(ekf_origin, loc) ,ekf_origin.alt);
//         return true;
//     }

//     // close enough to origin
//     return false;
// }

bool Copter::zr_radio_valid(){
    if(!motors->armed()){
        return true;
    }
    if (!rc().ignore_ctrl_channel() && (control_mode == Mode::Number::AUTO || control_mode == Mode::Number::RTL || control_mode == Mode::Number::LAND)){
        if(abs(channel_roll->get_control_in())>750 || \
            abs(channel_pitch->get_control_in())>750 || \
            abs(channel_yaw->get_control_in())>750)
        {
            // rc().set_disable_ch();
            gcs().send_text(MAV_SEVERITY_INFO,"注意遥控通道是否在中位");
            
            return false;
        }
    }
    return true;
}

void Copter::zr_SlowLoop(){

/********** @Brown,for get camera pos **********/
    static int32_t last_pos_lat;
    static int32_t last_pos_lng;

    camera.get_last_trigger_pos(last_pos_lat,last_pos_lng);
    if(mode_auto.mission._type_resume && last_pos_lat!=0)
        mode_auto.mission.auto_resume_mission((uint8_t)control_mode,motors->armed(),last_pos_lat,last_pos_lng);
    else
        mode_auto.mission.auto_resume_mission((uint8_t)control_mode,motors->armed(),current_loc.lat,current_loc.lng);

}

uint8_t Copter::BinarySearch2f(float a[], float value, int low, int high)
{

    if (low > high)
        return cacl_volt_pst;
    cacl_volt_pst = (low + high) / 2;

    if (fabs(a[cacl_volt_pst] - value)<0.01)
        //if (abs(a[mid] - value) < 30)
        return cacl_volt_pst;
    else if (a[cacl_volt_pst] > value)
        return BinarySearch2f(a, value, low, cacl_volt_pst - 1);
    else
        return BinarySearch2f(a, value, cacl_volt_pst + 1, high);
}

void Copter::zr_SuperSlowLoop(){
    static bool before_fly = true;
    static float batt_mah_teb[] =
    {
        16.80, 17.40, 18.00, 18.51, 18.80, 19.00, 19.19, 19.34, 19.53, 19.73,
        19.88, 20.04, 20.16, 20.29, 20.47, 20.56, 20.63, 20.69, 20.75, 20.80,
        20.85, 20.90, 20.96, 21.02, 21.07, 21.13, 21.18, 21.24, 21.29, 21.34,
        21.40, 21.45, 21.49, 21.54, 21.59, 21.64, 21.69, 21.72, 21.76, 21.79,
        21.84, 21.88, 21.92, 21.96, 22.01, 22.06, 22.11, 22.16, 22.21, 22.26,
        22.31, 22.37, 22.43, 22.48, 22.54, 22.59, 22.65, 22.70, 22.76, 22.81,
        22.86, 22.91, 22.95, 23.01, 23.07, 23.13, 23.19, 23.24, 23.30, 23.36,
        23.41, 23.46, 23.51, 23.58, 23.65, 23.71, 23.79, 23.88, 23.94, 24.00,
        24.07, 24.14, 24.21, 24.26, 24.32, 24.36, 24.40, 24.43, 24.45, 24.48,
        24.50, 24.52, 24.54, 24.57, 24.60, 24.64, 24.69, 24.76, 24.84, 24.90,
        25.20
    };
    static float batt_mah_teb_7s[] =
    {
        21.60, 21.80, 22.04, 22.28, 22.52, 22.74, 22.95, 23.19, 23.32, 23.52,
        23.65, 23.81, 23.94, 24.02, 24.10, 24.18, 24.22, 24.25, 24.30, 24.39,
        24.46, 24.52, 24.60, 24.67, 24.74, 24.80, 24.86, 24.93, 24.97, 25.03,
        25.08, 25.13, 25.18, 25.23, 25.27, 25.32, 25.36, 25.41, 25.45, 25.50,
        25.54, 25.58, 25.63, 25.69, 25.74, 25.79, 25.85, 25.91, 25.98, 26.05,
        26.10, 26.16, 26.21, 26.28, 26.35, 26.40, 26.47, 26.54, 26.61, 26.67,
        26.74, 26.79, 26.85, 26.93, 27.00, 27.07, 27.13, 27.19, 27.26, 27.32,
        27.38, 27.46, 27.52, 27.59, 27.66, 27.75, 27.80, 27.87, 27.94, 28.03,
        28.10, 28.16, 28.21, 28.28, 28.33, 28.38, 28.43, 28.46, 28.50, 28.53,
        28.56, 28.59, 28.61, 28.64, 28.67, 28.71, 28.77, 28.86, 28.97, 29.13,
        29.30
    };
    
    if(motors->armed()){
         if(before_fly){
            before_fly = false;
           // camera.create_new_folder(10);
            gcs().send_text(MAV_SEVERITY_INFO,"%s",get_sysid_board_id());
        }
        relay.on(3);
        gcs().send_message(MSG_ZR_FLYING_STATUS);
        // camera.set_in_arm_mode(true);

    }else{
        relay.off(3);
        // camera.set_in_arm_mode(false);
        // rc().set_enable_ch();
        updownStatus =UpDown_TakeOffStart;        
    }
    if(before_fly){
        cacl_volt_pst = 0;
        uint8_t bat_cnt;
        float now_volt = battery.voltage();
        switch (battery.get_batt_type(0))
        {
            case 0:
                /* code */
                break;
            case 1:
                bat_cnt =  BinarySearch2f(batt_mah_teb,now_volt,0,100);
                battery.reset_remaining(1,bat_cnt);
                // gcs().send_text(MAV_SEVERITY_INFO,"cnt:%d,mid:%d",bat_cnt,cacl_volt_pst);
                break;
            case 2:
                bat_cnt = BinarySearch2f(batt_mah_teb_7s,now_volt,0,100);
                battery.reset_remaining(1,bat_cnt);
                // gcs().send_text(MAV_SEVERITY_INFO,"cnt:%d,mid:%d",bat_cnt,cacl_volt_pst);
                break;
            
            default:
                break;
        }
    }
}

void Copter::do_avoid_action(){
    float proxy_dist = 0;


#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    uint16_t rc10_in = RC_Channels::rc_channel(CH_6)->get_radio_in();
    proxy_dist = (rc10_in - 1100)/900.0*50.0;
#else
    AP_Proximity *proximity = AP::proximity();
    if(proximity == nullptr)
        return;
    bool get_new_dist = proximity->get_horizontal_distance(0,proxy_dist);
    if(get_new_dist == false){
        proxy_dist = avoid.get_zr_avd_max() + 1.0;
    }
#endif

    avoid_area_detect(proxy_dist);
    avoid_state(proxy_dist);
    avoid_action(proxy_area,proxy_state,proxy_dist);

}


uint8_t Copter::avoid_area_detect(float dist)
{
    Proxy_Area area = PROXY_OUTRANGE;;
    if(dist < avoid.get_zr_avd_min()){
        if(area != PROXY_TOO_CLOSE && area != PROXY_FIND_BARRIER){  // 前一个状态不是障碍物过近
            area = PROXY_OUTRANGE;
        }else{
            area = PROXY_TOO_CLOSE;
        }
    }
    else if(dist< avoid.get_zr_avd_trig()){
        area = PROXY_TOO_CLOSE;
    }
    else if(dist<avoid.get_zr_avd_max()){
        area = PROXY_FIND_BARRIER;
    }
    else {
        area = PROXY_OUTRANGE;
    }
    proxy_area = area;
    return area;
}

uint8_t Copter::avoid_state(float dist)
{

    Proxy_State state;
    static float previous_dist = 0;
    float delt_dist = dist - previous_dist;
    previous_dist = dist;
    if(delt_dist > 1)
        state = PROXY_LEAVE;
    else if(delt_dist < -1)
        state = PROXY_APPROACH;
    else
        state = PROXY_HOLD;
    proxy_state = state;
    return state;
}

void Copter::avoid_action(uint8_t area,uint8_t state,float dist)
{
    static uint8_t delay_cnt;
    static Proxy_Action action;
    Vector3f pos_neu_cm;
    static int32_t max_climb_alt = 0;
    int32_t turn_yaw;
    static uint32_t rtl_wait_time = AP_HAL::millis();
    switch (action)
    {
    case PROXY_WAIT:
        if(control_mode == Mode::Number::LAND ){    // 降落模式不触发避障
            break;
        }
        if(area == PROXY_FIND_BARRIER && state == PROXY_APPROACH)
            action = PROXY_SLOW_DOWN;
        if(area == PROXY_TOO_CLOSE)
            action = PROXY_BRAKE;
        break;
    case PROXY_SLOW_DOWN:
        if(state == PROXY_APPROACH){
            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 发现障碍物，距离:%3.2fm",dist);
            // last_prx_diff = dist;
        }
        if(area == PROXY_TOO_CLOSE)
            action = PROXY_BRAKE;
        break;
    case PROXY_BRAKE:
        if(state != PROXY_LEAVE){
            gcs().send_text(MAV_SEVERITY_CRITICAL,"障碍物距离过近:%3.2fm，刹车",dist);
            set_mode(Mode::Number::BRAKE,ModeReason::AVOIDANCE);
            action = PROXY_DELAY;
            delay_cnt = 0;
        }
        break;
    case PROXY_DELAY:
        if(delay_cnt > 30){
            
            set_mode(Mode::Number::GUIDED,ModeReason::AVOIDANCE);
            // turn_yaw = current_loc.get_bearing_to(ahrs.get_home());
            // bool need_turn_yaw = abs(ahrs.yaw_sensor - turn_yaw)<4500;
            // if(need_turn_yaw){
            //     action = PROXY_CLIMB;
                max_climb_alt = current_loc.alt+avoid.get_zr_avd_max_climb();
                gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:准备爬升，Now:%.2f，MAX:%.2f，",current_loc.alt/100.0f,max_climb_alt/100.0f);

            // }else{
            //     action = PROXY_RTL;
            //     max_climb_alt = current_loc.alt;
            //     gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:角度在安全范围:%d，直接返航",abs(ahrs.yaw_sensor - turn_yaw));

            // }
            action = PROXY_CLIMB;
            delay_cnt = 0;
        }
        if(control_mode == Mode::Number::LOITER || control_mode == Mode::Number::ALT_HOLD ){
            action = PROXY_WAIT;
            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:切换到人为控制");
        }
        delay_cnt+=1;
        break;
    case PROXY_CLIMB:
        if (!current_loc.get_vector_from_origin_NEU(pos_neu_cm)) {
            
            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:无法获取位置");
            break;
        }
        pos_neu_cm.z += 1000;
        turn_yaw = current_loc.get_bearing_to(ahrs.get_home());
        mode_guided.set_destination(pos_neu_cm, true, turn_yaw, true, 200, false);

        // mode_guided.set_destination(pos_neu_cm, false, 0, false, 0, false);
        if(current_loc.alt > max_climb_alt){
            action = PROXY_NEED_CTL;
            rtl_wait_time = AP_HAL::millis();
            gcs().send_text(MAV_SEVERITY_CRITICAL,"超出爬升高度限制:%.2f，请手动控制",current_loc.alt/100.0f);

        }
        if(area == PROXY_OUTRANGE){
            action = PROXY_RTL;
            max_climb_alt = current_loc.alt + 1000;
            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:已避开障碍，准备返航");
        }
        if(control_mode == Mode::Number::LOITER || control_mode == Mode::Number::ALT_HOLD ){
            action = PROXY_WAIT;
            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:切换到人为控制");
        }
        break;
    case PROXY_RTL:
        if(current_loc.alt < max_climb_alt)
        {
            if (!current_loc.get_vector_from_origin_NEU(pos_neu_cm)) {
                gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:无法获取位置");
                break;
            }
            pos_neu_cm.z += max_climb_alt - current_loc.alt + 500;
            // turn_yaw = current_loc.get_bearing_to(ahrs.get_home());
            mode_guided.set_destination(pos_neu_cm, false, 0, false, 0, false);
            if(control_mode != Mode::Number::GUIDED){
                action = PROXY_WAIT;
                gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:切换到人为控制");
            }
        }else{       
            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:开始返航");
            set_mode(Mode::Number::RTL,ModeReason::AVOIDANCE);
            action = PROXY_WAIT;
        }
 
        break;
    case PROXY_NEED_CTL:
        {
            uint32_t tnow = AP_HAL::millis();
            if(area == PROXY_OUTRANGE){
                action = PROXY_WAIT;
                gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:人为控制，已离开障碍");
            }
            if(tnow - rtl_wait_time > (uint32_t)(g.zr_avd_wait * 1000)){
                if(avoid.get_zr_mode() == 2){ 
                    action = PROXY_WAIT;
                    gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:等待超时，原地降落");
                    set_mode(Mode::Number::LAND,ModeReason::AVOIDANCE);
                }else{
                    action = PROXY_RTL_ERR;
                    gcs().send_text(MAV_SEVERITY_INFO,"NOTICE:等待超时，关闭避障返航");
                    set_mode(Mode::Number::RTL,ModeReason::AVOIDANCE);
                }
            }
        }
        break;
    case PROXY_RTL_ERR:
    {
        if(control_mode == Mode::Number::LAND ){    // 如果切换到Land，则可以重新开始
            action = PROXY_WAIT;
            break;
        }
    }
    
    default:
        break;
    }
}


#endif