#include "Copter.h"




#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);

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

    // static uint8_t cnt;
    // cnt++;
    // if(cnt > 9){
    //     gcs().send_text(MAV_SEVERITY_INFO, "cam pos: %d,%d",last_pos_lat,last_pos_lng);
    //     gcs().send_text(MAV_SEVERITY_INFO, "gps pos: %d,%d",current_loc.lat,current_loc.lng);
    //     cnt = 0;
    // }
    // mission.set_last_trigger_pos(last_pos_lat,last_pos_lng);
    if(mode_auto.mission._type_resume && last_pos_lat!=0)
    // if(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);
    /********** @Brown,for get camera pos **********/
}
#endif

#ifdef USERHOOK_SUPERSLOWLOOP
static float batt_mah_teb[] =
{
    25.20,24.90,24.84,24.76,24.69,24.64,24.60,24.57,24.54,24.52,
    24.50,24.48,24.45,24.43,24.40,24.36,24.32,24.26,24.21,24.14,
    24.07,24.00,23.94,23.88,23.79,23.71,23.65,23.58,23.51,23.46,
    23.41,23.36,23.30,23.24,23.19,23.13,23.07,23.01,22.95,22.91,
    22.86,22.81,22.76,22.70,22.65,22.59,22.54,22.48,22.43,22.37,
    22.31,22.26,22.21,22.16,22.11,22.06,22.01,21.96,21.92,21.88,
    21.84,21.79,21.76,21.72,21.69,21.64,21.59,21.54,21.49,21.45,
    21.40,21.34,21.29,21.24,21.18,21.13,21.07,21.02,20.96,20.90,
    20.85,20.80,20.75,20.69,20.63,20.56,20.47,20.29,20.16,20.04,
    19.88,19.73,19.53,19.34,19.19,19.00,18.80,18.51,18.00,17.40,
    16.80
};
void Copter::userhook_SuperSlowLoop()
{
    // put your 1Hz code here
    static bool before_fly = true;
    static bool last_disable_ch = false;

    if(motors->armed()){
        before_fly = false;
        relay.on(3);
        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 || \
                channel_throttle->get_control_in()<380 || \
                channel_throttle->get_control_in()>590  ){
                    // rc().set_disable_ch();
                    disable_rc_in = true;
                    if(last_disable_ch != disable_rc_in){
                        gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: 注意遥控通道是否在中位");
                    }
                }else{
                    disable_rc_in = false;
                    // rc().set_enable_ch();
                }
        }else{
            // rc().set_enable_ch();
                    disable_rc_in = false;
        }

    }else{
        relay.off(3);
        // rc().set_enable_ch();
        disable_rc_in = false;
        updownStatus =UpDown_TakeOffStart;
        mavlink_zr_flying_status_t  zr_flying_status_t; 
        zr_flying_status_t.updown_status = updownStatus;
        //TODO UPDATE COUNTDOWN
        gcs().update_zr_fly_status(&zr_flying_status_t);
    }
    if(before_fly){
        uint8_t cnt,cacl_volt_pst;
        float delt_volt;
        for(cnt = 0; cnt<102; cnt++ ){
            delt_volt = batt_mah_teb[cnt] - battery.voltage();
            // delt_volt = batt_mah_teb[cnt] - test_volt;
            if(delt_volt <= 0)
                break;
        }
        cacl_volt_pst = 100 - cnt;
        battery.reset_remaining(1, cacl_volt_pst);
    }
    if(last_disable_ch != disable_rc_in){
        last_disable_ch = disable_rc_in;
    } 

}
#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;
    //     }
    // }
}

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)
}
#endif