#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::MoveStraight>::target_check() {
        return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
    }

    template <>
    inline void DWA<MgNavigationServer::MoveStraight>::target_init() {
        target_pos = glm::rotate(glm::dvec2(goal->distance, 0), theta) + pos;
    }

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

    template <>
    inline double DWA<MgNavigationServer::MoveStraight>::calc_score(int vl, int vr) {
        constexpr double delta = 0.8;

        auto [v, w] = to_vel(step * vl, step * vr);

        const glm::dvec2 dp    = glm::rotate(glm::dvec2(1, 0), theta) * delta * v;
        const glm::dvec2 n_pos = dp + pos;

        const double score = goal->pos_mult * (glm::distance(target_pos, pos) - glm::distance(target_pos, n_pos));

        return score;
    }

    template <>
    inline void DWA<MgNavigationServer::MoveStraight>::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 && glm::abs(v) < goal->max_vel) {
                vl.push_back(cvl + i);
                vr.push_back(cvr + i);
            }
        }
    }

}