/*
 * @Author: your name
 * @Date: 2020-10-29 14:13:48
 * @LastEditTime: 2020-10-29 14:19:30
 * @LastEditors: your name
 * @Description: In User Settings Edit
 * @FilePath: /zr-v4/ArduCopter/UserCode.cpp
 */
#include "Copter.h"

#if HAL_WITH_UAVCAN
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
#include <AP_UAVCAN/AP_UAVCAN.h>
//#include<uavcan/equipment/camera_gimbal/Status.hpp>
#endif

#define CAM_DEBUG  0

#ifdef USERHOOK_INIT
void Copter::userhook_init()
{
    // put your initialisation code here
    // this will be called once at start-up

    relay.on(1);
    set_mode(Mode::Number::LOITER, ModeReason::STARTUP);
    // gcs().send_text(MAV_SEVERITY_INFO, "NOTICE: version: ZRUAV v4.0.7-rc1");
}
#endif

#ifdef USERHOOK_FASTLOOP
void Copter::userhook_FastLoop()
{
    // put your 100Hz code here
    takeoff_crash_detect();
}
#endif

#ifdef USERHOOK_50HZLOOP
void Copter::userhook_50Hz()
{
    // put your 50Hz code here
    zr_app_50hz();
}
#endif

#ifdef USERHOOK_MEDIUMLOOP
void Copter::userhook_MediumLoop()
{
    // put your 10Hz code here
    zr_app_10hz();
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if(current_loc.alt > avoid.get_zr_avd_alt())
    do_avoid_action();
#else
bool enable_avoid = false;
static bool last_flag = false;

if(avoid.get_zr_mode() > 0 && (control_mode == Mode::Number::BRAKE || control_mode == Mode::Number::AUTO || control_mode == Mode::Number::RTL || control_mode == Mode::Number::GUIDED || control_mode == Mode::Number::CIRCLE)){
    if(far_from_home){
        enable_avoid = true;
    }
}else if(control_mode == Mode::Number::LOITER && !avoid.get_avoid_action() ){
    if(avoid.get_zr_mode() == 3){      // 1: 避不开障碍直接返航，2：避不开降落，3：避不开返航+可以Loiter测试功能
        if(far_from_home){
            enable_avoid = true;
        }
    }
}
if(avoid.get_zr_mode() > 0 && last_flag != enable_avoid){
    if(enable_avoid){
        gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 开启自动避障!");
    }else{
        gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 关闭自动避障!");
    }
    last_flag = enable_avoid;
}
if(enable_avoid){
    do_avoid_action();
}else{
    avoid_action_step = PROXY_WAIT;
}
#endif
}
#endif

#ifdef USERHOOK_SLOWLOOP
void Copter::userhook_SlowLoop()
{
    // put your 3.3Hz code here
    zr_SlowLoop();
    
}
#endif

#ifdef USERHOOK_SUPERSLOWLOOP

void Copter::userhook_SuperSlowLoop()
{
    // put your 1Hz code here
    zr_SuperSlowLoop();
    zr_set_uav_state_to_uavcan();
    zr_camera_mkdir();
    if(current_loc.alt < avoid.get_zr_avd_alt()){   // 飞行高度小于避障启用高度
        if(home_distance() < abs(avoid.get_zr_avd_on_dist())){   // 距离Home点小于启用距离
            far_from_home = false;
        }else{
            far_from_home = true;
        }
        alt_too_low = true;
    }else{
        far_from_home = true;    
        alt_too_low = false;
    }
    avoid.set_enable_aviod(far_from_home); // 远离Home点才启用避障
    /// 手动避障开启提示
    static bool last_manual_avoid = avoid.get_avoid_action();
    if(last_manual_avoid != avoid.get_avoid_action()){
        if(avoid.get_avoid_action()){
            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 开启手动避障!");
        }else{
            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 关闭手动避障!");
        }
        last_manual_avoid = avoid.get_avoid_action();
    }
#if CAM_DEBUG 
    if(in_debug_mode == 2 || control_mode==Mode::Number::ACRO){            
        camera.take_picture();
    }
#endif

}
#endif

