#include <QCoreApplication>
#include <QDebug>

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

#include "az_mission.h"
#include "az_mission_controller.h"
#include "az_utils.h"


AzMissionController::AzMissionController(System &system, AzMission &mission, QObject *parent)
    : QObject(parent)
    , mAzMission(mission)
    , mMissionStarted(false)
    , mMissionItemSeqNum(0)
    , mMissionRaw(nullptr)
    , mSystem(system)
{}


MissionRaw::MissionItem AzMissionController::makeRawMissionItem(
    float latitudeDeg1e7,
    float longitudeDeg1e7,
    int32_t altitude,
    float doPhoto,
    MAV_FRAME frame,
    MAV_CMD command,
    float p2,
    float p3)
{
    (void)p2;
    (void)p3;

    MissionRaw::MissionItem new_item{};
    new_item.seq          = mMissionItemSeqNum;
    new_item.frame        = static_cast<uint32_t>(frame);
    new_item.command      = static_cast<uint32_t>(command);
    new_item.param1       = doPhoto;
    new_item.x            = latitudeDeg1e7 * 1e7;
    new_item.y            = longitudeDeg1e7 * 1e7;
    new_item.z            = altitude;
    new_item.mission_type = MAV_MISSION_TYPE_MISSION;
    new_item.autocontinue = 1;

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

    mMissionItemSeqNum++;

    return new_item;
}


bool AzMissionController::uploadMission(void)
{
    cout << "[MISSION] stateUploadMission() Setting raw mission starts" << endl;

    mMissionItemSeqNum = 0;

    if (mMissionRaw == nullptr) {
        mMissionRaw = new MissionRaw(mSystem);
    }

    auto clearResult = mMissionRaw->clear_mission();
    if (clearResult != MissionRaw::Result::Success) {
        std::cout << "[MISSION] stateUploadMission() Clearing mMissionRaw failed" << std::endl;
        return false;
    }

    auto downloadResult = mMissionRaw->download_mission();
    if (downloadResult.first != MissionRaw::Result::Success) {
        std::cout << "[MISSION] stateUploadMission() Downloading mission failed" << std::endl;
        return false;
    }

    // First point in case of ArduPilot is always home
    auto missionPlan = downloadResult.second;
    MissionRaw::MissionItem homePoint = missionPlan[0];
    missionPlan.clear();

    // Going relative alt mission so we dont care about altitude
    auto latDeg = homePoint.x * 1e-7;
    auto lonDeg = homePoint.y * 1e-7;
    cout << "[MISSION] Home location: " << latDeg << ", " << lonDeg << endl;

    // In case of ardupilot we want to set lat lon to 0, to use current position as takeoff position
    missionPlan.push_back(makeRawMissionItem(
        0, // lat
        0, // lon
        50.0,
        0,
        MAV_FRAME_GLOBAL_RELATIVE_ALT,
        MAV_CMD_NAV_TAKEOFF));

    // Setup speed during mission execution
    missionPlan.push_back(makeRawMissionItem(
        0, 0, 0, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_DO_CHANGE_SPEED, 9.35f, -1.0f));

    // Action points
    const vector<AzActionPoint> &azMissions = mAzMission.getActionPoints();
    for (uint i = 0; i < azMissions.size(); i++) {
        missionPlan.push_back(makeRawMissionItem(
            azMissions[i].getPoint().latitude,
            azMissions[i].getPoint().longitude,
            azMissions[i].getHeight(),
            0,
            MAV_FRAME_GLOBAL_TERRAIN_ALT,
            MAV_CMD_NAV_WAYPOINT));
    }

    // Landing point
    missionPlan.push_back(makeRawMissionItem(
        mAzMission.getReturnPoint().latitude,
        mAzMission.getReturnPoint().longitude,
        50.00,
        0,
        MAV_FRAME_GLOBAL_TERRAIN_ALT,
        MAV_CMD_NAV_LAND));

    //missionPlan.push_back(makeRawMissionItem(0, 0, 0, 0, MAV_FRAME_GLOBAL_INT, MAV_CMD_NAV_RETURN_TO_LAUNCH));

    for (const auto& item : missionPlan) {
        std::cout << "[MISSION] stateUploadMission() seq: " << (int)item.seq << '\n';
    }

    auto uploadResult = mMissionRaw->upload_mission(missionPlan);
    if (uploadResult != MissionRaw::Result::Success) {
        std::cout << "[MISSION] stateUploadMission() upload failed. Result: " << uploadResult << std::endl;
        return false;
    }

    mMissionRaw->set_current_mission_item(0);
    cout << "[MISSION] stateUploadMission() Setting raw mission ends" << endl;

    return true;
}


bool AzMissionController::startMission()
{
    MissionRaw::Result startResult = mMissionRaw->start_mission();
    cout << "[MISSION] Start mission result:" << startResult << endl;
    return startResult == MissionRaw::Result::Success;
}


void AzMissionController::stopMission(void)
{
    mMissionRaw->pause_mission();
}


void AzMissionController::newPosition(Telemetry::Position pos)
{
    (void)pos;

    if (mMissionStarted == false) {
        return;
    }
}