#include "origincar_base/wall_fit_core.hpp"

#include <nlohmann/json.hpp>

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

#include <arpa/inet.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <unistd.h>

#include <algorithm>
#include <atomic>
#include <array>
#include <chrono>
#include <cmath>
#include <fstream>
#include <mutex>
#include <sstream>
#include <string>
#include <thread>
#include <vector>

namespace
{
std::vector<origincar_wall::WallPoint> scanMsgToPoints(const sensor_msgs::msg::LaserScan &msg, int stride)
{
  std::vector<origincar_wall::WallPoint> points;
  stride = std::max(1, stride);
  points.reserve(msg.ranges.size() / static_cast<std::size_t>(stride) + 1);
  for (std::size_t index = 0; index < msg.ranges.size(); index += static_cast<std::size_t>(stride))
  {
    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;
}

double quaternionToYaw(const geometry_msgs::msg::Quaternion &q)
{
  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 std::atan2(siny_cosp, cosy_cosp);
}

nlohmann::json poseJson(double x, double y, double theta)
{
  return nlohmann::json{{"x", x}, {"y", y}, {"theta", theta}};
}

std::string readTextFile(const std::string &path)
{
  std::ifstream file(path);
  if (!file.good())
  {
    return "";
  }
  std::ostringstream stream;
  stream << file.rdbuf();
  return stream.str();
}

std::vector<unsigned char> readBinaryFile(const std::string &path)
{
  std::ifstream file(path, std::ios::binary);
  if (!file.good())
  {
    return {};
  }
  return std::vector<unsigned char>((std::istreambuf_iterator<char>(file)), std::istreambuf_iterator<char>());
}

std::string requestPath(const std::string &request)
{
  const auto first_space = request.find(' ');
  if (first_space == std::string::npos)
  {
    return "/";
  }
  const auto second_space = request.find(' ', first_space + 1);
  auto path = request.substr(first_space + 1, second_space - first_space - 1);
  const auto query = path.find('?');
  if (query != std::string::npos)
  {
    path = path.substr(0, query);
  }
  return path;
}
}  // namespace

class WallPreviewServer : public rclcpp::Node
{
public:
  WallPreviewServer()
      : Node("wall_preview_server")
  {
    declare_parameter<std::string>("host", "0.0.0.0");
    declare_parameter<int>("port", 8772);
    declare_parameter<std::string>("config_path", "/home/sunrise/yiliao_ws/src/origincar_base/config/wall_fit.json");
    declare_parameter<std::string>("preview_dir", "/home/sunrise/yiliao_ws/src/origincar_base/preview");
    declare_parameter<std::string>("scan_topic", "/scan");
    declare_parameter<std::string>("combined_odom_topic", "/odom_combined");
    declare_parameter<int>("scan_stride", 3);
    declare_parameter<double>("preview_hz", 8.0);

    host_ = get_parameter("host").as_string();
    port_ = get_parameter("port").as_int();
    config_path_ = get_parameter("config_path").as_string();
    preview_dir_ = get_parameter("preview_dir").as_string();
    scan_stride_ = get_parameter("scan_stride").as_int();
    preview_period_ms_ = static_cast<int>(1000.0 / std::max(1.0, get_parameter("preview_hz").as_double()));
    wall_fit_ = origincar_wall::loadWallFitConfig(config_path_);

    combined_odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
        get_parameter("combined_odom_topic").as_string(), 10,
        [this](nav_msgs::msg::Odometry::SharedPtr msg) {
          std::lock_guard<std::mutex> lock(mutex_);
          combined_pose_ = poseJson(msg->pose.pose.position.x, msg->pose.pose.position.y,
                                    quaternionToYaw(msg->pose.pose.orientation));
          has_combined_pose_ = true;
        });
    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_);
          if (!has_combined_pose_)
          {
            return;
          }
          const origincar_wall::Pose2D pose{
              combined_pose_.at("x").get<double>(),
              combined_pose_.at("y").get<double>(),
              combined_pose_.at("theta").get<double>(),
          };
          latest_scan_map_ = origincar_wall::transformScanPointsToMap(scanMsgToPoints(*msg, scan_stride_), pose);
        });

    server_thread_ = std::thread([this]() { serve(); });
    RCLCPP_INFO(get_logger(), "Wall preview: http://%s:%d", host_.c_str(), port_);
  }

  ~WallPreviewServer() override
  {
    running_ = false;
    if (server_fd_ >= 0)
    {
      close(server_fd_);
    }
    if (server_thread_.joinable())
    {
      server_thread_.join();
    }
  }

