/*
 * @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
}
#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;
    //     }
    // }
#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 = AP::mission()->get_current_nav_index();
            uavcan->set_zr_uav_state(data);
        }
    }
#endif
}

void Copter::zr_camera_mkdir()
{
    static uint8_t mkdir_step = 0;
    if (motors->armed())
    {
        if (mkdir_step != 0)
            return;
            
        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);
                }
            }
        }
    }
    else
    {
        mkdir_step = 0;
    }
}

#endif