#include "Copter.h"

#if MODE_ARUCOLAND_ENABLED == ENABLED
bool ModeArucoLand::init(bool ignore_checks)
{
  
    if (!copter.motors->armed() || (!(copter.current_loc.alt > 100.0f)) || !copter.ahrs.healthy()) {
    // Print a message or handle the situation as needed
    gcs().send_text(MAV_SEVERITY_CRITICAL, "ArucoLand: Cannot initialize - Drone not armed, or takeoff not complete");
    return false;
    }

    current_stage = ArucoLand_gotodestination;
    cnt = 0;

    copter.mode_guided.init(ignore_checks);

    wp_nav->wp_and_spline_init(g.rtl_speed_cms);

    // Reset flag indicating if the pilot has applied roll or pitch inputs during landing
    copter.ap.land_repo_active = false;

    // This will be set true if prec land is later active
    copter.ap.prec_land_active = false;

#if AC_PRECLAND_ENABLED
    // Initialise precland state machine
    copter.precland_statemachine.init();
#endif

    return true;
}

// should be called at 100hz or more
void ModeArucoLand::run() {
     switch (current_stage) {
        case ArucoLand_gotodestination:
            gotodestination();
            break;
        case ArucoLand_DetectMarker:
            detectArUcoMarker();
            break;
        case ArucoLand_ProcessMessages:
            processMAVLinkMessages();
            break;
        case ArucoLand_PositionAltitudeControl:
            positionAltitudeControl();
            break;
        case ArucoLand_Stabilization:
            stabilization();
            break;
        case ArucoLand_ThrottleAdjustment:
            throttleAdjustment();
            break;
        case ArucoLand_SafelyLand:
            safelyLand();
            break;
        default:
            // Handle unknown stage or end of stages
            break;
    }
}

void ModeArucoLand::gotodestination() {
    
    //copter.mode_guided.set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
    Location destination(-353631830, 1491643970, 8000, Location::AltFrame::ABOVE_HOME);
    if(cnt<2){
       if(copter.set_target_location(destination)){
    // if (copter.mode_guided.set_destination(destination, true, 0, false, 0, false)) {
        copter.mode_guided.runPosControl();
        gcs().send_text(MAV_SEVERITY_INFO, "waypoint set");
    } else {
        gcs().send_text(MAV_SEVERITY_INFO, "waypoint not set");
    
    } 
    cnt++;
    }
    
    // call attitude controller
    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);

    // call height controller
    pos_control->set_pos_target_z_from_climb_rate_cm(0.0f);
    pos_control->update_z_controller();

}

void ModeArucoLand::detectArUcoMarker() {
    // Implement ArUco marker detection logic
    // ..
}

void ModeArucoLand::processMAVLinkMessages() {
    // Implement MAVLink message processing logic
    // ...
}

void ModeArucoLand::positionAltitudeControl() {
    // Implement PID controllers for position and altitude control
    // ...
}

void ModeArucoLand::stabilization() {
    // Implement stabilization logic using PID controllers
    // ...
}

void ModeArucoLand::throttleAdjustment() {
    // Implement fuzzy logic controller for throttle adjustment
    // ...
}
void ModeArucoLand::safelyLand() {
    // Implementation of safelyLand function
    // ...
}
#endif