#include "workshop_orchestrator_v2/control_gateway_client.hpp"

#include <chrono>
#include <cmath>
#include <future>
#include <map>
#include <sstream>
#include <stdexcept>
#include <unordered_map>

#include "calibration_common_interfaces/msg/key_value_pair.hpp"

#include "calibration_control_interfaces/msg/trajectory_point.hpp"
#include "workshop_orchestrator_v2/types.hpp"

namespace workshop_orchestrator_v2
{

using namespace std::chrono_literals;
using calibration_control_interfaces::msg::ControllerEvaluationTaskPurpose;
using calibration_control_interfaces::msg::ControllerEvaluationTaskType;
using calibration_workshop_orchestration_interfaces::msg::StagePlan;
using calibration_workshop_orchestration_interfaces::msg::StageResultSummary;
using calibration_workshop_orchestration_interfaces::msg::WorkshopSession;

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

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

bool ControlGatewayClient::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 = "control";
  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.trajectory_executor_ready) {
    failure_reason = "轨迹执行器未就绪，无法执行运控评估任务。";
    return false;
  }

  if (!response->response.control_output_ready) {
    failure_reason = "控制输出链路未就绪，无法执行运控评估任务。";
    return false;
  }

  return true;
}

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

  const auto task_type_it = metadata_map.find(metadata_keys::CONTROL_TASK_TYPE);
  if (task_type_it == metadata_map.end()) {
    failure_reason = "运控阶段缺少 control.task_type。";
    return ControllerEvaluationTaskType::CONTROL_EVALUATION_TASK_UNSPECIFIED;
  }

  if (task_type_it->second == "trajectory_tracking") {
    return ControllerEvaluationTaskType::TRAJECTORY_TRACKING;
  }
  if (task_type_it->second == "velocity_step") {
    return ControllerEvaluationTaskType::VELOCITY_STEP;
  }
  if (task_type_it->second == "accel_decel") {
    return ControllerEvaluationTaskType::ACCELERATION_DECELERATION;
  }
  if (task_type_it->second == "stop_accuracy") {
    return ControllerEvaluationTaskType::STOP_ACCURACY;
  }

  failure_reason = "不支持的运控任务类型：" + task_type_it->second;
  return ControllerEvaluationTaskType::CONTROL_EVALUATION_TASK_UNSPECIFIED;
}

bool ControlGatewayClient::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.test_case_id = stage.stage_id;

  // 当前目的：调参评估（不是最终验收）。
  goal.goal.task_purpose.value = ControllerEvaluationTaskPurpose::TUNING_EVALUATION;

  const auto selected_task_type = resolve_task_type(stage, failure_reason);
  if (selected_task_type == ControllerEvaluationTaskType::CONTROL_EVALUATION_TASK_UNSPECIFIED) {
    return false;
  }
  goal.goal.selected_task.value = selected_task_type;

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

  if (selected_task_type == ControllerEvaluationTaskType::TRAJECTORY_TRACKING) {
    goal.goal.trajectory_tracking.stop_at_end = true; // 执行完后停车，保证安全。
    goal.goal.trajectory_tracking.timeout_sec = 60.0;

    const auto stop_it = metadata_map.find(metadata_keys::CONTROL_STOP_AT_END);
    if (stop_it != metadata_map.end()) {
      goal.goal.trajectory_tracking.stop_at_end =
        stop_it->second == "true" || stop_it->second == "1" || stop_it->second == "TRUE";
    }

    const auto timeout_it = metadata_map.find(metadata_keys::CONTROL_TIMEOUT_SEC);
    if (timeout_it != metadata_map.end()) {
      try {
        goal.goal.trajectory_tracking.timeout_sec = std::stod(timeout_it->second);
      } catch (...) {
        failure_reason = "control.timeout_sec 格式错误。";
        return false;
      }
    }

    // 轨迹必须由流程配置显式提供；编排器不再隐式生成默认轨迹。
    goal.goal.trajectory_tracking.path = build_trajectory_from_metadata(stage, failure_reason);
    if (goal.goal.trajectory_tracking.path.empty()) {
      if (failure_reason.empty()) {
        failure_reason = "运控阶段缺少轨迹输入，无法启动评估任务。";
      }
      return false;
    }
  } else if (selected_task_type == ControllerEvaluationTaskType::VELOCITY_STEP) {
    const auto velocity_it = metadata_map.find(metadata_keys::CONTROL_VELOCITY_STEP_TARGET_VELOCITY_MS);
    const auto hold_it = metadata_map.find(metadata_keys::CONTROL_VELOCITY_STEP_HOLD_TIME_SEC);
    const auto settle_it = metadata_map.find(metadata_keys::CONTROL_VELOCITY_STEP_SETTLE_BEFORE_STEP_SEC);
    if (velocity_it == metadata_map.end() || hold_it == metadata_map.end() || settle_it == metadata_map.end()) {
      failure_reason = "velocity_step 缺少 target_velocity_ms、hold_time_sec 或 settle_before_step_sec。";
      return false;
    }
    try {
      goal.goal.velocity_step.target_velocity_ms = std::stod(velocity_it->second);
      goal.goal.velocity_step.hold_time_sec = std::stod(hold_it->second);
      goal.goal.velocity_step.settle_before_step_sec = std::stod(settle_it->second);
    } catch (...) {
      failure_reason = "velocity_step 参数格式错误。";
      return false;
    }
  } else if (selected_task_type == ControllerEvaluationTaskType::ACCELERATION_DECELERATION) {
    const auto start_it = metadata_map.find(metadata_keys::CONTROL_ACCEL_DECEL_START_VELOCITY_MS);
    const auto target_it = metadata_map.find(metadata_keys::CONTROL_ACCEL_DECEL_TARGET_VELOCITY_MS);
    const auto accel_it = metadata_map.find(metadata_keys::CONTROL_ACCEL_DECEL_TARGET_ACCEL_MS2);
    const auto hold_it = metadata_map.find(metadata_keys::CONTROL_ACCEL_DECEL_HOLD_TIME_SEC);
    if (start_it == metadata_map.end() || target_it == metadata_map.end() ||
      accel_it == metadata_map.end() || hold_it == metadata_map.end())
    {
      failure_reason = "accel_decel 缺少 start_velocity_ms、target_velocity_ms、target_accel_ms2 或 hold_time_sec。";
      return false;
    }
    try {
      goal.goal.accel_decel.start_velocity_ms = std::stod(start_it->second);
      goal.goal.accel_decel.target_velocity_ms = std::stod(target_it->second);
      goal.goal.accel_decel.target_accel_ms2 = std::stod(accel_it->second);
      goal.goal.accel_decel.hold_time_sec = std::stod(hold_it->second);
    } catch (...) {
      failure_reason = "accel_decel 参数格式错误。";
      return false;
    }
  } else if (selected_task_type == ControllerEvaluationTaskType::STOP_ACCURACY) {
    const auto x_it = metadata_map.find(metadata_keys::CONTROL_STOP_ACCURACY_X_M);
    const auto y_it = metadata_map.find(metadata_keys::CONTROL_STOP_ACCURACY_Y_M);
    const auto yaw_it = metadata_map.find(metadata_keys::CONTROL_STOP_ACCURACY_YAW_RAD);
    const auto timeout_it = metadata_map.find(metadata_keys::CONTROL_TIMEOUT_SEC);
    if (x_it == metadata_map.end() || y_it == metadata_map.end() ||
      yaw_it == metadata_map.end() || timeout_it == metadata_map.end())
    {
      failure_reason = "stop_accuracy 缺少 target_stop_x_m、target_stop_y_m、target_stop_yaw_rad 或 control.timeout_sec。";
      return false;
    }
    try {
      goal.goal.stop_accuracy.target_stop_x_m = std::stod(x_it->second);
      goal.goal.stop_accuracy.target_stop_y_m = std::stod(y_it->second);
      goal.goal.stop_accuracy.target_stop_yaw_rad = std::stod(yaw_it->second);
      goal.goal.stop_accuracy.timeout_sec = std::stod(timeout_it->second);
    } catch (...) {
      failure_reason = "stop_accuracy 参数格式错误。";
      return false;
    }
  }

  goal.goal.source_iteration_id = session.session_id;

  return true;
}

