/** Copyright 2025 The Magrob Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#pragma once
#include "handlers/dwm_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 DWM<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;

        return b > goal->tolerance;
    }

    template <>
    inline void DWM<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 DWM<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 DWM<MgNavigationServer::LookAt>::condition_check() {
        return false;
    }

    template <>
    inline double DWM<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;

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

        return score;
    }

    template <>
    inline void DWM<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);
            }
        }
    }

}