#ifdef USERHOOK_AUXSWITCH
void Copter::userhook_auxSwitch1(uint8_t ch_flag)
{
    // put your aux switch #1 handler here (CHx_OPT = 47)

    // switch (ch_flag) {
    //     case 2: {
    //         relay.on(2);
    //         break;
    //     }
    //     case 0: {
    //         relay.off(2);
    //         break;
    //     }
    // }
    switch (ch_flag) {
        case 2: {
            // relay.on(2);
            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode 2!");
            in_debug_mode = 1;
            break;
        }
        // case 1: {
        //     // relay.on(2);
        //     gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode 1!");
        //     in_debug_mode = 1;
        //     break;
        // }
        case 0: {
            // relay.off(2);
            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode off!");
            in_debug_mode = 0;
            break;
        }
    }
#if CAM_DEBUG      
    switch (ch_flag) {
        case 2: {
            // relay.on(2);
            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode 2!");
            in_debug_mode = 2;
            break;
        }
        // case 1: {
        //     // relay.on(2);
        //     gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode 1!");
        //     in_debug_mode = 1;
        //     break;
        // }
        case 0: {
            // relay.off(2);
            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode off!");
            in_debug_mode = 0;
            break;
        }
    }
#endif
}

void Copter::userhook_auxSwitch2(uint8_t ch_flag)
{
    // put your aux switch #2 handler here (CHx_OPT = 48)
}

void Copter::userhook_auxSwitch3(uint8_t ch_flag)
{
    // put your aux switch #3 handler here (CHx_OPT = 49)
}

void Copter::zr_set_uav_state_to_uavcan()
{
#if HAL_WITH_UAVCAN
    uint8_t can_num_drivers = AP::can().get_num_drivers();
    for (uint8_t i = 0; i < can_num_drivers; i++)
    {
        AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i);
        if (uavcan != nullptr)
        {
            zr_uav_state_data_t data{0};
            data.armd = motors->armed();
            data.fly_status = copter.updownStatus;
            data.gps_state = AP::gps().status();
            data.wp_number = g.hardware_flag&0xFF;//硬件版本
            uavcan->set_zr_uav_state(data);
        }
    }
#endif
}


void Copter::zr_camera_mkdir()
{
   
    if (motors->armed())
    {
        switch(mkdir_step)
        {
        case 0:
            zr_mkdir_in_takeoff();
            break;
        case 1: //起飞已经新建文件夹,判断是否返航或者降落 是新建文件夹
            if (control_mode == Mode::Number::LAND)
            {
                AP_Camera *cam = AP::camera();
                if (cam != nullptr)
                {
                    cam->create_new_folder(9);
                    mkdir_step = 2;
                }
            }
            break;
        case 2:
            break;
        default:
            break;
        }  
    }
    else
    {
        mkdir_step = 0;
    }
}

void Copter::zr_mkdir_in_takeoff()
{
    Location loc;
    if (AP::ahrs().get_position(loc))
    {
        int32_t alt_res;
        if (loc.relative_alt)
        {
            alt_res = loc.alt;
        }
        else
        {
            alt_res = loc.alt - AP::ahrs().get_home().alt;
        }
        if (alt_res > 1000) //相对高度>10m 新建文件夹
        {
            mkdir_step = 1;
            AP_Camera *cam = AP::camera();
            if (cam != nullptr)
            {
                cam->create_new_folder(10);
            }
        }
    }
}

void Copter::takeoff_crash_detect()
{
            
    static uint16_t crash_counter;  // number of iterations vehicle may have been crashed
        // return immediately if disarmed, or crash checking disabled
    if (!motors->armed() || g.fs_crash_check == 0) {
        crash_counter = 0;
        return;
    }

    if(g.zr_land_lock_att && rangefinder_alt_ok() && g.land_lock_alt \
        && current_loc.alt < g.land_slow_2nd_alt \
        && rangefinder_state.alt_cm_filt.get() < g.land_lock_alt){
        
        // check for lean angle over 15 degrees  角度判断，角度小于稳定飞行值退出
        const float lean_angle_deg = degrees(acosf(ahrs.cos_roll()*ahrs.cos_pitch()));
        if (lean_angle_deg > float(g.zr_takeoff_prt_deg)) {
            crash_counter +=1 ;
        }else{
            crash_counter = 0;
        }

        // // check for angle error over 30 degrees
        const float angle_error = attitude_control->get_att_error_angle_deg();
        if (angle_error > float(g.zr_takeoff_prt_deg)/2.0) {
            crash_counter +=1 ;
        }else{
            crash_counter = 0;
        }
        // check if crashing for 2 seconds
        AP::logger().Write("TKPT", "TimeUS,Ldeg,Aerr,Alt", "Qfff",
                            AP_HAL::micros64(),
                            lean_angle_deg,
                            angle_error,
                            rangefinder_state.alt_cm_filt.get());

        if (crash_counter >= (g.zr_takeoff_prt_ps)) {
            AP::logger().Write_Error(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_CRASH);
            // keep logging even if disarmed:
            AP::logger().set_force_log_disarmed(true);
            // send message to gcs
            gcs().send_text(MAV_SEVERITY_EMERGENCY,"Crash: Takeoff crash protect");//Crash: Disarming
            // disarm motors
            arming.disarm();
        }
    }
}
#endif