#include "Tasks/Task.h"
#include <cmath>
#include <vector>

// Данная функция выполняется только один раз при запуске программы
// (при подаче питания на плату, перепрошивке, или нажатии кнопки reset)

// Настройки
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<WayPoint> 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},
};

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

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) {
        // Проверяем расстояние до цели (радиус 5 метров)
        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;
}