#include "workshop_orchestrator_v2/chassis_gateway_client.hpp"

#include <chrono>
#include <cctype>
#include <future>
#include <sstream>
#include <unordered_map>
#include <vector>

#include "workshop_orchestrator_v2/types.hpp"

namespace workshop_orchestrator_v2
{

using namespace std::chrono_literals;
using calibration_chassis_interfaces::msg::ChassisMotionPrimitiveType;
using calibration_chassis_interfaces::msg::MotionPrimitiveTaskPurpose;
using calibration_workshop_orchestration_interfaces::msg::StagePlan;
using calibration_workshop_orchestration_interfaces::msg::StageResultSummary;
using calibration_workshop_orchestration_interfaces::msg::WorkshopSession;

namespace
{

bool parse_bool_value(const std::string & value)
{
  return value == "true" || value == "1" || value == "TRUE" || value == "True";
}

bool parse_double_metadata(
  const std::unordered_map<std::string, std::string> & metadata_map,
  const std::string & key,
  double & output)
{
  const auto it = metadata_map.find(key);
  if (it == metadata_map.end()) {
    return false;
  }
  try {
    output = std::stod(it->second);
    return true;
  } catch (...) {
    return false;
  }
}

bool parse_string_list_metadata(
  const std::unordered_map<std::string, std::string> & metadata_map,
  const std::string & key,
  std::vector<std::string> & output)
{
  const auto it = metadata_map.find(key);
  if (it == metadata_map.end()) {
    return false;
  }

  output.clear();
  std::string token;
  for (char ch : it->second) {
    if (ch == ',' || ch == ';' || std::isspace(static_cast<unsigned char>(ch))) {
      if (!token.empty()) {
        output.push_back(token);
        token.clear();
      }
      continue;
    }
    token.push_back(ch);
  }
  if (!token.empty()) {
    output.push_back(token);
  }
  return !output.empty();
}

}  // namespace

ChassisGatewayClient::ChassisGatewayClient(rclcpp::Node * node)
: node_(node)
{
  readiness_client_ = node_->create_client<ReadinessSrv>("/chassis/get_readiness");
  execute_task_client_ = rclcpp_action::create_client<ExecuteTask>(
    node_, "/chassis/execute_motion_primitive");
}

std::string ChassisGatewayClient::make_request_id() const
{
  std::ostringstream oss;
  oss << "chassis_req_" << now_us();
  return oss.str();
}

bool ChassisGatewayClient::check_ready(std::string & failure_reason)
{
  if (!readiness_client_->wait_for_service(2s)) {
    failure_reason = "底盘 readiness 服务不可用。";
    return false;
  }

  auto request = std::make_shared<ReadinessSrv::Request>();
  request->request.agent_name = "chassis";
  request->request.include_details = true;

  auto future = readiness_client_->async_send_request(request);
  if (future.wait_for(3s) != std::future_status::ready) {
    failure_reason = "等待底盘 readiness 响应超时。";
    return false;
  }

  const auto response = future.get();
  if (!response->response.success) {
    failure_reason = response->response.message.empty() ?
      "底盘 readiness 检查失败。" : response->response.message;
    return false;
  }

  if (!response->response.agent_ready) {
    failure_reason = "底盘 agent 当前未就绪。";
    return false;
  }

  // 急停未释放或车辆不安全时，禁止启动标定动作。
  if (!response->response.estop_released) {
    failure_reason = "底盘急停未释放，禁止执行标定动作。";
    return false;
  }

  if (!response->response.vehicle_safe_to_move) {
    failure_reason = "当前车辆不允许移动，无法执行底盘标定。";
    return false;
  }

  if (!response->response.motion_control_ready) {
    failure_reason = "运控链路未就绪，无法执行底盘动作原语。";
    return false;
  }

  return true;
}

bool ChassisGatewayClient::build_goal(
  const WorkshopSession & session,
  const StagePlan & stage,
  ExecuteTask::Goal & goal,
  std::string & failure_reason) const
{

  // 填充请求头，标识来源与任务归属。
  goal.goal.header.session_id = session.session_id;
  goal.goal.header.task_id = stage.stage_id;
  goal.goal.header.vehicle_id = session.vehicle_profile_snapshot.base_info.vehicle_id;
  goal.goal.header.request_id = make_request_id();
  goal.goal.header.client_send_timestamp_us = now_us();
  goal.goal.header.operator_id = session.operator_info.operator_id;
  goal.goal.header.workshop_host = "workshop_orchestrator_v2";

  // 当前阶段目的：数据采集（具体底盘动作和参数由流程配置或底盘专项包决定）。
  goal.goal.task_purpose.value = MotionPrimitiveTaskPurpose::DATA_COLLECTION;

  if (stage.metadata.empty()) {
    failure_reason = "底盘阶段缺少动作原语配置，无法启动底盘任务。";
    return false;
  }

  std::unordered_map<std::string, std::string> metadata_map;
  for (const auto & kv : stage.metadata) {
    metadata_map[kv.key] = kv.value;
  }

  const auto primitive_it = metadata_map.find(metadata_keys::CHASSIS_PRIMITIVE_TYPE);
  if (primitive_it == metadata_map.end()) {
    failure_reason = "底盘阶段缺少 primitive_type，无法确定要执行的动作原语。";
    return false;
  }

  if (primitive_it->second == "straight_line") {
    double target_distance_m = 0.0;
    double target_speed_ms = 0.0;
    if (!parse_double_metadata(
        metadata_map, metadata_keys::CHASSIS_STRAIGHT_LINE_DISTANCE_M, target_distance_m) ||
      !parse_double_metadata(
        metadata_map, metadata_keys::CHASSIS_STRAIGHT_LINE_SPEED_MS, target_speed_ms))
    {
      failure_reason = "straight_line 缺少 target_distance_m 或 target_speed_ms，或参数格式错误。";
      return false;
    }

    goal.goal.selected_primitive.value = ChassisMotionPrimitiveType::STRAIGHT_LINE;
    goal.goal.straight_line.target_distance_m = target_distance_m;
    goal.goal.straight_line.target_speed_ms = target_speed_ms;
    const auto reverse_it = metadata_map.find(metadata_keys::CHASSIS_STRAIGHT_LINE_REVERSE);
    goal.goal.straight_line.reverse =
      reverse_it != metadata_map.end() && parse_bool_value(reverse_it->second);
  } else if (primitive_it->second == "arc") {
    double target_speed_ms = 0.0;
    double radius_m = 0.0;
    double sweep_angle_deg = 0.0;
    if (!parse_double_metadata(
        metadata_map, metadata_keys::CHASSIS_ARC_SPEED_MS, target_speed_ms) ||
      !parse_double_metadata(
        metadata_map, metadata_keys::CHASSIS_ARC_RADIUS_M, radius_m) ||
      !parse_double_metadata(
        metadata_map, metadata_keys::CHASSIS_ARC_SWEEP_ANGLE_DEG, sweep_angle_deg))
    {
      failure_reason = "arc 缺少 target_speed_ms、radius_m 或 sweep_angle_deg，或参数格式错误。";
      return false;
    }

    goal.goal.selected_primitive.value = ChassisMotionPrimitiveType::ARC;
    goal.goal.arc.target_speed_ms = target_speed_ms;
    goal.goal.arc.radius_m = radius_m;
    goal.goal.arc.sweep_angle_deg = sweep_angle_deg;
    const auto clockwise_it = metadata_map.find(metadata_keys::CHASSIS_ARC_CLOCKWISE);
    goal.goal.arc.clockwise =
      clockwise_it != metadata_map.end() && parse_bool_value(clockwise_it->second);
  } else if (primitive_it->second == "in_place_rotation") {
    double target_yaw_deg = 0.0;
    double target_angular_vel_deg_s = 0.0;
    if (!parse_double_metadata(
        metadata_map, metadata_keys::CHASSIS_ROTATION_TARGET_YAW_DEG, target_yaw_deg) ||
      !parse_double_metadata(
        metadata_map, metadata_keys::CHASSIS_ROTATION_TARGET_ANGULAR_VEL_DEG_S,
        target_angular_vel_deg_s))
    {
      failure_reason = "in_place_rotation 缺少 target_yaw_deg 或 target_angular_vel_deg_s，或参数格式错误。";
      return false;
    }

    goal.goal.selected_primitive.value = ChassisMotionPrimitiveType::IN_PLACE_ROTATION;
    goal.goal.in_place_rotation.target_yaw_deg = target_yaw_deg;
    goal.goal.in_place_rotation.target_angular_vel_deg_s = target_angular_vel_deg_s;
  } else if (primitive_it->second == "steering_sweep") {
    double target_angle_deg = 0.0;
    double sweep_amplitude_deg = 0.0;
    double sweep_frequency_hz = 0.0;
    double duration_sec = 0.0;
    if (!parse_double_metadata(
        metadata_map, metadata_keys::CHASSIS_STEERING_SWEEP_TARGET_ANGLE_DEG, target_angle_deg) ||
      !parse_double_metadata(
        metadata_map, metadata_keys::CHASSIS_STEERING_SWEEP_AMPLITUDE_DEG, sweep_amplitude_deg) ||
      !parse_double_metadata(
        metadata_map, metadata_keys::CHASSIS_STEERING_SWEEP_FREQUENCY_HZ, sweep_frequency_hz) ||
      !parse_double_metadata(
        metadata_map, metadata_keys::CHASSIS_STEERING_SWEEP_DURATION_SEC, duration_sec))
    {
      failure_reason = "steering_sweep 缺少目标角、幅值、频率或持续时间，或参数格式错误。";
      return false;
    }

    goal.goal.selected_primitive.value = ChassisMotionPrimitiveType::STEERING_SWEEP;
    goal.goal.steering_sweep.target_angle_deg = target_angle_deg;
    goal.goal.steering_sweep.sweep_amplitude_deg = sweep_amplitude_deg;
    goal.goal.steering_sweep.sweep_frequency_hz = sweep_frequency_hz;
    goal.goal.steering_sweep.duration_sec = duration_sec;
  } else if (primitive_it->second == "lateral_translation") {
    double target_speed_ms = 0.0;
    double target_distance_m = 0.0;
    if (!parse_double_metadata(
        metadata_map, metadata_keys::CHASSIS_LATERAL_SPEED_MS, target_speed_ms) ||
      !parse_double_metadata(
        metadata_map, metadata_keys::CHASSIS_LATERAL_DISTANCE_M, target_distance_m))
    {
      failure_reason = "lateral_translation 缺少 target_speed_ms 或 target_distance_m，或参数格式错误。";
      return false;
    }

    goal.goal.selected_primitive.value = ChassisMotionPrimitiveType::LATERAL_TRANSLATION;
    goal.goal.lateral_translation.target_speed_ms = target_speed_ms;
    goal.goal.lateral_translation.target_distance_m = target_distance_m;
    const auto move_left_it = metadata_map.find(metadata_keys::CHASSIS_LATERAL_MOVE_LEFT);
    goal.goal.lateral_translation.move_left =
      move_left_it != metadata_map.end() && parse_bool_value(move_left_it->second);
  } else if (primitive_it->second == "diagonal_motion") {
    double target_speed_ms = 0.0;
    double target_distance_m = 0.0;
    double heading_deg = 0.0;
    if (!parse_double_metadata(
        metadata_map, metadata_keys::CHASSIS_DIAGONAL_SPEED_MS, target_speed_ms) ||
      !parse_double_metadata(
        metadata_map, metadata_keys::CHASSIS_DIAGONAL_DISTANCE_M, target_distance_m) ||
      !parse_double_metadata(
        metadata_map, metadata_keys::CHASSIS_DIAGONAL_HEADING_DEG, heading_deg))
    {
      failure_reason = "diagonal_motion 缺少 target_speed_ms、target_distance_m 或 heading_deg，或参数格式错误。";
      return false;
    }

    goal.goal.selected_primitive.value = ChassisMotionPrimitiveType::DIAGONAL_MOTION;
    goal.goal.diagonal_motion.target_speed_ms = target_speed_ms;
    goal.goal.diagonal_motion.target_distance_m = target_distance_m;
    goal.goal.diagonal_motion.heading_deg = heading_deg;
  } else if (primitive_it->second == "module_alignment") {
    std::vector<std::string> module_ids;
    double target_zero_deg = 0.0;
    double tolerance_deg = 0.0;
    if (!parse_string_list_metadata(
        metadata_map, metadata_keys::CHASSIS_MODULE_ALIGNMENT_IDS, module_ids) ||
      !parse_double_metadata(
        metadata_map, metadata_keys::CHASSIS_MODULE_ALIGNMENT_ZERO_DEG, target_zero_deg) ||
      !parse_double_metadata(
        metadata_map, metadata_keys::CHASSIS_MODULE_ALIGNMENT_TOLERANCE_DEG, tolerance_deg))
    {
      failure_reason = "module_alignment 缺少 module_ids、target_zero_deg 或 tolerance_deg，或参数格式错误。";
      return false;
    }

    goal.goal.selected_primitive.value = ChassisMotionPrimitiveType::MODULE_ALIGNMENT;
    goal.goal.module_alignment.module_ids = module_ids;
    goal.goal.module_alignment.target_zero_deg = target_zero_deg;
    goal.goal.module_alignment.tolerance_deg = tolerance_deg;
  } else if (primitive_it->second == "coordinated_steering") {
    std::vector<std::string> module_ids;
    double target_angle_deg = 0.0;
    double hold_time_sec = 0.0;
    if (!parse_string_list_metadata(
        metadata_map, metadata_keys::CHASSIS_COORDINATED_STEERING_IDS, module_ids) ||
      !parse_double_metadata(
        metadata_map, metadata_keys::CHASSIS_COORDINATED_STEERING_TARGET_ANGLE_DEG,
        target_angle_deg) ||
      !parse_double_metadata(
        metadata_map, metadata_keys::CHASSIS_COORDINATED_STEERING_HOLD_TIME_SEC, hold_time_sec))
    {
      failure_reason = "coordinated_steering 缺少 module_ids、target_angle_deg 或 hold_time_sec，或参数格式错误。";
      return false;
    }

    goal.goal.selected_primitive.value = ChassisMotionPrimitiveType::COORDINATED_STEERING;
    goal.goal.coordinated_steering.module_ids = module_ids;
    goal.goal.coordinated_steering.target_angle_deg = target_angle_deg;
    goal.goal.coordinated_steering.hold_time_sec = hold_time_sec;
  } else {
    failure_reason = "不支持的底盘动作原语类型：" + primitive_it->second;
    return false;
  }

  goal.goal.brake_when_finished = true;
  const auto brake_it = metadata_map.find(metadata_keys::COMMON_BRAKE_WHEN_FINISHED);
  if (brake_it != metadata_map.end()) {
    goal.goal.brake_when_finished = parse_bool_value(brake_it->second);
  }

  goal.goal.timeout_sec = 60.0;
  const auto timeout_it = metadata_map.find(metadata_keys::COMMON_TIMEOUT_SEC);
  if (timeout_it != metadata_map.end()) {
    try {
      goal.goal.timeout_sec = std::stod(timeout_it->second);
    } catch (...) {
      failure_reason = "timeout_sec 参数格式错误，无法解析为数字。";
      return false;
    }
  }

  goal.goal.source_iteration_id = session.session_id;
  goal.goal.test_case_id = stage.stage_id;
  return true;
}

bool ChassisGatewayClient::execute_chassis_stage(
  const WorkshopSession & session,
  const StagePlan & stage,
  StageResultSummary & result,
  std::string & failure_reason)
{
  // Step 1：就绪检查。
  if (!check_ready(failure_reason)) {
    result.success = false;
    result.auto_acceptance_passed = false;
    result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED);
    result.summary = failure_reason;
    result.failure_root_cause = failure_reason;
    return false;
  }

