#pragma once

// gateway_codec.hpp
// 作用：vehicle_agent_gateway 的 JSON 序列化/反序列化工具函数。
//
// 设计原则：
//   1. 只依赖 nlohmann/json 和 ROS2 消息头，不引入 protobuf 运行时。
//   2. JSON key 命名完全对齐 proto 字段名，方便未来直接切换 protobuf 二进制。
//   3. 每个函数都是纯函数，不持有状态。
//
// 帧 payload 格式：UTF-8 JSON 字符串，不含 BOM。

#include <string>
#include <nlohmann/json.hpp>

#include "calibration_chassis_interfaces/msg/motion_primitive_request.hpp"
#include "calibration_chassis_interfaces/msg/chassis_readiness_response.hpp"
#include "calibration_chassis_interfaces/msg/chassis_job_result.hpp"
#include "calibration_control_interfaces/msg/controller_evaluation_request.hpp"
#include "calibration_control_interfaces/msg/control_readiness_response.hpp"
#include "calibration_control_interfaces/msg/control_job_result.hpp"
#include "calibration_common_interfaces/msg/agent_readiness_request.hpp"

namespace vehicle_agent_gateway
{
namespace codec
{

// =========================================================
// 公共辅助：ErrorCode
// =========================================================
inline nlohmann::json encode_error_code(
  const calibration_common_interfaces::msg::ErrorCode & ec)
{
  return nlohmann::json{{"code", ec.code}};
}

inline void decode_error_code(
  const nlohmann::json & j,
  calibration_common_interfaces::msg::ErrorCode & ec)
{
  ec.code = j.value("code", static_cast<uint32_t>(0));
}

// =========================================================
// 公共辅助：RequestHeader
// =========================================================
inline nlohmann::json encode_request_header(
  const calibration_common_interfaces::msg::RequestHeader & h)
{
  return nlohmann::json{
    {"session_id",              h.session_id},
    {"task_id",                 h.task_id},
    {"vehicle_id",              h.vehicle_id},
    {"request_id",              h.request_id},
    {"client_send_timestamp_us", h.client_send_timestamp_us},
    {"operator_id",             h.operator_id},
    {"workshop_host",           h.workshop_host}
  };
}

// =========================================================
// 公共辅助：FileReference
// =========================================================
inline calibration_common_interfaces::msg::FileReference decode_file_reference(
  const nlohmann::json & j)
{
  calibration_common_interfaces::msg::FileReference ref;
  ref.file_name   = j.value("file_name",   std::string{});
  ref.file_uri    = j.value("file_uri",    std::string{});
  ref.size_bytes  = j.value("size_bytes",  int64_t{0});
  ref.description = j.value("description", std::string{});
  return ref;
}

// =========================================================
// 底盘：MotionPrimitiveRequest → JSON
// =========================================================
inline std::string encode_motion_primitive_request(
  const calibration_chassis_interfaces::msg::MotionPrimitiveRequest & req)
{
  using namespace calibration_chassis_interfaces::msg;

  nlohmann::json j;
  j["header"]              = encode_request_header(req.header);
  j["test_case_id"]        = req.test_case_id;
  j["task_purpose"]        = req.task_purpose.value;
  j["selected_primitive"]  = req.selected_primitive.value;
  j["brake_when_finished"] = req.brake_when_finished;
  j["timeout_sec"]         = req.timeout_sec;
  j["source_iteration_id"] = req.source_iteration_id;

  // 根据实际选择的原语类型只序列化对应字段，其余省略。
  switch (req.selected_primitive.value) {
    case ChassisMotionPrimitiveType::STRAIGHT_LINE:
      j["straight_line"] = {
        {"target_speed_ms",    req.straight_line.target_speed_ms},
        {"target_distance_m", req.straight_line.target_distance_m},
        {"reverse",            req.straight_line.reverse}
      };
      break;
    case ChassisMotionPrimitiveType::ARC:
      j["arc"] = {
        {"target_speed_ms",  req.arc.target_speed_ms},
        {"radius_m",          req.arc.radius_m},
        {"sweep_angle_deg",   req.arc.sweep_angle_deg},
        {"clockwise",         req.arc.clockwise}
      };
      break;
    case ChassisMotionPrimitiveType::IN_PLACE_ROTATION:
      j["in_place_rotation"] = {
        {"target_yaw_deg",          req.in_place_rotation.target_yaw_deg},
        {"target_angular_vel_deg_s", req.in_place_rotation.target_angular_vel_deg_s}
      };
      break;
    case ChassisMotionPrimitiveType::STEERING_SWEEP:
      j["steering_sweep"] = {
        {"target_angle_deg",    req.steering_sweep.target_angle_deg},
        {"sweep_amplitude_deg", req.steering_sweep.sweep_amplitude_deg},
        {"sweep_frequency_hz",  req.steering_sweep.sweep_frequency_hz},
        {"duration_sec",        req.steering_sweep.duration_sec}
      };
      break;
    case ChassisMotionPrimitiveType::LATERAL_TRANSLATION:
      j["lateral_translation"] = {
        {"target_speed_ms",    req.lateral_translation.target_speed_ms},
        {"target_distance_m", req.lateral_translation.target_distance_m},
        {"move_left",          req.lateral_translation.move_left}
      };
      break;
    case ChassisMotionPrimitiveType::DIAGONAL_MOTION:
      j["diagonal_motion"] = {
        {"target_speed_ms",    req.diagonal_motion.target_speed_ms},
        {"target_distance_m", req.diagonal_motion.target_distance_m},
        {"heading_deg",        req.diagonal_motion.heading_deg}
      };
      break;
    case ChassisMotionPrimitiveType::MODULE_ALIGNMENT:
      j["module_alignment"] = {
        {"module_ids",      req.module_alignment.module_ids},
        {"target_zero_deg", req.module_alignment.target_zero_deg},
        {"tolerance_deg",   req.module_alignment.tolerance_deg}
      };
      break;
    case ChassisMotionPrimitiveType::COORDINATED_STEERING:
      j["coordinated_steering"] = {
        {"module_ids",       req.coordinated_steering.module_ids},
        {"target_angle_deg", req.coordinated_steering.target_angle_deg},
        {"hold_time_sec",    req.coordinated_steering.hold_time_sec}
      };
      break;
    default:
      break;
  }

  return j.dump();
}

// =========================================================
// 底盘：AgentReadinessRequest → JSON
// =========================================================
inline std::string encode_chassis_readiness_request(
  const calibration_common_interfaces::msg::AgentReadinessRequest & req)
{
  nlohmann::json j;
  j["agent_name"]         = req.agent_name;
  j["include_details"]    = req.include_details;
  nlohmann::json ids = nlohmann::json::array();
  for (const auto & id : req.target_resource_ids) { ids.push_back(id); }
  j["target_resource_ids"] = ids;
  return j.dump();
}

// =========================================================
// 运控：AgentReadinessRequest → JSON
// =========================================================
inline std::string encode_control_readiness_request(
  const calibration_common_interfaces::msg::AgentReadinessRequest & req)
{
  nlohmann::json j;
  j["agent_name"]         = req.agent_name;
  j["include_details"]    = req.include_details;
  nlohmann::json ids = nlohmann::json::array();
  for (const auto & id : req.target_resource_ids) { ids.push_back(id); }
  j["target_resource_ids"] = ids;
  return j.dump();
}

// =========================================================
// 底盘：JSON → ChassisReadinessResponse
// =========================================================
inline void decode_chassis_readiness_response(
  const std::string & payload,
  calibration_chassis_interfaces::msg::ChassisReadinessResponse & rsp)
{
  if (payload.empty()) return;
  try {
    auto j = nlohmann::json::parse(payload);
    rsp.success               = j.value("success",               false);
    rsp.message               = j.value("message",               std::string{});
    rsp.agent_ready           = j.value("agent_ready",           false);
    rsp.chassis_driver_online = j.value("chassis_driver_online", false);
    rsp.motion_control_ready  = j.value("motion_control_ready",  false);
    rsp.estop_released        = j.value("estop_released",        false);
    rsp.vehicle_safe_to_move  = j.value("vehicle_safe_to_move",  false);
    rsp.telemetry_available   = j.value("telemetry_available",   false);
    rsp.checked_timestamp_us  = j.value("checked_timestamp_us",  int64_t{0});
    if (j.contains("error_code")) {
      decode_error_code(j["error_code"], rsp.error_code);
    }
  } catch (...) {
    // payload 解析失败保持调用方已设置的默认值不变
  }
}

// =========================================================
// 底盘：JSON → ChassisJobResult
// =========================================================
inline void decode_chassis_job_result(
  const std::string & payload,
  calibration_chassis_interfaces::msg::ChassisJobResult & result)
{
  if (payload.empty()) return;
  try {
    auto j = nlohmann::json::parse(payload);
    result.success                       = j.value("success",                       false);
    result.message                       = j.value("message",                       std::string{});
    result.job_id                        = j.value("job_id",                        std::string{});
    result.data_quality_passed           = j.value("data_quality_passed",           false);
    result.suitable_for_commit           = j.value("suitable_for_commit",           false);
    result.recommended_parameter_version = j.value("recommended_parameter_version", std::string{});
    result.estimated_straight_line_bias  = j.value("estimated_straight_line_bias",  0.0);
    if (j.contains("error_code")) {
      decode_error_code(j["error_code"], result.error_code);
    }
    if (j.contains("validation_summary")) {
      auto & vs = j["validation_summary"];
      result.validation_summary.max_lateral_error_m     = vs.value("max_lateral_error_m",     0.0);
      result.validation_summary.max_yaw_error_rad       = vs.value("max_yaw_error_rad",       0.0);
      result.validation_summary.rms_lateral_error_m     = vs.value("rms_lateral_error_m",     0.0);
      result.validation_summary.rms_yaw_error_rad       = vs.value("rms_yaw_error_rad",       0.0);
      result.validation_summary.repeatability_error_m   = vs.value("repeatability_error_m",   0.0);
      result.validation_summary.curvature_error         = vs.value("curvature_error",         0.0);
      result.validation_summary.module_consistency_error = vs.value("module_consistency_error", 0.0);
      result.validation_summary.auto_acceptance_passed  = vs.value("auto_acceptance_passed",  false);
    }
    if (j.contains("artifacts") && j["artifacts"].is_array()) {
      for (const auto & a : j["artifacts"]) {
        result.artifacts.push_back(decode_file_reference(a));
      }
    }
  } catch (...) {}
}

// =========================================================
// 运控：ControllerEvaluationRequest → JSON
// =========================================================
inline std::string encode_controller_evaluation_request(
  const calibration_control_interfaces::msg::ControllerEvaluationRequest & req)
{
  using namespace calibration_control_interfaces::msg;

  nlohmann::json j;
  j["header"]              = encode_request_header(req.header);
  j["test_case_id"]        = req.test_case_id;
  j["task_purpose"]        = req.task_purpose.value;
  j["selected_task"]       = req.selected_task.value;
  j["source_iteration_id"] = req.source_iteration_id;

  switch (req.selected_task.value) {
    case ControllerEvaluationTaskType::TRAJECTORY_TRACKING: {
      nlohmann::json path_arr = nlohmann::json::array();
      for (const auto & pt : req.trajectory_tracking.path) {
        path_arr.push_back({
          {"x_m",            pt.x_m},
          {"y_m",            pt.y_m},
          {"yaw_rad",        pt.yaw_rad},
          {"target_speed_ms", pt.target_speed_ms}
        });
      }
      j["trajectory_tracking"] = {
        {"path",        path_arr},
        {"stop_at_end", req.trajectory_tracking.stop_at_end},
        {"timeout_sec", req.trajectory_tracking.timeout_sec},
        {"required_external_pose_source_id", req.trajectory_tracking.required_external_pose_source_id},
        {"max_external_pose_age_ms", req.trajectory_tracking.max_external_pose_age_ms},
        {"min_external_pose_quality_score", req.trajectory_tracking.min_external_pose_quality_score},
        {"trajectory_id", req.trajectory_tracking.trajectory_id},
        {"segment_index", req.trajectory_tracking.segment_index},
        {"total_segments", req.trajectory_tracking.total_segments},
        {"is_final_segment", req.trajectory_tracking.is_final_segment}
      };
      break;
    }
    case ControllerEvaluationTaskType::VELOCITY_STEP:
      j["velocity_step"] = {
        {"target_velocity_ms",    req.velocity_step.target_velocity_ms},
        {"hold_time_sec",         req.velocity_step.hold_time_sec},
        {"settle_before_step_sec", req.velocity_step.settle_before_step_sec}
      };
      break;
    case ControllerEvaluationTaskType::ACCELERATION_DECELERATION:
      j["accel_decel"] = {
        {"start_velocity_ms",  req.accel_decel.start_velocity_ms},
        {"target_velocity_ms", req.accel_decel.target_velocity_ms},
        {"target_accel_ms2",   req.accel_decel.target_accel_ms2},
        {"hold_time_sec",      req.accel_decel.hold_time_sec}
      };
      break;
    case ControllerEvaluationTaskType::STOP_ACCURACY:
      j["stop_accuracy"] = {
        {"target_stop_x_m",   req.stop_accuracy.target_stop_x_m},
        {"target_stop_y_m",   req.stop_accuracy.target_stop_y_m},
        {"target_stop_yaw_rad", req.stop_accuracy.target_stop_yaw_rad},
        {"timeout_sec",        req.stop_accuracy.timeout_sec}
      };
      break;
    default:
      break;
  }

  return j.dump();
}

// =========================================================
// 运控：JSON → ControlReadinessResponse
// =========================================================
inline void decode_control_readiness_response(
  const std::string & payload,
  calibration_control_interfaces::msg::ControlReadinessResponse & rsp)
{
  if (payload.empty()) return;
  try {
    auto j = nlohmann::json::parse(payload);
    rsp.success                   = j.value("success",                   false);
    rsp.message                   = j.value("message",                   std::string{});
    rsp.agent_ready               = j.value("agent_ready",               false);
    rsp.trajectory_executor_ready = j.value("trajectory_executor_ready", false);
    rsp.vehicle_feedback_ready    = j.value("vehicle_feedback_ready",    false);
    rsp.control_output_ready      = j.value("control_output_ready",      false);
    rsp.estop_released            = j.value("estop_released",            false);
    rsp.vehicle_safe_to_move      = j.value("vehicle_safe_to_move",      false);
    rsp.checked_timestamp_us      = j.value("checked_timestamp_us",      int64_t{0});
    rsp.external_pose_feedback_ready = j.value("external_pose_feedback_ready", false);
    rsp.external_pose_source_name = j.value("external_pose_source_name", std::string{});
    rsp.external_pose_age_ms = j.value("external_pose_age_ms", 0.0);
    rsp.external_pose_quality_score = j.value("external_pose_quality_score", 0.0);
    rsp.external_pose_transport = j.value("external_pose_transport", std::string{});
    if (j.contains("error_code")) {
      decode_error_code(j["error_code"], rsp.error_code);
    }
  } catch (...) {}
}

// =========================================================
// 运控：JSON → ControlJobResult
// =========================================================
inline void decode_control_job_result(
  const std::string & payload,
  calibration_control_interfaces::msg::ControlJobResult & result)
{
  if (payload.empty()) return;
  try {
    auto j = nlohmann::json::parse(payload);
    result.success                       = j.value("success",                       false);
    result.message                       = j.value("message",                       std::string{});
    result.job_id                        = j.value("job_id",                        std::string{});
    result.data_quality_passed           = j.value("data_quality_passed",           false);
    result.suitable_for_commit           = j.value("suitable_for_commit",           false);
    result.recommended_parameter_version = j.value("recommended_parameter_version", std::string{});
    if (j.contains("error_code")) {
      decode_error_code(j["error_code"], result.error_code);
    }
    if (j.contains("validation_summary")) {
      auto & vs = j["validation_summary"];
      result.validation_summary.rms_lateral_error_m  = vs.value("rms_lateral_error_m",  0.0);
      result.validation_summary.rms_heading_error_rad = vs.value("rms_heading_error_rad", 0.0);
      result.validation_summary.rms_speed_error_ms   = vs.value("rms_speed_error_ms",   0.0);
      result.validation_summary.overshoot_ratio       = vs.value("overshoot_ratio",       0.0);
      result.validation_summary.settle_time_sec       = vs.value("settle_time_sec",       0.0);
      result.validation_summary.stop_position_error_m = vs.value("stop_position_error_m", 0.0);
      result.validation_summary.max_jerk              = vs.value("max_jerk",              0.0);
      result.validation_summary.saturation_ratio      = vs.value("saturation_ratio",      0.0);
      result.validation_summary.auto_acceptance_passed = vs.value("auto_acceptance_passed", false);
    }
    if (j.contains("artifacts") && j["artifacts"].is_array()) {
      for (const auto & a : j["artifacts"]) {
        result.artifacts.push_back(decode_file_reference(a));
      }
    }
  } catch (...) {}
}

}  // namespace codec
}  // namespace vehicle_agent_gateway
