#include "mg_planner/mg_planner_node.hpp"
#include "mg_msgs/srv/calc_path.hpp"
#include "mg_planner/a_star/a_star.hpp"
#include "mg_planner/config.hpp"
#include <array>
#include <exception>
#include <iostream>
#include <memory>
#include <rclcpp/time.hpp>
#include <rclcpp/wait_result_kind.hpp>

#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2/convert.hpp"

mg::PlannerNode::PlannerNode() : Node("planner"), astar_(this) {
    astar_service = create_service<mg_msgs::srv::CalcPath>(
      "GeneratePath",
      [this](mg_msgs::srv::CalcPath::Request::ConstSharedPtr req,
             mg_msgs::srv::CalcPath::Response::SharedPtr     resp) -> void {
          std::cout << "Bro recieved request\n";
          auto                    elapsed = get_clock()->now();
          std::vector<glm::ivec2> goals;
          std::transform(req->x.begin(),
                         req->x.end(),
                         req->y.begin(),
                         std::back_inserter(goals),
                         [](const auto& aa, const auto& bb) {
                             std::cout << aa << " " << bb << "\n";
                             return AStar::CoordsToGrid(glm::ivec2(aa * 1000, bb * 1000));
                         });
          for (const auto& goal : goals) {
              if (goal.x < 0 || GRID_X < goal.x || goal.y < 0 || GRID_Y < goal.y) {
                  return;
              }
          }
          astar_.execute(get_pos(), std::move(goals), resp->indicies);
          std::reverse(resp->indicies.begin(), resp->indicies.end());
          mg_msgs::msg::Path path;
          path.indicies = std::vector<int>(resp->indicies);
          path_pub_->publish(path);
          RCLCPP_INFO(get_logger(), "Handled request in %lf seconds", (get_clock()->now() - elapsed).seconds());
      });

    path_pub_ = create_publisher<mg_msgs::msg::Path>("/GeneratedPath", rclcpp::QoS(4).keep_last(2));

    tf_buf_     = std::make_shared<tf2_ros::Buffer>(get_clock());
    tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buf_);

    obstacle_manager_ = std::make_shared<ObstacleManager>(this, tf_buf_);
}

glm::ivec2 mg::PlannerNode::get_pos() {
    try {
        auto tmsg = tf_buf_->lookupTransform("odom", "base_footprint", tf2::TimePointZero);

        tf2::Transform t;
        tf2::convert(tmsg.transform, t);
        auto vec3 = tmsg.transform.translation;

        return glm::clamp(
          glm::ivec2{ vec3.x * 1000, vec3.y * 1000 }, glm::ivec2(MIN_X, MIN_Y), glm::ivec2(MAX_X, MAX_Y));

    } catch (const std::exception& e) { RCLCPP_DEBUG(get_logger(), "Failed to find transform, ohno"); }
    return { 1000, 1000 };
}