#include "workshop_orchestrator_v2/wifi6_link_client.hpp"

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

#include <cstring>
#include <sstream>
#include <utility>

#include "nlohmann/json.hpp"
#include "workshop_orchestrator_v2/types.hpp"

namespace workshop_orchestrator_v2
{

namespace
{

void write_le32(uint8_t * buf, uint32_t value)
{
  buf[0] = static_cast<uint8_t>(value & 0xFF);
  buf[1] = static_cast<uint8_t>((value >> 8) & 0xFF);
  buf[2] = static_cast<uint8_t>((value >> 16) & 0xFF);
  buf[3] = static_cast<uint8_t>((value >> 24) & 0xFF);
}

uint32_t read_le32(const uint8_t * buf)
{
  return static_cast<uint32_t>(buf[0]) |
         (static_cast<uint32_t>(buf[1]) << 8) |
         (static_cast<uint32_t>(buf[2]) << 16) |
         (static_cast<uint32_t>(buf[3]) << 24);
}

bool write_all(int fd, const uint8_t * buf, size_t len)
{
  size_t written = 0;
  while (written < len) {
    const ssize_t n = ::write(fd, buf + written, len - written);
    if (n <= 0) {
      return false;
    }
    written += static_cast<size_t>(n);
  }
  return true;
}

bool read_all(int fd, uint8_t * buf, size_t len)
{
  size_t got = 0;
  while (got < len) {
    const ssize_t n = ::read(fd, buf + got, len - got);
    if (n <= 0) {
      return false;
    }
    got += static_cast<size_t>(n);
  }
  return true;
}

int connect_tcp(const std::string & host, uint16_t port, int timeout_ms)
{
  const int fd = ::socket(AF_INET, SOCK_STREAM, 0);
  if (fd < 0) {
    return -1;
  }

  struct timeval tv;
  tv.tv_sec = timeout_ms / 1000;
  tv.tv_usec = (timeout_ms % 1000) * 1000;
  ::setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
  ::setsockopt(fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv));

  struct sockaddr_in addr;
  std::memset(&addr, 0, sizeof(addr));
  addr.sin_family = AF_INET;
  addr.sin_port = htons(port);
  if (::inet_pton(AF_INET, host.c_str(), &addr.sin_addr) <= 0) {
    ::close(fd);
    return -1;
  }
  if (::connect(fd, reinterpret_cast<struct sockaddr *>(&addr), sizeof(addr)) < 0) {
    ::close(fd);
    return -1;
  }
  return fd;
}

std::string endpoint_string(const std::string & host, uint16_t port)
{
  std::ostringstream oss;
  oss << host << ":" << port;
  return oss.str();
}

std::string readiness_payload(const std::string & agent_name)
{
  nlohmann::json request;
  request["agent_name"] = agent_name;
  request["include_details"] = true;
  request["target_resource_ids"] = nlohmann::json::array();
  return request.dump();
}

std::string external_pose_probe_payload()
{
  nlohmann::json request;
  request["hardware_timestamp_us"] = now_us();
  request["pose_valid"] = false;
  request["workshop_pose"] = {
    {"x_m", 0.0},
    {"y_m", 0.0},
    {"z_m", 0.0},
    {"roll_rad", 0.0},
    {"pitch_rad", 0.0},
    {"yaw_rad", 0.0},
  };
  request["position_stddev_m"] = 0.0;
  request["yaw_stddev_rad"] = 0.0;
  request["tracking_loss_ratio"] = 0.0;
  request["time_sync_offset_ms"] = 0.0;
  request["quality_score"] = 1.0;
  request["observed_target_count"] = 0;
  request["reference_source_name"] = "workshop_orchestrator_precheck";
  request["active_job_id"] = "wifi6_link_precheck";
  return request.dump();
}

bool json_bool(const nlohmann::json & payload, const std::string & key, bool default_value)
{
  const auto it = payload.find(key);
  if (it == payload.end() || !it->is_boolean()) {
    return default_value;
  }
  return it->get<bool>();
}

std::string json_message(const nlohmann::json & payload, const std::string & fallback)
{
  const auto it = payload.find("message");
  if (it == payload.end() || !it->is_string() || it->get<std::string>().empty()) {
    return fallback;
  }
  return it->get<std::string>();
}

bool optional_bool_passed(
  const nlohmann::json & payload,
  const std::string & key,
  std::string & failure_reason)
{
  const auto it = payload.find(key);
  if (it == payload.end() || !it->is_boolean() || it->get<bool>()) {
    return true;
  }
  failure_reason = key + "=false";
  return false;
}

}  // namespace

