#pragma once
#include "handlers/dwa_core.hpp"

#include "glm/gtx/norm.hpp"
#include "glm/gtx/rotate_vector.hpp"
#include "glm/gtx/vector_angle.hpp"
#include <glm/gtc/constants.hpp>

namespace mg {

    template <>
    inline bool DWA<MgNavigationServer::LookAt>::target_check() {

        const double a = glm::abs(theta - target_ornt);
        const double b = (a > glm::pi<double>()) ? glm::two_pi<double>() - a : a;
        const double c = (b > glm::pi<double>() / 2) ? glm::pi<double>() - b : b;

        return c > goal->tolerance;
    }

    template <>
    inline void DWA<MgNavigationServer::LookAt>::target_init() {

        target_pos = glm::vec2(goal->x, goal->y);

        const double angle = glm::angle(glm::dvec2(1, 0), glm::normalize(target_pos - pos));
        const double cross = glm::cross(glm::dvec3(1, 0, 0), glm::dvec3(target_pos - pos, 0)).z;
        const double sign  = (cross < 0) ? -1 : 1;

        target_ornt = angle * sign;
    }

    template <>
    inline void DWA<MgNavigationServer::LookAt>::target_update() {

        const double angle = glm::angle(glm::dvec2(1, 0), glm::normalize(target_pos - pos));
        const double cross = glm::cross(glm::dvec3(1, 0, 0), glm::dvec3(target_pos - pos, 0)).z;
        const double sign  = (cross < 0) ? -1 : 1;

        target_ornt = angle * sign;
    }

    template <>
    inline bool DWA<MgNavigationServer::LookAt>::condition_check() {
        return false;
    }

    template <>
    inline double DWA<MgNavigationServer::LookAt>::calc_score(int vl, int vr) {
        constexpr double delta = 0.5;
        const auto [v, w]      = to_vel(step * vl, step * vr);
        const double n_theta   = theta + w * delta;

        double dist_old = glm::abs(target_ornt - theta);
        double dist_new = glm::abs(target_ornt - n_theta);

        dist_old = (dist_old > glm::pi<double>()) ? glm::two_pi<double>() - dist_old : dist_old;
        dist_new = (dist_new > glm::pi<double>()) ? glm::two_pi<double>() - dist_new : dist_new;

        dist_old = (dist_old > glm::pi<double>() / 2) ? glm::pi<double>() - dist_old : dist_old;
        dist_new = (dist_new > glm::pi<double>() / 2) ? glm::pi<double>() - dist_new : dist_new;

        const double score = goal->ornt_mult * (dist_old - dist_new);

        return score;
    }

    template <>
    inline void DWA<MgNavigationServer::LookAt>::populate_candidates(std::vector<int>& vl,
                                                                     std::vector<int>& vr,
                                                                     int               dimx,
                                                                     int) {
        vl.clear();
        vr.clear();

        for (int i = -dimx / 2; i <= dimx / 2; i++) {
            auto [v, w] = to_vel(step * (cvl + i), step * (cvr + i));

            if (step * abs(cvl - i) <= goal->max_wheel_speed && step * abs(cvr + i) <= goal->max_wheel_speed
                && glm::abs(w) < goal->max_angular) {
                vl.push_back(cvl - i);
                vr.push_back(cvr + i);
            }
        }
    }

}