#include "external_localization_service/external_reference_executor.hpp"

#include <string>

#include "calibration_common_interfaces/msg/key_value_pair.hpp"
#include "calibration_common_interfaces/msg/job_state.hpp"
#include "external_localization_service/external_localization_common.hpp"

namespace external_localization_service
{

bool ExternalReferenceExecutor::validate_goal(
  const ExecuteTask::Goal & goal,
  std::string & reject_reason) const
{
  if (goal.goal.localization_source_id.empty()) {
    reject_reason = "localization_source_id 不能为空。";
    return false;
  }
  if (goal.goal.workcell_zone_id.empty()) {
    reject_reason = "workcell_zone_id 不能为空。";
    return false;
  }
  if (goal.goal.header.request_id.empty()) {
    reject_reason = "request_id 不能为空。";
    return false;
  }
  return true;
}

bool ExternalReferenceExecutor::build_result(
  const ExecuteTask::Goal & goal,
  const calibration_external_localization_interfaces::msg::ExternalLocalizationReadinessResponse & readiness,
  const std::vector<calibration_external_localization_interfaces::msg::ExternalLocalizationTelemetry> & telemetry_history,
  const DataInputConfig & data_input_config,
  ExecuteTask::Result & external_result,
  std::string & failure_reason) const
{
  if (!validate_goal(goal, failure_reason)) {
    return false;
  }

  ExternalLocalizationInput input;
  input.request = goal;
  input.task_type = goal.goal.selected_task;
  input.reference_pose_collection_task = goal.goal.reference_pose_collection;
  input.marker_alignment_task = goal.goal.marker_alignment;
  input.truth_source_validation_task = goal.goal.truth_source_validation;

  input.required_reference_pose_sample_count = goal.goal.reference_pose_collection.sample_count;
  input.required_marker_observation_count = goal.goal.marker_alignment.min_valid_observation_count;
  input.required_static_sample_count = goal.goal.truth_source_validation.static_sample_count;
  input.required_dynamic_sample_count = goal.goal.truth_source_validation.dynamic_sample_count;
  input.require_vehicle_static = goal.goal.reference_pose_collection.require_vehicle_static;
  input.require_short_motion_segment = goal.goal.truth_source_validation.require_short_motion_segment;
  input.reference_timeout_sec = 0.0;
  if (goal.goal.selected_task.value == input.task_type.REFERENCE_POSE_COLLECTION) {
    input.reference_timeout_sec = goal.goal.reference_pose_collection.timeout_sec;
  } else if (goal.goal.selected_task.value == input.task_type.MARKER_ALIGNMENT) {
    input.reference_timeout_sec = goal.goal.marker_alignment.timeout_sec;
  } else if (goal.goal.selected_task.value == input.task_type.TRUTH_SOURCE_VALIDATION) {
    input.reference_timeout_sec = goal.goal.truth_source_validation.timeout_sec;
  }
  input.min_sample_interval_sec = goal.goal.reference_pose_collection.min_sample_interval_sec;
  input.accepted_max_position_stddev_m = goal.goal.truth_source_validation.max_position_stddev_m;
  input.accepted_max_yaw_stddev_rad = goal.goal.truth_source_validation.max_yaw_stddev_rad;
  input.accepted_max_tracking_loss_ratio = goal.goal.truth_source_validation.max_tracking_loss_ratio;
  input.accepted_max_time_sync_offset_ms = goal.goal.truth_source_validation.max_time_sync_offset_ms;
  input.reference_board_id = goal.goal.marker_alignment.target_board_id;
  input.localization_source_id = goal.goal.localization_source_id;
  input.workcell_zone_id = goal.goal.workcell_zone_id;

  input.candidate_calibration_result.workshop_frame_id = "workshop";
  input.candidate_calibration_result.localization_frame_id = goal.goal.localization_source_id;
  input.candidate_calibration_result.localization_source_id = goal.goal.localization_source_id;
  input.candidate_calibration_result.workcell_zone_id = goal.goal.workcell_zone_id;
  input.external_localization_readiness = readiness;
  input.external_localization_telemetry_history = telemetry_history;
  if (!telemetry_history.empty()) {
    input.latest_external_localization_telemetry = telemetry_history.back();
    input.candidate_calibration_result.workshop_to_localization = telemetry_history.back().workshop_pose;
    input.candidate_calibration_result.tracking_loss_ratio = telemetry_history.back().tracking_loss_ratio;
    input.candidate_calibration_result.time_sync_offset_ms = telemetry_history.back().time_sync_offset_ms;
  }
  input.truth_source_diagnostics.push_back("telemetry_samples=" + std::to_string(telemetry_history.size()));
  input.truth_source_diagnostics.push_back(
    std::string("ready_for_reference_validation=") +
    (readiness.ready_for_reference_validation ? "true" : "false"));

  input.external_history_available = !input.external_localization_telemetry_history.empty();
  input.chassis_history_available = !input.chassis_telemetry_history.empty();
  input.control_history_available = !input.control_telemetry_history.empty();
  input.sensor_history_available = !input.sensor_telemetry_history.empty();
  input.data_window_start_timestamp_us = data_input_config.data_window_start_timestamp_us;
  input.data_window_end_timestamp_us = data_input_config.data_window_end_timestamp_us;
  input.external_observation_files = data_input_config.external_observation_files;
  input.marker_observation_files = data_input_config.marker_observation_files;
  input.time_sync_diagnostic_files = data_input_config.time_sync_diagnostic_files;
  input.acquisition_diagnostics.push_back(
    "external_observation_files=" + std::to_string(input.external_observation_files.size()));

  ExternalLocalizationOutput output;
  if (!algorithm_.run(input, output, failure_reason)) {
    fill_external_localization_common_failure(output, failure_reason);
    external_result = output.response;
    return false;
  }

  external_result = output.response;
  return true;
}

void ExternalReferenceExecutor::fill_stage_result_from_external_result(
  const ExecuteTask::Result & external_result,
  calibration_workshop_orchestration_interfaces::msg::StageResultSummary & stage_result,
  std::string & failure_reason) const
{
  stage_result.success = external_result.result.success;
  stage_result.auto_acceptance_passed =
    external_result.result.data_quality_passed &&
    external_result.result.validation_summary.recommended_as_truth_source;
  stage_result.final_job_state.state =
    external_result.result.success ? calibration_common_interfaces::msg::JobState::SUCCEEDED
                                   : calibration_common_interfaces::msg::JobState::FAILED;
  stage_result.parameter_version = external_result.result.recommended_parameter_version;
  stage_result.artifacts = external_result.result.artifacts;
  stage_result.summary = external_result.result.message.empty() ?
    "外部真值校核完成。" : external_result.result.message;
  stage_result.failure_root_cause = external_result.result.success ? "" : stage_result.summary;
  stage_result.metadata.clear();

  calibration_common_interfaces::msg::KeyValuePair kv;
  kv.key = "task_purpose";
  kv.value = std::to_string(external_result.result.task_purpose.value);
  stage_result.metadata.push_back(kv);
  kv.key = "task_type";
  kv.value = std::to_string(external_result.result.task_purpose.value);
  stage_result.metadata.push_back(kv);
  kv.key = "data_quality_passed";
  kv.value = external_result.result.data_quality_passed ? "true" : "false";
  stage_result.metadata.push_back(kv);
  kv.key = "suitable_for_commit";
  kv.value = external_result.result.suitable_for_commit ? "true" : "false";
  stage_result.metadata.push_back(kv);
  kv.key = "recommended_as_truth_source";
  kv.value = external_result.result.validation_summary.recommended_as_truth_source ? "true" : "false";
  stage_result.metadata.push_back(kv);

  if (!external_result.result.success) {
    failure_reason = stage_result.summary;
    return;
  }
  if (!stage_result.auto_acceptance_passed) {
    failure_reason = "外部真值校核未通过自动验收。";
    stage_result.failure_root_cause = failure_reason;
  }
}

}  // namespace external_localization_service
