#include "chassis_calibration_service/chassis_calibration_algorithm_template.hpp"

#include <cmath>

namespace chassis_calibration_service
{

namespace
{

using calibration_chassis_interfaces::msg::ChassisMotionPrimitiveType;

bool is_positive(double value)
{
  return std::isfinite(value) && value > 0.0;
}

bool is_non_zero(double value)
{
  return std::isfinite(value) && std::abs(value) > 0.0;
}

bool validate_straight_line(
  const ExecuteTask::Goal & request,
  std::string & failure_reason)
{
  if (!is_positive(request.goal.straight_line.target_distance_m)) {
    failure_reason = "straight_line.target_distance_m 必须大于 0。";
    return false;
  }
  if (!is_positive(request.goal.straight_line.target_speed_ms)) {
    failure_reason = "straight_line.target_speed_ms 必须大于 0。";
    return false;
  }
  return true;
}

bool validate_arc(
  const ExecuteTask::Goal & request,
  std::string & failure_reason)
{
  if (!is_positive(request.goal.arc.target_speed_ms)) {
    failure_reason = "arc.target_speed_ms 必须大于 0。";
    return false;
  }
  if (!is_positive(request.goal.arc.radius_m)) {
    failure_reason = "arc.radius_m 必须大于 0。";
    return false;
  }
  if (!is_non_zero(request.goal.arc.sweep_angle_deg)) {
    failure_reason = "arc.sweep_angle_deg 不能为 0。";
    return false;
  }
  return true;
}

bool validate_in_place_rotation(
  const ExecuteTask::Goal & request,
  std::string & failure_reason)
{
  if (!is_non_zero(request.goal.in_place_rotation.target_yaw_deg)) {
    failure_reason = "in_place_rotation.target_yaw_deg 不能为 0。";
    return false;
  }
  if (!is_positive(request.goal.in_place_rotation.target_angular_vel_deg_s)) {
    failure_reason = "in_place_rotation.target_angular_vel_deg_s 必须大于 0。";
    return false;
  }
  return true;
}

bool validate_steering_sweep(
  const ExecuteTask::Goal & request,
  std::string & failure_reason)
{
  if (!std::isfinite(request.goal.steering_sweep.target_angle_deg)) {
    failure_reason = "steering_sweep.target_angle_deg 必须是有效数字。";
    return false;
  }
  if (!is_positive(request.goal.steering_sweep.sweep_amplitude_deg)) {
    failure_reason = "steering_sweep.sweep_amplitude_deg 必须大于 0。";
    return false;
  }
  if (!is_positive(request.goal.steering_sweep.sweep_frequency_hz)) {
    failure_reason = "steering_sweep.sweep_frequency_hz 必须大于 0。";
    return false;
  }
  if (!is_positive(request.goal.steering_sweep.duration_sec)) {
    failure_reason = "steering_sweep.duration_sec 必须大于 0。";
    return false;
  }
  return true;
}

bool validate_lateral_translation(
  const ExecuteTask::Goal & request,
  std::string & failure_reason)
{
  if (!is_positive(request.goal.lateral_translation.target_speed_ms)) {
    failure_reason = "lateral_translation.target_speed_ms 必须大于 0。";
    return false;
  }
  if (!is_positive(request.goal.lateral_translation.target_distance_m)) {
    failure_reason = "lateral_translation.target_distance_m 必须大于 0。";
    return false;
  }
  return true;
}

bool validate_diagonal_motion(
  const ExecuteTask::Goal & request,
  std::string & failure_reason)
{
  if (!is_positive(request.goal.diagonal_motion.target_speed_ms)) {
    failure_reason = "diagonal_motion.target_speed_ms 必须大于 0。";
    return false;
  }
  if (!is_positive(request.goal.diagonal_motion.target_distance_m)) {
    failure_reason = "diagonal_motion.target_distance_m 必须大于 0。";
    return false;
  }
  if (!std::isfinite(request.goal.diagonal_motion.heading_deg)) {
    failure_reason = "diagonal_motion.heading_deg 必须是有效数字。";
    return false;
  }
  return true;
}

bool validate_module_alignment(
  const ExecuteTask::Goal & request,
  std::string & failure_reason)
{
  if (request.goal.module_alignment.module_ids.empty()) {
    failure_reason = "module_alignment.module_ids 不能为空。";
    return false;
  }
  if (!std::isfinite(request.goal.module_alignment.target_zero_deg)) {
    failure_reason = "module_alignment.target_zero_deg 必须是有效数字。";
    return false;
  }
  if (!is_positive(request.goal.module_alignment.tolerance_deg)) {
    failure_reason = "module_alignment.tolerance_deg 必须大于 0。";
    return false;
  }
  return true;
}

bool validate_coordinated_steering(
  const ExecuteTask::Goal & request,
  std::string & failure_reason)
{
  if (request.goal.coordinated_steering.module_ids.empty()) {
    failure_reason = "coordinated_steering.module_ids 不能为空。";
    return false;
  }
  if (!std::isfinite(request.goal.coordinated_steering.target_angle_deg)) {
    failure_reason = "coordinated_steering.target_angle_deg 必须是有效数字。";
    return false;
  }
  if (!is_positive(request.goal.coordinated_steering.hold_time_sec)) {
    failure_reason = "coordinated_steering.hold_time_sec 必须大于 0。";
    return false;
  }
  return true;
}

}  // namespace

bool ChassisCalibrationAlgorithmTemplate::validate_input(
  const Input & input,
  std::string & failure_reason) const
{
  if (input.request.goal.header.request_id.empty()) {
    failure_reason = "chassis 任务 request_id 不能为空。";
    return false;
  }

  if (input.chassis_type.value == ChassisType::CHASSIS_TYPE_UNSPECIFIED) {
    failure_reason = "底盘类型不能为空，必须明确是阿克曼、差速轮、单舵轮或多舵轮。";
    return false;
  }

  if (!is_positive(input.request.goal.timeout_sec)) {
    failure_reason = "timeout_sec 必须大于 0。";
    return false;
  }

  switch (input.request.goal.selected_primitive.value) {
    case ChassisMotionPrimitiveType::STRAIGHT_LINE:
      return validate_straight_line(input.request, failure_reason);
    case ChassisMotionPrimitiveType::ARC:
      return validate_arc(input.request, failure_reason);
    case ChassisMotionPrimitiveType::IN_PLACE_ROTATION:
      return validate_in_place_rotation(input.request, failure_reason);
    case ChassisMotionPrimitiveType::STEERING_SWEEP:
      return validate_steering_sweep(input.request, failure_reason);
    case ChassisMotionPrimitiveType::LATERAL_TRANSLATION:
      return validate_lateral_translation(input.request, failure_reason);
    case ChassisMotionPrimitiveType::DIAGONAL_MOTION:
      return validate_diagonal_motion(input.request, failure_reason);
    case ChassisMotionPrimitiveType::MODULE_ALIGNMENT:
      return validate_module_alignment(input.request, failure_reason);
    case ChassisMotionPrimitiveType::COORDINATED_STEERING:
      return validate_coordinated_steering(input.request, failure_reason);
    default:
      failure_reason = "底盘动作原语类型不能为空，且必须是已支持的动作原语。";
      return false;
  }
}

const ChassisCalibrationAlgorithm * ChassisCalibrationAlgorithmTemplate::resolve_algorithm(
  const ChassisType & chassis_type) const
{
  switch (chassis_type.value) {
    case ChassisType::ACKERMANN:
      return &ackermann_algorithm_;
    case ChassisType::DIFFERENTIAL:
      return &differential_algorithm_;
    case ChassisType::SINGLE_STEER_WHEEL:
      return &single_steer_wheel_algorithm_;
    case ChassisType::MULTI_STEER_WHEEL:
      return &multi_steer_wheel_algorithm_;
    default:
      return nullptr;
  }
}

bool ChassisCalibrationAlgorithmTemplate::run(
  const Input & input,
  Output & output,
  std::string & failure_reason) const
{
  if (!validate_input(input, failure_reason)) {
    return false;
  }

  const auto * algorithm = resolve_algorithm(input.chassis_type);
  if (algorithm == nullptr) {
    failure_reason = "不支持的底盘类型。";
    return false;
  }

  // 这里是标定算法的统一入口：先按底盘类型分发，再由对应模板文件实现具体算法。
  // 算法工程师通常会先检查：
  // - input.workshop_session / input.vehicle_profile：会话级上下文和车辆静态画像
  // - input.chassis_*：底盘能力、就绪状态、工作模式、参数、遥测
  // - input.control_*：控制链路、控制参数、控制误差和输出
  // - input.sensor_*：传感器就绪状态、已生效参数、采集质量
  // - input.external_localization_*：真值源就绪状态和外部定位观测
  // 再进入具体拟合与参数求解。
  return algorithm->run(input, output, failure_reason);
}

}  // namespace chassis_calibration_service
