//
// Demonstrates how to add and fly Waypoint missions using the MAVSDK.
//

#include <bits/chrono.h>
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/mission/mission.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
#include <mavsdk/plugins/mission_raw/mission_raw.h>
#include <mavsdk/plugins/offboard/offboard.h>

// reexport mavlink headers using this plugin
#include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>

#include <chrono>
#include <functional>
#include <future>
#include <iostream>
#include <ratio>
#include <thread>

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

static void wait_armable(bool& is_armable)
{
    while (is_armable != true) {
        std::cout << "Vehicle is getting ready to arm\n";
        sleep_for(seconds(1));
    }
}

void usage(const std::string& bin_name)
{
    std::cerr << "Usage : " << bin_name << " <connection_url>\n"
              << "Connection URL format should be :\n"
              << " For TCP : tcp://[server_host][:server_port]\n"
              << " For UDP : udp://[bind_host][:bind_port]\n"
              << " For Serial : serial:///path/to/serial/dev[:baudrate]\n"
              << "For example, to connect to the simulator use URL: udp://:14540\n";
}

MissionRaw::MissionItem make_mission_item_wp(
    float latitude_deg1e7,
    float longitude_deg1e7,
    int32_t altitude_m,
    float do_photo,
    MAV_FRAME frame,
    MAV_CMD command,
    float p2 = 0,
    float p3 = 0)
{
    // WARNING this is done in consideration of CLEAN!! mission
    static uint32_t seq_num = 0;
    MissionRaw::MissionItem new_item{};
    new_item.seq = seq_num;
    new_item.frame = static_cast<uint32_t>(frame);
    new_item.command = static_cast<uint32_t>(command);
    new_item.param1 = do_photo;
    new_item.x = latitude_deg1e7 * 1e7;
    new_item.y = longitude_deg1e7 * 1e7;
    new_item.z = altitude_m;
    new_item.mission_type = MAV_MISSION_TYPE_MISSION;
    new_item.autocontinue = 1;

    if (seq_num == 1) {
        new_item.current = 1;
    }

    seq_num++;

    return new_item;
}

std::shared_ptr<System> get_system(Mavsdk& mavsdk)
{
    std::cout << "Waiting to discover system...\n";
    auto prom = std::promise<std::shared_ptr<System>>{};
    auto fut = prom.get_future();

    // We wait for new systems to be discovered, once we find one that has an
    // autopilot, we decide to use it.
    mavsdk.subscribe_on_new_system([&mavsdk, &prom]() {
        auto system = mavsdk.systems().back();

        if (system->has_autopilot()) {
            std::cout << "Discovered autopilot\n";

            // Unsubscribe again as we only want to find one system.
            mavsdk.subscribe_on_new_system(nullptr);
            prom.set_value(system);
        }
    });

    // We usually receive heartbeats at 1Hz, therefore we should find a
    // system after around 3 seconds max, surely.
    if (fut.wait_for(seconds(50)) == std::future_status::timeout) {
        std::cerr << "No autopilot found.\n";
        return {};
    }

    // Get discovered system now.
    return fut.get();
}

