#pragma once
// external_pose_store.hpp
// 车端缓存车间电脑推送来的外部定位车辆位姿。

#include <chrono>
#include <mutex>
#include <string>

#include "data_types.hpp"

namespace vaw
{

struct ExternalPoseStatus
{
    bool ready{false};
    std::string source_name;
    double age_ms{0.0};
    double quality_score{0.0};
    std::string transport{"wifi6_tcp"};
};

class ExternalPoseStore
{
public:
    StandardResponse update(const ExternalLocalizationTelemetry& telemetry)
    {
        std::lock_guard<std::mutex> lock(mutex_);
        latest_ = telemetry;
        receive_wall_time_us_ = now_us();
        has_latest_ = true;

        StandardResponse rsp;
        rsp.success = true;
        rsp.error_code = ErrorCode::OK;
        rsp.message = telemetry.pose_valid ? "外部定位位姿已接收" : "外部定位位姿已接收，但 pose_valid=false";
        return rsp;
    }

    bool latest(ExternalLocalizationTelemetry& telemetry) const
    {
        std::lock_guard<std::mutex> lock(mutex_);
        if (!has_latest_) {
            return false;
        }
        telemetry = latest_;
        return true;
    }

    ExternalPoseStatus status(double max_age_ms) const
    {
        std::lock_guard<std::mutex> lock(mutex_);
        ExternalPoseStatus status;
        if (!has_latest_) {
            return status;
        }

        status.source_name = latest_.reference_source_name;
        status.quality_score = latest_.quality_score;
        status.age_ms = static_cast<double>(now_us() - receive_wall_time_us_) / 1000.0;
        status.ready = latest_.pose_valid && status.age_ms <= max_age_ms && latest_.quality_score > 0.0;
        return status;
    }

private:
    static int64_t now_us()
    {
        return std::chrono::duration_cast<std::chrono::microseconds>(
            std::chrono::system_clock::now().time_since_epoch()).count();
    }

    mutable std::mutex mutex_;
    ExternalLocalizationTelemetry latest_;
    int64_t receive_wall_time_us_{0};
    bool has_latest_{false};
};

}  // namespace vaw
