#include <chrono>
#include <cmath>
#include <cstdint>
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/manual_control/manual_control.h>
#include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
#include <iostream>
#include <fstream>
#include <functional>
#include <future>
#include <memory>
#include <thread>

#include "mavsdk_wrapper.h"

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

#define ERROR_CONSOLE_TEXT "\033[31m" // Turn text on console red
#define NORMAL_CONSOLE_TEXT "\033[0m" // Restore normal console colour

static std::ofstream log_file_fd;

static Mavsdk _mavsdk;
static Telemetry * telemetry;
static Action * action;
static ManualControl * manual_control;
static MavlinkPassthrough * mavlink_passthrough;
static std::shared_ptr<System> msystem;

static auto prom = std::promise<std::shared_ptr<System>>{};
static std::future<std::shared_ptr<System>> fut;

static const double EARTH_GRAVITY = 9.81;
static const double EARTH_RADIUS = 6371000.F;
static const double MIN_YAW_DIFF = 1;
static const double MAX_ROLL = 35;
static const double YAW_VELOCITY_COEF = 0.5;

static bool autocontinue = false;
static double initialBearing;
static double previousBearing;
static double xy_ratio;
static double target_latitude;
static double target_longitude;
static int mavsdk_started = 0;
static Telemetry::Position origin;

// Logs functions

static void log(std::string message) {
    log_file_fd << message << std::endl;
}

static void log_error(std::string message) {
    log(ERROR_CONSOLE_TEXT + message + NORMAL_CONSOLE_TEXT);
}

template <class Enumeration>
static void log_error_from_result(std::string message, Enumeration result) {
  std::ostringstream oss;
  oss << message << ": " << result;
  log_error(oss.str());
}

// Action functions

static int doAction(Action::Result (Action::*toDo)(void) const, std::string failure_message) {
    if(!mavsdk_started) {
        log_error("Mavsdk not started");
        return -1;
    }

    const Action::Result result = (action->*toDo)();
    if(result != Action::Result::Success) {
        log_error_from_result(failure_message, result);
        return -1;
    }
    return 0;
}

static int doMavlinkCommand(MavlinkPassthrough::CommandLong command, std::string failure_message) {
    const MavlinkPassthrough::Result result = mavlink_passthrough->send_command_long(command);
    if(result != MavlinkPassthrough::Result::Success) {
        log_error_from_result(failure_message, result);
        return -1;
    }
    return 0;
}

// Connexion management functions

static double toRad(double angle) {
    return M_PI * angle / 180;
}

static double toDeg(double angle) {
    return 180 * angle / M_PI;
}