  ExecuteTask::Goal goal;
  if (!build_goal(session, stage, goal, failure_reason)) {
    result.success = false;
    result.auto_acceptance_passed = false;
    result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED);
    result.summary = failure_reason;
    result.failure_root_cause = failure_reason;
    return false;
  }

  // Step 2：等待 action server 上线。
  if (!execute_task_client_->wait_for_action_server(2s)) {
    failure_reason = "底盘 action server 不可用。";
    result.success = false;
    result.auto_acceptance_passed = false;
    result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED);
    result.summary = failure_reason;
    result.failure_root_cause = failure_reason;
    return false;
  }

  // Step 3：发送 goal。
  auto goal_handle_future = execute_task_client_->async_send_goal(goal);
  if (goal_handle_future.wait_for(5s) != std::future_status::ready) {
    failure_reason = "发送底盘动作原语请求超时。";
    result.success = false;
    result.auto_acceptance_passed = false;
    result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED);
    result.summary = failure_reason;
    result.failure_root_cause = failure_reason;
    return false;
  }

  auto goal_handle = goal_handle_future.get();
  if (!goal_handle) {
    failure_reason = "底盘拒绝了本次动作原语请求。";
    result.success = false;
    result.auto_acceptance_passed = false;
    result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED);
    result.summary = failure_reason;
    result.failure_root_cause = failure_reason;
    return false;
  }

  // Step 4：等待执行结果，超时 90 秒（底盘动作比外部定位耗时更长）。
  auto result_future = execute_task_client_->async_get_result(goal_handle);
  if (result_future.wait_for(90s) != std::future_status::ready) {
    failure_reason = "等待底盘动作原语结果超时。";
    result.success = false;
    result.auto_acceptance_passed = false;
    result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED);
    result.summary = failure_reason;
    result.failure_root_cause = failure_reason;
    return false;
  }

  auto wrapped_result = result_future.get();
  if (!wrapped_result.result) {
    failure_reason = "底盘返回了空结果。";
    result.success = false;
    result.auto_acceptance_passed = false;
    result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED);
    result.summary = failure_reason;
    result.failure_root_cause = failure_reason;
    return false;
  }

  // Step 5：翻译结果到 StageResultSummary。
  const auto & job_result = wrapped_result.result->result;
  result.success = job_result.success;
  // 自动验收：数据质量达标 且 估计参数适合写入。
  result.auto_acceptance_passed = job_result.data_quality_passed && job_result.suitable_for_commit;
  result.final_job_state = make_job_state(
    job_result.success ? calibration_common_interfaces::msg::JobState::SUCCEEDED
                       : calibration_common_interfaces::msg::JobState::FAILED);
  result.parameter_version = job_result.recommended_parameter_version;

  // 底盘接口中 artifacts 类型为 FileReference，此处直接赋值。
  // （StageResultSummary.artifacts 为 FileReference[]，类型一致。）
  result.artifacts = job_result.artifacts;

  result.summary = job_result.message.empty() ? "底盘标定数据采集完成。" : job_result.message;
  result.failure_root_cause = job_result.success ? "" : result.summary;

  if (!job_result.success) {
    failure_reason = result.summary;
    return false;
  }
  if (!result.auto_acceptance_passed) {
    failure_reason = "底盘标定数据未通过自动验收（质量或参数适合性不满足）。";
    result.failure_root_cause = failure_reason;
    return false;
  }

  return true;
}

}  // namespace workshop_orchestrator_v2
