#pragma once

#include <algorithm>
#include <chrono>
#include <cmath>
#include <fstream>
#include <sstream>
#include <string>
#include <thread>
#include <vector>
#include <unistd.h>

namespace r1_vision {

struct Detection {
    bool valid = false;
    std::string class_name;
    double timestamp = 0.0;
    double confidence = 0.0;
    double u = 0.0;
    double v = 0.0;
};

inline bool ExtractNumber(const std::string& json, const std::string& key, double& value) {
    const std::string search = "\"" + key + "\"";
    const size_t p = json.find(search);
    if (p == std::string::npos) return false;
    const size_t colon = json.find(':', p);
    if (colon == std::string::npos) return false;
    try {
        value = std::stod(json.substr(colon + 1));
        return true;
    } catch (...) {
        return false;
    }
}

inline bool ExtractString(const std::string& json, const std::string& key, std::string& value) {
    const std::string search = "\"" + key + "\"";
    const size_t p = json.find(search);
    if (p == std::string::npos) return false;
    const size_t colon = json.find(':', p);
    if (colon == std::string::npos) return false;
    const size_t q1 = json.find('"', colon + 1);
    if (q1 == std::string::npos) return false;
    const size_t q2 = json.find('"', q1 + 1);
    if (q2 == std::string::npos) return false;
    value = json.substr(q1 + 1, q2 - q1 - 1);
    return true;
}

inline bool ReadDetection(const std::string& path, Detection& d) {
    std::ifstream f(path);
    if (!f) return false;
    std::stringstream buffer;
    buffer << f.rdbuf();
    const std::string json = buffer.str();
    if (json.empty()) return false;

    if (!ExtractNumber(json, "timestamp", d.timestamp)) return false;
    if (!ExtractNumber(json, "confidence", d.confidence)) return false;
    if (!ExtractNumber(json, "u", d.u)) return false;
    if (!ExtractNumber(json, "v", d.v)) return false;
    ExtractString(json, "class", d.class_name);
    d.valid = true;
    return true;
}

inline bool IsFresh(const Detection& d, double max_age_sec = 0.5) {
    const double now = std::chrono::duration<double>(
        std::chrono::system_clock::now().time_since_epoch()).count();
    return std::isfinite(d.timestamp) && std::abs(now - d.timestamp) <= max_age_sec;
}

inline bool GetStableBottleCenter(
    const std::string& path,
    double& u,
    double& v,
    int samples = 8,
    double min_confidence = 0.7,
    double timeout_sec = 5.0)
{
    std::vector<double> us;
    std::vector<double> vs;
    const auto start = std::chrono::steady_clock::now();

    while (static_cast<int>(us.size()) < samples) {
        Detection d;
        if (ReadDetection(path, d) && d.valid &&
            (d.class_name.empty() || d.class_name == "bottle") &&
            d.confidence >= min_confidence && IsFresh(d)) {
            us.push_back(d.u);
            vs.push_back(d.v);
        }

        const double elapsed = std::chrono::duration<double>(
            std::chrono::steady_clock::now() - start).count();
        if (elapsed > timeout_sec) return false;
        std::this_thread::sleep_for(std::chrono::milliseconds(25));
    }

    std::sort(us.begin(), us.end());
    std::sort(vs.begin(), vs.end());
    const auto median = [](const std::vector<double>& x) {
        const size_t n = x.size();
        if (n & 1u) return x[n / 2];
        return 0.5 * (x[n / 2 - 1] + x[n / 2]);
    };

    u = median(us);
    v = median(vs);
    return true;
}

} // namespace r1_vision