std::vector<calibration_control_interfaces::msg::TrajectoryPoint>
ControlGatewayClient::build_trajectory_from_metadata(
  const StagePlan & stage,
  std::string & failure_reason) const
{
  using TrajectoryPoint = calibration_control_interfaces::msg::TrajectoryPoint;

  // 从 metadata 中按约定的 key 解析轨迹点。
  // key 约定："traj_pt_{index}_{field}"，field 取 x_m / y_m / yaw_rad / speed_ms。
  // 例如：traj_pt_0_x_m = "0.0", traj_pt_0_y_m = "0.0", ...
  std::map<int, TrajectoryPoint> pt_map;
  for (const auto & kv : stage.metadata) {
    // 解析 key 格式
    const std::string & key = kv.key;
    if (key.rfind(metadata_keys::CONTROL_TRAJECTORY_PREFIX, 0) != 0) {
      continue; // 不是轨迹点 key，跳过。
    }
    // key 形如 "traj_pt_0_x_m"，提取 index 和 field。
    const auto after_prefix = key.substr(8); // 去掉 metadata_keys::CONTROL_TRAJECTORY_PREFIX
    const auto sep = after_prefix.find('_');
    if (sep == std::string::npos) {
      continue;
    }
    int idx = 0;
    try {
      idx = std::stoi(after_prefix.substr(0, sep));
    } catch (...) {
      continue;
    }
    const std::string field = after_prefix.substr(sep + 1);
    double val = 0.0;
    try {
      val = std::stod(kv.value);
    } catch (...) {
      continue;
    }

    if (field == "x_m") {
      pt_map[idx].x_m = val;
    } else if (field == "y_m") {
      pt_map[idx].y_m = val;
    } else if (field == "yaw_rad") {
      pt_map[idx].yaw_rad = val;
    } else if (field == "speed_ms") {
      pt_map[idx].target_speed_ms = val;
    }
  }

  if (!pt_map.empty()) {
    // 按 index 升序收集轨迹点。
    std::vector<TrajectoryPoint> path;
    path.reserve(pt_map.size());
    for (const auto & [idx, pt] : pt_map) {
      (void)idx;
      path.push_back(pt);
    }
    return path;
  }

  failure_reason = "trajectory_tracking 缺少轨迹点数据（traj_pt_{i}_{x_m|y_m|yaw_rad|speed_ms}）。";
  return {};
}

bool ControlGatewayClient::execute_control_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：等待结果，超时 120 秒（轨迹跟踪评估需要较长时间）。
  auto result_future = execute_task_client_->async_get_result(goal_handle);
  if (result_future.wait_for(120s) != 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：翻译 ControlJobResult → 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;
  result.artifacts.clear();
  for (const auto & artifact : job_result.artifacts) {
    result.artifacts.push_back(artifact);
  }
  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
