/*
 * @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
}
#endif

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

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

if(avoid.get_zr_mode()>0 && in_debug_mode > 0){
    bool just_alarm = false;
    if(current_loc.alt < avoid.get_zr_avd_alt()){
        if(home_distance() < avoid.get_zr_avd_on_dist()){
            just_alarm = true;
        }
    }
    if(!just_alarm){
        do_avoid_action();
    }
}
#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 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);
            }
        }
    }
}

#endif