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

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

struct WayPoint {
    float x;
    float z;
};

std::vector wayPoints = {
    WayPoint{.x = -480.f, .z = 85.f},
    WayPoint{.x = -30.f, .z = 500.f},
    WayPoint{.x = 150.f, .z = 330.f},
    WayPoint{.x = 150.f, .z = 5.f},
};

JsonDocument doc;

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

float distance(const float x1, const float y1, const float x2, const float y2) {
    return static_cast<float>(
        sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2))
    );
}

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 auto target_psi = static_cast<float>(
        -degrees(atan2(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++;
        }
    }

    // Отправляем команды на симулятор
    // БЛА будет пытаться выдерживать заданные углы крена и тангажа
    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;
    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 uartBuffer;
bool ReceivedNewMessage(std::string &err) {
    while (Serial.available() > 0) {
        const char c = Serial.read();
        uartBuffer += c;
        if (c == '\n') { // Ожидание конца строки
            const DeserializationError error = deserializeJson(doc, uartBuffer);
            uartBuffer = ""; // Очистка буфера
            if (error.code() != DeserializationError::Ok) {
                err = error.c_str();
                return false;
            }
            return true;
        }
    }
    return false;
}

void printDebugMessage(const String &_) {
}

void ProcessNewMessage(std::string &err) {
    if (!doc.containsKey("path")) {
        err = "Missing 'path' key";
        printDebugMessage(String(err.c_str()));
        return;
    }

    // Получение массива точек
    const auto pointsArray = doc["path"].as<JsonArray>();
    if (pointsArray.isNull()) {
        err = "'wp' is not an array";
        printDebugMessage(String(err.c_str()));
        return;
    }

    std::vector<WayPoint> newWayPoints;

    // Обход всех элементов массива
    for (JsonVariant value : pointsArray) {
        auto point = value.as<JsonObject>();
        if (point.isNull()) {
            err = "Invalid waypoint format";
            printDebugMessage(String(err.c_str()));
            return;
        }

        // Проверка наличия полей x и z
        if (!point.containsKey("x") || !point.containsKey("z")) {
            err = "Waypoint missing x or z field";
            printDebugMessage(String(err.c_str()));
            return;
        }

        // Проверка типов данных
        if (!point["x"].is<float>() || !point["z"].is<float>()) {
            err = "Invalid x or z data type";
            printDebugMessage(String(err.c_str()));
            return;
        }

        // Извлечение значений
        float x = point["x"];
        float z = point["z"];
        newWayPoints.push_back(WayPoint{.x = x, .z = z});
    }

    // Проверка на пустой массив
    if (newWayPoints.empty()) {
        err = "No waypoints provided";
        printDebugMessage(String(err.c_str()));
        return;
    }

    wayPoints = std::move(newWayPoints);
    err.clear(); // Успешное выполнение без ошибок
}