#include "chassis_calibration_service/chassis_calibration_algorithms.hpp"

#include "calibration_common_interfaces/msg/error_code.hpp"

namespace chassis_calibration_service
{

namespace
{
void fill_common_result(
  const ChassisCalibrationInput & input,
  ChassisCalibrationOutput & output)
{
  output.response.result.success = true;
  output.response.result.error_code.code = calibration_common_interfaces::msg::ErrorCode::OK;
  output.response.result.job_id = input.request.goal.header.request_id;
  output.response.result.data_quality_passed = true;
  output.response.result.suitable_for_commit = true;
  output.response.result.estimated_straight_line_bias = 0.0;
  output.response.result.validation_summary.max_lateral_error_m = 0.0;
  output.response.result.validation_summary.max_yaw_error_rad = 0.0;
  output.response.result.validation_summary.rms_lateral_error_m = 0.0;
  output.response.result.validation_summary.rms_yaw_error_rad = 0.0;
  output.response.result.validation_summary.repeatability_error_m = 0.0;
  output.response.result.validation_summary.curvature_error = 0.0;
  output.response.result.validation_summary.module_consistency_error = 0.0;
  output.response.result.validation_summary.auto_acceptance_passed = true;
  output.response.result.estimated_params.chassis_type = input.chassis_type;
}
}

bool AckermannChassisAlgorithm::run(
  const ChassisCalibrationInput & input,
  ChassisCalibrationOutput & output,
  std::string & failure_reason) const
{
  (void)failure_reason;
  fill_common_result(input, output);
  output.response.result.message = "ackermann chassis template executed.";
  output.response.result.recommended_parameter_version = "ackermann_template_v1";
  return true;
}

bool DifferentialChassisAlgorithm::run(
  const ChassisCalibrationInput & input,
  ChassisCalibrationOutput & output,
  std::string & failure_reason) const
{
  (void)failure_reason;
  fill_common_result(input, output);
  output.response.result.message = "differential chassis template executed.";
  output.response.result.recommended_parameter_version = "differential_template_v1";
  return true;
}

bool SingleSteerWheelChassisAlgorithm::run(
  const ChassisCalibrationInput & input,
  ChassisCalibrationOutput & output,
  std::string & failure_reason) const
{
  (void)failure_reason;
  fill_common_result(input, output);
  output.response.result.message = "single steer wheel chassis template executed.";
  output.response.result.recommended_parameter_version = "single_steer_template_v1";
  return true;
}

bool MultiSteerWheelChassisAlgorithm::run(
  const ChassisCalibrationInput & input,
  ChassisCalibrationOutput & output,
  std::string & failure_reason) const
{
  (void)failure_reason;
  fill_common_result(input, output);
  output.response.result.message = "multi steer wheel chassis template executed.";
  output.response.result.recommended_parameter_version = "multi_steer_template_v1";
  return true;
}

}  // namespace chassis_calibration_service
