#include "Tasks/Task.h"
#include <cmath>
#include <vector>
#define ARDUINOJSON_ENABLE_PROGMEM 0
#include <ArduinoJson.h>

// Настройки
constexpr float Ki = 0.03f;
constexpr float waypoint_radius = 3.f;
constexpr float pitch_constraint = 20.f;
constexpr float yaw_constraint = 20.f;

// https://github.com/GyverLibs/AsyncStream @ 1.1
// https://github.com/bblanchon/ArduinoJson @ 7.3.1

struct WayPoint {
    float x;
    float z;
    float y;
};

std::vector<WayPoint> wayPoints = {};

JsonDocument doc;
bool cargoDrop = false;

String HandleMessages();
bool ReceiveNewMessage(String &err);
void RouteNewMessage(String &err);
void HandleNewWaypoints(String &err);
void HandleDropCargo();

void Task_solution::setup(HardwareSerial *a_Serial) {
    debug_serial = a_Serial;
    _Point_Index = 0;
    Serial2.begin(115200);
}

void printDebugMessage(const String &_) {
}

float distance(const float x1, const float y1, const float x2, const float y2) {
    return sqrtf(powf(x2 - x1, 2.f) + powf(y2 - y1, 2.f));
}

bool isEqual(const float a, const float b, const float accuracy) {
    return abs(a - b) <= accuracy;
}

/*
### УЧАСТНИКАМ ОЛИМПИАДЫ  -  Основная функция отправки пакета значений крена и тангажа (град) ####
Данная функция осуществляет циклический обмен данных с симулятором с частотой 100 Гц.
*/
SignalBody Task_solution::loop(Skywalker2015PacketTelemetry a_telemetry) {
    /* a_telemetry - структура данных, полученная с симулятора. Она содержит текущие параметры БЛА
    a_telemetry.L - координата Х (север) [м]
    a_telemetry.Z - координата Z (восток) [м]
    a_telemetry.H - координата Y (высота) [м]
    a_telemetry.Psi - угол курса [град]
    a_telemetry.Gam - Угол крена [град]
    a_telemetry.Tan - Угол тангажа [град]
    a_telemetry.V - Скорость полёта БЛА [м/с]
    a_telemetry.Vx1 - Продольная скорость [м/с]
    a_telemetry.Vz1 - Поперечная скорость [м/с]
    a_telemetry.Vy1 - Вертикальная скорость [м/с]
    a_telemetry.wx - Угловая скорость вокруг продольной оси [1/с]
    a_telemetry.wy - Угловая скорость вокруг вертикальной оси [1/с]
    a_telemetry.wz - Угловая скорость вокруг поперечной оси [1/с]
    */

    // РЕКОМЕНДУЕМЫЙ АЛГОРИТМ РЕШЕНИЯ ЗАДАЧИ 4
    //
    SignalBody _ans; // Структура которая отсылается на симулятор
    // Проверить близость БЛА к текущей путевой точке. Изменить путевую точку при необходимости
    // _Point_Index = GetNowPointIndex(a_telemetry.L, a_telemetry.Z, a_telemetry.H);
    const float target_x = wayPoints[_Point_Index].x;
    const float target_y = wayPoints[_Point_Index].z;
    const float target_height = wayPoints[_Point_Index].y;

    const auto target_psi = static_cast<float>(
        -degrees(atan2f(target_y - a_telemetry.Z, target_x - a_telemetry.L))
    );

    _ans.Gamma_direct = GammaReg(a_telemetry.Psi, target_psi);
    _ans.Tang_direct = HeightReg(a_telemetry.H, a_telemetry.Vy1, target_height);

    const float dist_to_target = distance(a_telemetry.L, a_telemetry.Z, target_x, target_y);
    if (dist_to_target <= waypoint_radius) {
        // Проверяем расстояние до цели
        if (_Point_Index < wayPoints.size() - 1) {
            _Point_Index++;
        }
    }

    if (const String err = HandleMessages(); !err.isEmpty()) {
        printtoDebugSerial(err);
    }

    if (cargoDrop) {
        // Propagate changed state
        cargo_drop = cargoDrop;
        cargoDrop = false;
    }


    // Отправляем команды на симулятор
    // БЛА будет пытаться выдерживать заданные углы крена и тангажа
    return _ans;
}

float roll_err_last = 0.f;
float integral_err = 0.05f;

float Task_solution::GammaReg(float Psi, float Psi_dir) {
    float roll_err = AngDefines(Psi - Psi_dir);
    integral_err += roll_err * 10.f; // ошибка: 10.f вместо 0.01f
    integral_err = constrain(integral_err, -50.f, 50.f);
    float gamma_cmd = 2.0f * roll_err + (Ki * integral_err) + 35.f * (roll_err - roll_err_last);
    roll_err_last = roll_err;
    gamma_cmd = constrain(gamma_cmd, -yaw_constraint, yaw_constraint);
    return gamma_cmd;
}

float Task_solution::HeightReg(float Yg, float Vy, float Hz) {
    constexpr float K_H = 0.56f;
    constexpr float K_vy_1 = 7.7591f;
    constexpr float K_vy_2 = 9.8941f;
    const float kh1 = constrain(K_H * (Hz - Yg), -7.5f, 7.5f);
    const float Pitch_direct = constrain((kh1 * K_vy_1 - Vy * K_vy_2), -pitch_constraint, pitch_constraint);
    return Pitch_direct;
}

String HandleMessages() {
    String err;
    if (ReceiveNewMessage(err)) {
        RouteNewMessage(err);
    }
    return err;
}

String uartBuffer;
bool ReceiveNewMessage(String &err) {
    while (Serial2.available() > 0) {
        if (const char c = Serial2.read(); c == '\n') { // Ожидание конца строки
            if (uartBuffer.length() <= 1) {
                printDebugMessage("Invalid message");
            }
            const DeserializationError error = deserializeJson(doc, uartBuffer);
            uartBuffer = ""; // Очистка буфера
            if (error.code() != DeserializationError::Ok) {
                err = error.c_str();
                return false;
            }
            return true;
        } else {
            uartBuffer += c;
        }
    }
    return false;
}

void RouteNewMessage(String &err) {
    if (!doc["cmd"].is<String>()) {
        err = "Required 'cmd' field";
        printDebugMessage(err);
        return;
    }

   if (doc["cmd"] == "add_wp") {
       HandleNewWaypoints(err);
   } else if (doc["cmd"] == "drop") {
       HandleDropCargo();
   } else {
       err = "Unknown command";
   }
}

void HandleNewWaypoints(String &err) {
    if (!doc["path"].is<JsonArray>()) {
        err = "Required 'path' field";
        printDebugMessage(err);
        return;
    }
    const auto pointsArray = doc["path"].as<JsonArray>();
    for (const auto &p : pointsArray) {
        if (!p["x"].is<float>() || !p["y"].is<float>() || !p["z"].is<float>()) {
            err = "Missing point field";
            continue;
        }
        WayPoint point{};
        point.x = p["x"].as<float>();
        point.z = p["y"].as<float>();
        point.y = p["z"].as<float>();
        wayPoints.push_back(point);
    }
}

void HandleDropCargo() {
    cargoDrop = true;
}