/*
 * @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();
#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 = AP::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
}

#endif