#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
#include <iostream>
#include <chrono>
#include <thread>

using namespace mavsdk;
using std::chrono::seconds;
using std::this_thread::sleep_for;

int main(int argc, char** argv)
{
    if (argc != 2) {
        std::cerr << "Usage: " << argv[0] << " <connection_url>\n";
        return 1;
    }

    Mavsdk mavsdk;
    ConnectionResult connection_result = mavsdk.add_any_connection(argv[1]);

    if (connection_result != ConnectionResult::Success) {
        std::cerr << "Connection failed: " << connection_result << '\n';
        return 1;
    }

    auto system = mavsdk.systems().at(0);

    // Instantiate mavlink_passthrough plugin.
    auto mavlink_passthrough = MavlinkPassthrough{system};

    mavlink_message_t message;
    mavlink_command_long_t command;
    command.target_system = system->get_system_id();
    command.target_component = MAV_COMP_ID_AUTOPILOT1;
    command.command = MAV_CMD_DO_SET_MODE;
    command.param1 = 4;
    mavlink_msg_command_long_encode(mavlink_passthrough.get_our_sysid(),
                                    mavlink_passthrough.get_our_compid(),
                                    &message,
                                    &command);
    mavlink_passthrough.send_message(message);

    // Wait for the flight mode to change.
    sleep_for(seconds(1));

    // Check the current flight mode.
    const Telemetry::FlightMode mode_result = system->telemetry().flight_mode();
    std::cout << "Current Flight Mode: " << mode_result << '\n';

    return 0;
}