int mavsdk_start(const char * url, const char * log_file, int timeout) {
    std::string connection_url(url);
    ConnectionResult connection_result;
    float abs_altitude;
    log_file_fd.open(log_file);

    connection_result = _mavsdk.add_any_connection(connection_url);
    if (connection_result != ConnectionResult::Success) {
        log_error_from_result("Connection failed", connection_result);
        return -1;
    }

    log("Waiting to discover msystem...");
    fut = prom.get_future();

    _mavsdk.subscribe_on_new_system([]() {
        auto msystem_tmp = _mavsdk.systems().back();

        if (msystem_tmp->has_autopilot()) {
            log("Discovered autopilot");

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

    if (fut.wait_for(seconds(timeout)) == std::future_status::timeout) {
        log_error("No autopilot found, exiting.");
        return -1;
    }

    msystem = fut.get();
    telemetry = new Telemetry(msystem);
    action = new Action(msystem);
    manual_control = new ManualControl(msystem);
    mavlink_passthrough = new MavlinkPassthrough(msystem);

    log("Subscribing to raw GPS...");
    telemetry->subscribe_raw_gps([](Telemetry::RawGps gps) {
        std::ostringstream oss;
        Telemetry::EulerAngle euler_angle = telemetry->attitude_euler();
        Telemetry::FixedwingMetrics metrics = telemetry->fixedwing_metrics();
        Telemetry::Position position = telemetry->position();

        oss << gps.timestamp_us << "; "
            << gps.latitude_deg << "; " << gps.longitude_deg << "; "
            << gps.absolute_altitude_m << "; " << position.relative_altitude_m << "; "
            << euler_angle.pitch_deg << "; " << euler_angle.roll_deg << "; " << euler_angle.yaw_deg << "; "
            << metrics.airspeed_m_s << "; " << metrics.throttle_percentage << "; " << metrics.climb_rate_m_s;
        log(oss.str());
    });

    do {
        log("Waiting for first telemetry");
        sleep_for(seconds(1));
        abs_altitude = mavsdk_getAltitude();
    } while(isnan(abs_altitude) || abs_altitude == 0);
    origin = telemetry->position();
    xy_ratio = std::cos(toRad(origin.latitude_deg));

    log("MAVSDK started...");
    mavsdk_started = 1;
    log("timestamp; latitude; longitude; AMSL (m); rel altitude (m); pitch (°); roll(°); yaw(°); air speed (m/s); throttle(%); climb rate(m/s)");
    return 0;
}

int mavsdk_stop() {
    if (!mavsdk_started)
        return -1;
    
    if (!mavsdk_landed()) {
      log_error("You must land first !");
      return -1;
    }

    if (doAction(&Action::shutdown, "Shutdown failed"))
        return -1;

    // Delete pointers
    delete action;
    delete manual_control;
    delete mavlink_passthrough;
    delete telemetry;
    log_file_fd.close();

    return 0;
}

int mavsdk_reboot() {
    return doAction(&Action::reboot, "Rebooting failed");
}

// Flight state management functions

int mavsdk_arm(void) {
    if(!mavsdk_started)
        return -1;

    while(!telemetry->health().is_home_position_ok) {
        log("Waiting for home position to be set");
        sleep_for(seconds(1));
    }

    log("Arming...");
    return doAction(&Action::arm, "Arming failed");
}

int mavsdk_land(void) {
    log("Landing...");
    if (doAction(&Action::terminate, "Land failed"))
        return -1;

    // Check if vehicle is still in air
    while (telemetry->in_air()) {
        log("Vehicle is landing...");
        sleep_for(seconds(1));
    }
    log("Landed!");

    while (telemetry->armed())
        sleep_for(seconds(1));
    log("Finished...");

    return 0;
}

int mavsdk_takeOff(void) {
    if (doAction(&Action::takeoff, "Takeoff failed"))
        return -1;

    while (telemetry->flight_mode() != Telemetry::FlightMode::Takeoff)
        sleep_for(seconds(1));

    log("Taking off...");
    return 0;
}

int mavsdk_takeOffAndWait(void) {
    if (mavsdk_takeOff() < 0)
        return -1;

    while (telemetry->flight_mode() == Telemetry::FlightMode::Takeoff)
        sleep_for(seconds(1));
    return 0;
}

int mavsdk_triggerParachute(void) {
    if (!mavsdk_started)
        return -1;

    MavlinkPassthrough::CommandLong command;
    command.command = MAV_CMD_DO_PARACHUTE;
    command.param1 = 2; //see https://mavlink.io/en/messages/common.html#PARACHUTE_ACTION
    command.target_sysid = mavlink_passthrough->get_target_sysid();
    command.target_compid = mavlink_passthrough->get_target_compid();
    return doMavlinkCommand(command, "Parachute release failed");
}

// Flight management functions

int mavsdk_loiter(float radius) {
    if (!mavsdk_started)
        return -1;
    Telemetry::Position position = telemetry->position();

    MavlinkPassthrough::CommandLong command;
    command.command = MAV_CMD_DO_REPOSITION;
    command.param1 = -1; // Ground speed, -1 for default
    command.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE; //will go to navigate mode
    command.param3 = radius; // loiter radius
    command.param5 = (float)position.latitude_deg;
    command.param6 = (float)position.longitude_deg;
    command.param7 = position.absolute_altitude_m; //altitude is ignored, use altitude override package
    command.target_sysid = mavlink_passthrough->get_target_sysid();
    command.target_compid = mavlink_passthrough->get_target_compid();

    return doMavlinkCommand(command, "Entering loiter mode failed");
}

int mavsdk_setAirspeed(float airspeed) {
    if (!mavsdk_started)
        return -1;

    MavlinkPassthrough::CommandLong command;
    command.command = MAV_CMD_DO_OVERRIDE;
    command.param1 = 1 | 2 | 4 | 8;
    command.param2 = 2 | 4 | 8;
    command.param4 = airspeed;
    command.target_sysid = mavlink_passthrough->get_target_sysid();
    command.target_compid = mavlink_passthrough->get_target_compid();

    return doMavlinkCommand(command, "Setting airspeed failed");
}

int mavsdk_setAltitude(float altitude) {
    if (!mavsdk_started)
        return -1;

    MavlinkPassthrough::CommandLong command;
    command.command = MAV_CMD_DO_OVERRIDE;
    command.param1 = 1 | 2 | 4 | 8;
    command.param2 = 1 | 2 | 8;
    command.param3 = altitude;
    command.target_sysid = mavlink_passthrough->get_target_sysid();
    command.target_compid = mavlink_passthrough->get_target_compid();

    return doMavlinkCommand(command, "Setting altitude failed");
}

static int startAltitudeControl(void) {
    const ManualControl::Result result = manual_control->start_altitude_control();
    if (result != ManualControl::Result::Success) {
        log_error_from_result("Set manual control failed!", result);
        return -1;
    }
    return 0;
}

static double bearing(double lat1, double lon1, double lat2, double lon2) {
    double lat1_rad = toRad(lat1);
    double lat2_rad = toRad(lat2);
    double dL = toRad(lon2 - lon1);
    double x = cos(lat2_rad) * sin(dL);
    double y = cos(lat1_rad) * sin(lat2_rad) - sin(lat1_rad) * cos(lat2_rad) * cos(dL);
    return atan2(x, y) * 180 / M_PI;
}

int mavsdk_setTargetCoordinates(double la, double lo, float a) {
    int result = 0;
    Telemetry::Position position = telemetry->position();

    if (!mavsdk_started)
        return -1;

    target_latitude = la;
    target_longitude = lo;
    initialBearing = previousBearing = bearing(position.latitude_deg,
                                               position.longitude_deg,
                                               la, lo);
    autocontinue = false;

    if (!mavsdk_isInManualMode()) {
        result = mavsdk_setManualControlInput();
        result |= startAltitudeControl();
    }
    result |= mavsdk_setAltitude(a);
    return result;
}

static int setManualControlInput(float x, float y, float z, float r) {
    const ManualControl::Result result = manual_control->set_manual_control_input(x, y, z, r);
    if(result != ManualControl::Result::Success) {
        log_error_from_result("Set manual control input failed!", result);
        return -1;
    }
    return 0;
}

int mavsdk_setManualControlInput(void) {
    double b;
    float speed = mavsdk_getSpeed();

    if (autocontinue) {
        b = initialBearing;
    } else {
        Telemetry::Position position = telemetry->position();
        b = bearing(position.latitude_deg, position.longitude_deg,
                    target_latitude, target_longitude);
/*
  If there is a difference of more than 160° (180° for a half circle and 20° of imprecision)
  between the required bearing and the one some milliseconds ago,
  it means that the target is now behind the drone so the drone just went over the target.

  In this case, we don't follow the target anymore, we simply keep the same trajectory.
*/
        if (abs(b - previousBearing) > 160) {
            autocontinue = true;
            return 0;
        }
        previousBearing = b;
    }

    float angle_diff = (float)b - mavsdk_getYaw();
    if (angle_diff < -180) {
        angle_diff += 360;
    } else if (angle_diff > 180) {
        angle_diff -= 360;
    }

    if (abs(angle_diff) < MIN_YAW_DIFF) {
        initialBearing = b;
        return 0;
    }

    /*
    * radius is speed²/g*tan(B) where B is roll angle
    * let's define yaw angular velocity Ys as speed*360/perimeter
    * so tan(B) = 2PI*Ys*speed / 360*g
    */
    double wanted_yaw_velocity = angle_diff * YAW_VELOCITY_COEF;
    double roll = toDeg(atan(
      2 * M_PI * wanted_yaw_velocity * speed / (360 * EARTH_GRAVITY)
    ));
    return setManualControlInput(
      0,
      (float)((roll > 0 ? 1 : -1) * std::min(abs(roll), MAX_ROLL) / MAX_ROLL),
      0,
      0
    );
}

// Information functions

float mavsdk_getAltitude(void) {
    return telemetry->position().absolute_altitude_m;
}

float mavsdk_getAltitudeRel(void) {
    return telemetry->position().relative_altitude_m;
}

float mavsdk_getInitialAltitude(void) {
    return origin.absolute_altitude_m;
}

double mavsdk_getInitialLatitude(void) {
    return origin.latitude_deg;
}

double mavsdk_getInitialLongitude(void) {
    return origin.longitude_deg;
}

double mavsdk_getLatitude(void) {
    return telemetry->position().latitude_deg;
}

double mavsdk_getLongitude(void) {
    return telemetry->position().longitude_deg;
}

double *mavsdk_getPositionArray(void) {
    Telemetry::Position position = telemetry->position();
    double *positionArray = (double*) malloc(POSITION_ARRAY_SIZE * sizeof(double));
    positionArray[0] = position.latitude_deg;
    positionArray[1] = position.longitude_deg;
    positionArray[2] = (double) position.absolute_altitude_m;
    positionArray[3] = (double) position.relative_altitude_m;
    return positionArray;
}

float *mavsdk_getSpeedArray(void) {
    Telemetry::FixedwingMetrics metrics = telemetry->fixedwing_metrics();
    float *speedArray = (float*) malloc(SPEED_ARRAY_SIZE * sizeof(float));
    speedArray[0] = mavsdk_getYaw();
    speedArray[1] = metrics.airspeed_m_s;
    speedArray[2] = metrics.climb_rate_m_s;
    return speedArray;
}

double mavsdk_getTakeOffAltitude(void) {
    const std::pair<Action::Result, float> response = action->get_takeoff_altitude();

    if (response.first != Action::Result::Success) {
        log_error_from_result("Get takeoff altitude failed", response.first);
        return -1;
    }
    return response.second;
}

float mavsdk_getYaw(void) {
    return telemetry->attitude_euler().yaw_deg;
}

float mavsdk_getSpeed(void) {
    return telemetry->fixedwing_metrics().airspeed_m_s;
}

float mavsdk_getClimbRate(void) {
    return telemetry->fixedwing_metrics().climb_rate_m_s;
}

int mavsdk_healthAllOk(void) {
    return telemetry->health_all_ok();
}

bool mavsdk_isInManualMode(void) {
    return telemetry->flight_mode() == Telemetry::FlightMode::Altctl;
}

int mavsdk_landed(void) {
    return !telemetry->in_air();
}