#include "origincar_base/wall_fit_core.hpp"

#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>

#include <poll.h>
#include <termios.h>
#include <unistd.h>

#include <atomic>
#include <cmath>
#include <map>
#include <mutex>
#include <string>
#include <thread>
#include <vector>

namespace
{
std::vector<origincar_wall::WallPoint> scanMsgToPoints(const sensor_msgs::msg::LaserScan &msg)
{
  std::vector<origincar_wall::WallPoint> points;
  points.reserve(msg.ranges.size());
  for (std::size_t index = 0; index < msg.ranges.size(); ++index)
  {
    const auto distance = static_cast<double>(msg.ranges[index]);
    if (!std::isfinite(distance) || distance < msg.range_min || distance > msg.range_max)
    {
      continue;
    }
    const auto angle = static_cast<double>(msg.angle_min) + static_cast<double>(index) * msg.angle_increment;
    points.push_back({distance * std::cos(angle), distance * std::sin(angle)});
  }
  return points;
}

origincar_wall::Pose2D odomMsgToMapPose(const nav_msgs::msg::Odometry &msg,
                                        const origincar_wall::WallFitConfig &config)
{
  const auto &q = msg.pose.pose.orientation;
  const auto siny_cosp = 2.0 * (q.w * q.z + q.x * q.y);
  const auto cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z);
  return {
      msg.pose.pose.position.x + config.coordinate_frame.odom_zero_map.x,
      msg.pose.pose.position.y + config.coordinate_frame.odom_zero_map.y,
      std::atan2(siny_cosp, cosy_cosp),
  };
}
}  // namespace

class WallFitCalibrator : public rclcpp::Node
{
public:
  WallFitCalibrator()
      : Node("wall_fit_calibrator")
  {
    declare_parameter<std::string>("scan_topic", "/scan");
    declare_parameter<std::string>("odom_topic", "/odom_combined");
    declare_parameter<std::string>("config_path", "/home/sunrise/yiliao_ws/src/origincar_base/config/wall_fit.json");
    declare_parameter<double>("laser_x", 0.0);
    declare_parameter<double>("laser_y", 0.0);
    declare_parameter<double>("laser_yaw", 0.0);

    config_path_ = get_parameter("config_path").as_string();
    config_ = origincar_wall::loadWallFitConfig(config_path_);
    laser_pose_ = {
        get_parameter("laser_x").as_double(),
        get_parameter("laser_y").as_double(),
        get_parameter("laser_yaw").as_double(),
    };

    scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
        get_parameter("scan_topic").as_string(), 10,
        [this](sensor_msgs::msg::LaserScan::SharedPtr msg) {
          std::lock_guard<std::mutex> lock(mutex_);
          latest_scan_ = origincar_wall::transformScanPointsToPoseFrame(scanMsgToPoints(*msg), laser_pose_);
          has_scan_ = true;
        });
    odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
        get_parameter("odom_topic").as_string(), 10,
        [this](nav_msgs::msg::Odometry::SharedPtr msg) {
          std::lock_guard<std::mutex> lock(mutex_);
          latest_odom_ = odomMsgToMapPose(*msg, config_);
          has_odom_ = true;
        });

    printHelp();
    keyboard_thread_ = std::thread([this]() { keyboardLoop(); });
  }

  ~WallFitCalibrator() override
  {
    running_ = false;
    if (keyboard_thread_.joinable())
    {
      keyboard_thread_.join();
    }
  }