Wifi6LinkClient::Wifi6LinkClient(Config config)
: config_(std::move(config))
{
}

Wifi6LinkClient::CheckResult Wifi6LinkClient::send_json_request(
  FrameType request_type,
  FrameType response_type,
  const std::string & request_payload,
  const std::string & link_name) const
{
  const int fd = connect_tcp(config_.vehicle_host, config_.vehicle_port, config_.timeout_ms);
  const auto endpoint = endpoint_string(config_.vehicle_host, config_.vehicle_port);
  if (fd < 0) {
    return {false, link_name + " 无法连接 " + endpoint};
  }

  uint8_t header[8];
  write_le32(header, static_cast<uint32_t>(request_type));
  write_le32(header + 4, static_cast<uint32_t>(request_payload.size()));
  bool ok = write_all(fd, header, sizeof(header));
  if (ok && !request_payload.empty()) {
    ok = write_all(fd, reinterpret_cast<const uint8_t *>(request_payload.data()), request_payload.size());
  }
  if (!ok) {
    ::close(fd);
    return {false, link_name + " 向 " + endpoint + " 发送请求失败"};
  }

  uint8_t response_header[8];
  if (!read_all(fd, response_header, sizeof(response_header))) {
    ::close(fd);
    return {false, link_name + " 等待 " + endpoint + " 响应超时或连接断开"};
  }
  const auto actual_type = static_cast<FrameType>(read_le32(response_header));
  const auto payload_len = read_le32(response_header + 4);
  std::string response_payload(payload_len, '\0');
  if (payload_len > 0 &&
    !read_all(fd, reinterpret_cast<uint8_t *>(response_payload.data()), payload_len))
  {
    ::close(fd);
    return {false, link_name + " 读取 " + endpoint + " 响应 payload 失败"};
  }
  ::close(fd);

  if (actual_type != response_type) {
    return {false, link_name + " 响应帧类型不匹配"};
  }

  try {
    const auto payload = nlohmann::json::parse(response_payload.empty() ? "{}" : response_payload);
    const bool success = json_bool(payload, "success", false);
    if (!success) {
      return {false, json_message(payload, link_name + " readiness=false")};
    }
    std::string readiness_failure;
    const char * readiness_keys[] = {
      "agent_ready",
      "estop_released",
      "vehicle_safe_to_move",
      "motion_control_ready",
      "trajectory_executor_ready",
      "vehicle_feedback_ready",
      "control_output_ready",
      "external_pose_feedback_ready",
      "capture_pipeline_ready",
      "sensor_data_stream_ready",
    };
    for (const auto * key : readiness_keys) {
      if (!optional_bool_passed(payload, key, readiness_failure)) {
        return {false, json_message(payload, link_name + " " + readiness_failure)};
      }
    }
    return {true, json_message(payload, link_name + " 链路正常")};
  } catch (const std::exception & exc) {
    return {false, link_name + " 响应 JSON 解析失败: " + std::string(exc.what())};
  }
}

Wifi6LinkClient::CheckResult Wifi6LinkClient::check_chassis() const
{
  auto result = send_json_request(
    FrameType::CHASSIS_GET_READINESS_REQ,
    FrameType::CHASSIS_GET_READINESS_RSP,
    readiness_payload("chassis"),
    "底盘 WiFi6 链路");
  if (!result.passed) {
    return result;
  }
  return result;
}

Wifi6LinkClient::CheckResult Wifi6LinkClient::check_control() const
{
  return send_json_request(
    FrameType::CONTROL_GET_READINESS_REQ,
    FrameType::CONTROL_GET_READINESS_RSP,
    readiness_payload("control"),
    "运控 WiFi6 链路");
}

Wifi6LinkClient::CheckResult Wifi6LinkClient::check_sensor() const
{
  return send_json_request(
    FrameType::SENSOR_GET_READINESS_REQ,
    FrameType::SENSOR_GET_READINESS_RSP,
    readiness_payload("sensor"),
    "传感器 WiFi6 链路");
}

Wifi6LinkClient::CheckResult Wifi6LinkClient::check_external_pose() const
{
  return send_json_request(
    FrameType::EXTERNAL_POSE_PUSH_REQ,
    FrameType::EXTERNAL_POSE_PUSH_RSP,
    external_pose_probe_payload(),
    "外部真值位姿 WiFi6 链路");
}

}  // namespace workshop_orchestrator_v2
