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

#include "glm/gtx/norm.hpp"
#include "glm/gtx/rotate_vector.hpp"
#include "glm/gtx/vector_angle.hpp"

namespace mg {

    template <>
    inline bool DWA<MgNavigationServer::MovePoint>::target_check() {
        return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
    }

    template <>
    inline void DWA<MgNavigationServer::MovePoint>::target_update() {
        target_pos = glm::dvec2(goal->x, goal->y);
    }

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

    template <>
    inline double DWA<MgNavigationServer::MovePoint>::calc_score(int vl, int vr) {
        auto [v, w]        = to_vel(step * vl, step * vr);
        const double delta = 0.5;
        glm::dvec2   n_pos;
        double       n_theta = NAN;
        double       score   = 0;

        // The angle we will be facing after lookahead
        n_theta = w * delta;

        if (n_theta <= 1e-6) { //NOLINT
            n_theta += theta;
            const glm::dvec2 dp = glm::rotate(glm::dvec2(1, 0), theta) * delta * v;
            n_pos               = dp + pos;
        } else {
            n_theta += theta;
            const double r = v / w;
            n_pos.x        = r * (glm::sin(n_theta) - glm::sin(theta));
            n_pos.y        = -r * (glm::cos(n_theta) - glm::cos(theta));
            n_pos += pos;
        }

        const glm::dvec2 n_face = glm::rotate(glm::dvec2(1, 0), n_theta);
        const glm::dvec2 face   = glm::rotate(glm::dvec2(1, 0), theta);

        // Calculate angle to goal post/pre movement
        const double angl   = glm::asin(glm::determinant(glm::dmat2{ face, glm::normalize(target_pos - pos) }));
        const double n_angl = glm::asin(glm::determinant(glm::dmat2{ n_face, glm::normalize(target_pos - n_pos) }));

        score += goal->pos_mult * (glm::distance(target_pos, pos) - glm::distance(target_pos, n_pos));
        score += goal->ornt_mult * (glm::abs(angl) - glm::abs(n_angl));

        return score;
    }

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

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

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

}