private:
  void printHelp()
  {
    RCLCPP_INFO(get_logger(), "Wall calibration order: P -> task_release -> passage -> clinic");
    RCLCPP_INFO(get_logger(), "Keys: Space=capture, A=previous, D=next, Q=clear, Enter=save");
    printSelected();
  }

  void printSelected()
  {
    RCLCPP_INFO(get_logger(), "Selected [%zu/%zu]: %s",
                selected_index_ + 1, reference_order_.size(), reference_order_[selected_index_].c_str());
  }

  void captureCurrent()
  {
    std::lock_guard<std::mutex> lock(mutex_);
    if (!has_scan_ || !has_odom_)
    {
      RCLCPP_WARN(get_logger(), "Waiting for both /scan and /odom_combined before capture");
      return;
    }
    const auto &key = reference_order_[selected_index_];
    const auto reference = config_.reference_points.at(key);
    origincar_wall::WallFitCapture capture;
    capture.key = key;
    capture.map = reference.map;
    capture.odom_pose = {reference.map.x, reference.map.y, latest_odom_.theta};
    capture.scan_points = latest_scan_;
    captures_[key] = capture;
    RCLCPP_INFO(get_logger(), "Captured %s with %zu scan points", key.c_str(), latest_scan_.size());
  }

  void clearCaptures()
  {
    std::lock_guard<std::mutex> lock(mutex_);
    captures_.clear();
    RCLCPP_WARN(get_logger(), "Cleared all wall calibration captures");
  }

  void saveCaptures()
  {
    std::lock_guard<std::mutex> lock(mutex_);
    std::vector<origincar_wall::WallFitCapture> ordered_captures;
    for (const auto &key : reference_order_)
    {
      const auto found = captures_.find(key);
      if (found == captures_.end())
      {
        RCLCPP_WARN(get_logger(), "Missing capture for %s; not saving", key.c_str());
        return;
      }
      ordered_captures.push_back(found->second);
    }
    try
    {
      const auto fitted = origincar_wall::fitWallConfigFromCaptures(ordered_captures, config_);
      origincar_wall::saveWallFitConfig(config_path_, fitted);
      config_ = fitted;
      const auto bounds = origincar_wall::wallBounds(config_);
      RCLCPP_INFO(get_logger(), "Saved %s; bounds x=[%.3f, %.3f] y=[%.3f, %.3f]",
                  config_path_.c_str(), bounds.x_min, bounds.x_max, bounds.y_min, bounds.y_max);
    }
    catch (const std::exception &error)
    {
      RCLCPP_ERROR(get_logger(), "Failed to save wall fit: %s", error.what());
    }
  }

  void keyboardLoop()
  {
    termios old_terminal{};
    if (tcgetattr(STDIN_FILENO, &old_terminal) != 0)
    {
      RCLCPP_WARN(get_logger(), "Keyboard control unavailable: stdin is not a terminal");
      return;
    }
    termios raw_terminal = old_terminal;
    raw_terminal.c_lflag &= static_cast<unsigned int>(~(ICANON | ECHO));
    raw_terminal.c_cc[VMIN] = 0;
    raw_terminal.c_cc[VTIME] = 0;
    tcsetattr(STDIN_FILENO, TCSANOW, &raw_terminal);

    while (running_ && rclcpp::ok())
    {
      pollfd descriptor{STDIN_FILENO, POLLIN, 0};
      const auto ready = poll(&descriptor, 1, 100);
      if (ready <= 0 || !(descriptor.revents & POLLIN))
      {
        continue;
      }
      char key = 0;
      if (read(STDIN_FILENO, &key, 1) != 1)
      {
        continue;
      }
      if (key == ' ')
      {
        captureCurrent();
      }
      else if (key == 'q' || key == 'Q')
      {
        clearCaptures();
      }
      else if (key == 'a' || key == 'A')
      {
        selected_index_ = selected_index_ == 0 ? reference_order_.size() - 1 : selected_index_ - 1;
        printSelected();
      }
      else if (key == 'd' || key == 'D')
      {
        selected_index_ = (selected_index_ + 1) % reference_order_.size();
        printSelected();
      }
      else if (key == '\n' || key == '\r')
      {
        saveCaptures();
      }
    }
    tcsetattr(STDIN_FILENO, TCSANOW, &old_terminal);
  }

  std::string config_path_;
  origincar_wall::WallFitConfig config_;
  origincar_wall::Pose2D laser_pose_{0.0, 0.0, 0.0};
  std::vector<std::string> reference_order_{"P", "task_release", "passage", "clinic"};
  std::size_t selected_index_{0};
  std::mutex mutex_;
  bool has_scan_{false};
  bool has_odom_{false};
  std::vector<origincar_wall::WallPoint> latest_scan_;
  origincar_wall::Pose2D latest_odom_{0.0, 0.0, 0.0};
  std::map<std::string, origincar_wall::WallFitCapture> captures_;
  std::atomic<bool> running_{true};
  std::thread keyboard_thread_;
  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
};

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<WallFitCalibrator>());
  rclcpp::shutdown();
  return 0;
}