int main(int argc, char** argv)
{
    if (argc != 2) {
        usage(argv[0]);
        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 = get_system(mavsdk);
    if (!system) {
        return 1;
    }

    auto mission_raw = MissionRaw(system);
    auto action = Action(system);
    auto telemetry = Telemetry{system};
    bool is_armable = false;

    {
        auto set_rate_result = telemetry.set_rate_position(10.0);
        if (set_rate_result != Telemetry::Result::Success) {
            std::cerr << "Setting rate failed: " << set_rate_result << '\n';
            return 1;
        }
    }

    /*
        This one is neccesarry for ArduPilot because this subscription request
        SYS_STATUS message https://mavlink.io/en/messages/common.html#SYS_STATUS
        which is used for is_armable flag

        this is bug in case of ardupilot. See https://github.com/mavlink/MAVSDK/issues/1996
    */
    {
        auto set_rate_result = telemetry.set_rate_battery(10.0);
        if (set_rate_result != Telemetry::Result::Success) {
            std::cerr << "Setting bat rate failed: " << set_rate_result << '\n';
            return 1;
        }
    }

    telemetry.subscribe_health([&is_armable](Telemetry::Health h) { is_armable = h.is_armable; });

// debug timings for position messages
#if 0
    telemetry.subscribe_position([](Telemetry::Position p) {
        static auto tstatic = std::chrono::high_resolution_clock::now();
        auto t = high_resolution_clock::now();
        std::cout << "Pos HZ: " << 1e9/std::chrono::duration_cast<std::chrono::nanoseconds>(t-tstatic).count() << std::endl;
        tstatic = std::chrono::high_resolution_clock::now();
    });
#endif

#if 0
    std::cout << "subscribing battery status" << std::endl;
    telemetry.subscribe_battery([](Telemetry::Battery b) { std::cout << b << std::endl; });
#endif

    mission_raw.subscribe_mission_progress([&action](MissionRaw::MissionProgress progress) {
        std::cout << progress << std::endl;
// enable this if you want to pause mission halfway
#if 0
        // if (3 == progress.current) {
        //     action.hold();
        // }
#endif
    });

    /*
     * wait for armable flag, this is neccesarry,
     * because we should allow autopilot to initialize all its systems
     */
    wait_armable(is_armable);

    auto clear_result = mission_raw.clear_mission();
    if (clear_result != MissionRaw::Result::Success) {
        std::cout << "clear failed" << std::endl;
        return 1;
    }

    auto download_result = mission_raw.download_mission();
    if (download_result.first != MissionRaw::Result::Success) {
        std::cout << "Download failed" << std::endl;
        return 1;
    }

    // first point in case of ardupilot is always home
    auto mission_plan = download_result.second;
    MissionRaw::MissionItem home_point = mission_plan[0];

    // going relative alt mission so we dont care about altitude
    auto lat_deg = home_point.x * 1e-7;
    auto lon_deg = home_point.y * 1e-7;

    // in case of ardupilot we want to set lat lon to 0, to use current position as takeoff position
    mission_plan.push_back(make_mission_item_wp(
        0, // lat
        0, // lon
        5.0,
        0,
        MAV_FRAME_GLOBAL_RELATIVE_ALT,
        MAV_CMD_NAV_TAKEOFF));

    // setup speed during mission execution
    mission_plan.push_back(make_mission_item_wp(
        0, 0, 0, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_DO_CHANGE_SPEED, 9.35f, -1.0f));

    mission_plan.push_back(make_mission_item_wp(
        lat_deg + 0.0001,
        lon_deg + 0.0001,
        5.00,
        0,
        MAV_FRAME_GLOBAL_TERRAIN_ALT,
        MAV_CMD_NAV_WAYPOINT));

    mission_plan.push_back(make_mission_item_wp(
        lat_deg + 0.0001,
        lon_deg - 0.0001,
        5.00,
        0,
        MAV_FRAME_GLOBAL_TERRAIN_ALT,
        MAV_CMD_NAV_WAYPOINT));

    mission_plan.push_back(make_mission_item_wp(
        lat_deg - 0.0001, lon_deg, 5.00, 0, MAV_FRAME_GLOBAL_TERRAIN_ALT, MAV_CMD_NAV_WAYPOINT));

    mission_plan.push_back(make_mission_item_wp(
        lat_deg, lon_deg, 5.00, 0, MAV_FRAME_GLOBAL_TERRAIN_ALT, MAV_CMD_NAV_LAND));

    mission_plan.push_back(
        make_mission_item_wp(0, 0, 0, 0, MAV_FRAME_GLOBAL_INT, MAV_CMD_NAV_RETURN_TO_LAUNCH));

    auto upload_result = mission_raw.upload_mission(mission_plan);
    if (upload_result != MissionRaw::Result::Success) {
        std::cout << "upload failed" << std::endl;
        return 1;
    }

    mission_raw.set_current_mission_item(0);

    // start mission, this set autopilot auto mode.
    // ignore result, we dont care for now
    auto start_result = mission_raw.start_mission();
    // wait for two secs to be shure is_armable flag updated
    sleep_for(seconds(2));
    while (true) {
        /*
         * Need to wait for is_armable flag.
         * Ardupilot sets is_aramble flag in all non auto controlled modes earlier,
         * because it doesnt check position consistency
         * In auto controlled modes, we should wait a bit more so we recheck it again
         */
        wait_armable(is_armable);
        std::cout << "Arming...\n";
        const Action::Result arm_result = action.arm();
        switch (arm_result) {
            case Action::Result::Success: { // somehow this version of mavsdk sometime reset mode on
                                            // arm, so we explicitly set mode to auto again
                break;
            }
            case Action::Result::CommandDenied:
                // arming denied for some reason probably we are not ready yet or some serios error
                // in autopilot occured
                std::cerr << "Arming Denied: " << '\n';
                sleep_for(seconds(1));
                continue;
            default:
                std::cerr << "Arming failed: " << arm_result << '\n';
                return 1;
        }
        break;
    }

    // wait some time to allow copter to gain altitude, and only then start in_air check
    sleep_for(seconds(10));

    // Check if vehicle is still in air
    while (telemetry.in_air()) {
        sleep_for(seconds(1));
    }
    std::cout << "Landed!\n";

    // We are relying on auto-disarming but let's keep watching the telemetry for a bit longer.
    sleep_for(seconds(3));
    std::cout << "Finished...\n";
}