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

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

#define LOOKAHEAD 0.2

namespace mg {

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

    template <>
    inline void DWA<MgNavigationServer::MoveGlobal>::target_init() {
        baseNode.path_buffer()->update_path_block(goal);
        target_pos = baseNode.path_buffer()->get_next(pos, LOOKAHEAD);

        dimy = 4;
        dimx = 11;
    }

    template <>
    inline void DWA<MgNavigationServer::MoveGlobal>::target_update() {
        baseNode.path_buffer()->update_path();
        target_pos = baseNode.path_buffer()->get_next(pos, LOOKAHEAD);
    }

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

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

        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);

        // const double angl   = glm::angle(face, glm::normalize(target_pos - pos));
        // const double n_angl = glm::angle(n_face, glm::normalize(target_pos - n_pos));

        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) }));

        const double dist            = baseNode.obstacle_manager()->box_colide(n_pos, { 0.29, 0.33 }, n_theta);
        const double dist2           = baseNode.obstacle_manager()->dist_to_nearest(n_pos) - 0.22;
        const double obstacle_scoreA = glm::max(0.0, 0.02 - dist);
        const double obstacle_scoreB = glm::max(0.0, 0.03 - dist2);

        // RCLCPP_INFO(baseNode.get_logger(), "Door Stuck %lf", dist);
        // RCLCPP_INFO(baseNode.get_logger(), "Door Stuck2 %lf", dist2);

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

        return score;
    }

    template <>
    inline void DWA<MgNavigationServer::MoveGlobal>::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);
                }
            }
        }
    }

}