private:
  nlohmann::json snapshot()
  {
    std::lock_guard<std::mutex> lock(mutex_);
    nlohmann::json scan_points = nlohmann::json::array();
    for (const auto &point : latest_scan_map_)
    {
      scan_points.push_back({{"x", point.x}, {"y", point.y}});
    }
    return {
        {"status", "ok"},
        {"wall_fit", nlohmann::json::parse(origincar_wall::wallFitConfigToJson(wall_fit_))},
        {"odom_combined", has_combined_pose_ ? combined_pose_ : nlohmann::json(nullptr)},
        {"scan_map", {{"points", scan_points}}},
    };
  }

  void sendAll(int fd, const std::string &data)
  {
    const char *buffer = data.data();
    std::size_t remaining = data.size();
    while (remaining > 0)
    {
      const auto sent = send(fd, buffer, remaining, MSG_NOSIGNAL);
      if (sent <= 0)
      {
        return;
      }
      buffer += sent;
      remaining -= static_cast<std::size_t>(sent);
    }
  }

  void sendBytes(int fd, const std::string &content_type, const std::vector<unsigned char> &bytes)
  {
    std::ostringstream header;
    header << "HTTP/1.1 200 OK\r\nContent-Type: " << content_type
           << "\r\nContent-Length: " << bytes.size()
           << "\r\nConnection: close\r\n\r\n";
    sendAll(fd, header.str());
    if (!bytes.empty())
    {
      send(fd, bytes.data(), bytes.size(), MSG_NOSIGNAL);
    }
  }

  void sendText(int fd, const std::string &content_type, const std::string &body)
  {
    std::ostringstream response;
    response << "HTTP/1.1 200 OK\r\nContent-Type: " << content_type
             << "\r\nContent-Length: " << body.size()
             << "\r\nConnection: close\r\n\r\n"
             << body;
    sendAll(fd, response.str());
  }

  void sendNotFound(int fd)
  {
    sendText(fd, "text/plain; charset=utf-8", "not found\n");
  }

  void sendEvents(int fd)
  {
    sendAll(fd, "HTTP/1.1 200 OK\r\nContent-Type: text/event-stream\r\nCache-Control: no-cache\r\nConnection: keep-alive\r\n\r\n");
    while (running_ && rclcpp::ok())
    {
      const auto payload = std::string("event: snapshot\ndata: ") + snapshot().dump() + "\n\n";
      if (send(fd, payload.data(), payload.size(), MSG_NOSIGNAL) <= 0)
      {
        break;
      }
      std::this_thread::sleep_for(std::chrono::milliseconds(preview_period_ms_));
    }
  }

  void handleClient(int fd)
  {
    std::array<char, 2048> buffer{};
    const auto received = recv(fd, buffer.data(), buffer.size() - 1, 0);
    if (received <= 0)
    {
      close(fd);
      return;
    }
    const auto path = requestPath(std::string(buffer.data(), static_cast<std::size_t>(received)));
    if (path == "/" || path == "/wall_localization.html")
    {
      const auto html = readTextFile(preview_dir_ + "/wall_localization.html");
      html.empty() ? sendNotFound(fd) : sendText(fd, "text/html; charset=utf-8", html);
    }
    else if (path == "/wall_fit.json")
    {
      wall_fit_ = origincar_wall::loadWallFitConfig(config_path_);
      sendText(fd, "application/json; charset=utf-8", origincar_wall::wallFitConfigToJson(wall_fit_));
    }
    else if (path == "/map_reference.png")
    {
      const auto bytes = readBinaryFile(preview_dir_ + "/map_reference.png");
      bytes.empty() ? sendNotFound(fd) : sendBytes(fd, "image/png", bytes);
    }
    else if (path == "/events")
    {
      sendEvents(fd);
    }
    else
    {
      sendNotFound(fd);
    }
    close(fd);
  }

  void serve()
  {
    server_fd_ = socket(AF_INET, SOCK_STREAM, 0);
    if (server_fd_ < 0)
    {
      RCLCPP_ERROR(get_logger(), "Failed to create preview socket");
      return;
    }
    int opt = 1;
    setsockopt(server_fd_, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
    sockaddr_in address{};
    address.sin_family = AF_INET;
    address.sin_port = htons(static_cast<uint16_t>(port_));
    address.sin_addr.s_addr = host_ == "0.0.0.0" ? INADDR_ANY : inet_addr(host_.c_str());
    if (bind(server_fd_, reinterpret_cast<sockaddr *>(&address), sizeof(address)) < 0)
    {
      RCLCPP_ERROR(get_logger(), "Failed to bind preview port %d", port_);
      return;
    }
    if (listen(server_fd_, 8) < 0)
    {
      RCLCPP_ERROR(get_logger(), "Failed to listen on preview port %d", port_);
      return;
    }
    while (running_ && rclcpp::ok())
    {
      const auto client_fd = accept(server_fd_, nullptr, nullptr);
      if (client_fd < 0)
      {
        continue;
      }
      std::thread([this, client_fd]() { handleClient(client_fd); }).detach();
    }
  }

  std::string host_;
  int port_{8772};
  std::string config_path_;
  std::string preview_dir_;
  int scan_stride_{3};
  int preview_period_ms_{125};
  int server_fd_{-1};
  std::atomic<bool> running_{true};
  std::thread server_thread_;
  std::mutex mutex_;
  origincar_wall::WallFitConfig wall_fit_;
  nlohmann::json combined_pose_;
  bool has_combined_pose_{false};
  std::vector<origincar_wall::WallPoint> latest_scan_map_;
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr combined_odom_sub_;
  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
};

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