#include "sweep_orchestrator.hpp"

#include <chrono>
#include <exception>
#include <stdexcept>
#include <thread>
#include <vector>

namespace radar::acq {
namespace {

[[nodiscard]] inline auto should_stop(const std::atomic<bool>& stop_requested) -> bool {
    return stop_requested.load(std::memory_order_relaxed);
}

void sleep_if_needed_ms(std::uint32_t delay_ms) {
    if (delay_ms == 0U) {
        return;
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
}

void validate_sweep(const drivers::SweepTrace& sweep) {
    if (sweep.frequency_hz.size() != sweep.s11.size() || sweep.frequency_hz.size() != sweep.s21.size()) {
        throw std::runtime_error("Radar driver returned inconsistent sweep vectors");
    }
}

class DriverLifecycleGuard {
  public:
    DriverLifecycleGuard(
        drivers::RadarDriver& radar_driver,
        drivers::SwitchDriver& input_switch_driver,
        drivers::SwitchDriver& output_switch_driver
    )
        : radar_driver_(radar_driver),
          input_switch_driver_(input_switch_driver),
          output_switch_driver_(output_switch_driver) {}

    ~DriverLifecycleGuard() noexcept {
        if (!active_) {
            return;
        }

        // Shutdown should never hide the primary error path.
        try {
            output_switch_driver_.close();
        } catch (...) {
        }
        try {
            input_switch_driver_.close();
        } catch (...) {
        }
        try {
            radar_driver_.close();
        } catch (...) {
        }
    }

    void open_all() {
        radar_driver_.open();
        input_switch_driver_.open();
        output_switch_driver_.open();
        active_ = true;
    }

    void close_all() {
        if (!active_) {
            return;
        }
        output_switch_driver_.close();
        input_switch_driver_.close();
        radar_driver_.close();
        active_ = false;
    }

  private:
    drivers::RadarDriver& radar_driver_;
    drivers::SwitchDriver& input_switch_driver_;
    drivers::SwitchDriver& output_switch_driver_;
    bool active_ = false;
};

void publish_collection(ipc::ShmRing& raw_ring, ipc::ShmRing* raw_tap_ring, const ipc::RawSweepCollection& collection) {
    const auto serialized_collection = ipc::serialize_raw_collection(collection);
    if (!raw_ring.push(serialized_collection)) {
        throw std::runtime_error("Raw ring slot is too small for serialized collection");
    }
    if (raw_tap_ring != nullptr && !raw_tap_ring->push(serialized_collection)) {
        throw std::runtime_error("Raw tap ring slot is too small for serialized collection");
    }
}

}  // namespace

SweepOrchestrator::SweepOrchestrator(
    const config::RunConfig& config,
    drivers::RadarDriver& radar_driver,
    drivers::SwitchDriver& input_switch_driver,
    drivers::SwitchDriver& output_switch_driver,
    ipc::ShmRing& raw_ring,
    ipc::ShmRing* raw_tap_ring
)
    : config_(config),
      radar_driver_(radar_driver),
      input_switch_driver_(input_switch_driver),
      output_switch_driver_(output_switch_driver),
      raw_ring_(raw_ring),
      raw_tap_ring_(raw_tap_ring) {}

void SweepOrchestrator::run(const std::atomic<bool>& stop_requested) {
    DriverLifecycleGuard lifecycle_guard(radar_driver_, input_switch_driver_, output_switch_driver_);
    // Open devices once and keep them active for the whole acquisition loop.
    lifecycle_guard.open_all();

    std::uint64_t collection_id = 0;
    while (!should_stop(stop_requested)) {
        auto raw_collection = acquire_one_collection(++collection_id, stop_requested);
        if (raw_collection.traces.empty()) {
            if (!config_.runtime.continuous) {
                break;
            }
            sleep_if_needed_ms(config_.runtime.idle_sleep_ms);
            continue;
        }

        publish_collection(raw_ring_, raw_tap_ring_, raw_collection);

        if (!config_.runtime.continuous) {
            break;
        }
    }

    lifecycle_guard.close_all();
}

auto SweepOrchestrator::acquire_one_collection(
    std::uint64_t collection_id,
    const std::atomic<bool>& stop_requested
) -> ipc::RawSweepCollection {
    ipc::RawSweepCollection collection{};
    collection.collection_id = collection_id;
    collection.monotonic_ns = ipc::current_monotonic_ns();
    collection.traces.reserve(config_.run_combos.size());
    bool interrupted = false;

    for (const auto& combo : config_.run_combos) {
        if (should_stop(stop_requested)) {
            interrupted = true;
            break;
        }

        // Hardware flow for each combo:
        // 1) switch output path, 2) switch input path, 3) optional settle.
        output_switch_driver_.switch_to(combo.output_pos);
        input_switch_driver_.switch_to(combo.input_pos);
        sleep_if_needed_ms(config_.runtime.settling_ms);

        auto sweep = radar_driver_.acquire_sweep();
        validate_sweep(sweep);

        ipc::SweepTraceBlock trace{};
        trace.combo = combo;
        trace.frequency_hz = std::move(sweep.frequency_hz);
        trace.s11 = std::move(sweep.s11);
        trace.s21 = std::move(sweep.s21);
        collection.traces.push_back(std::move(trace));
    }

    if (interrupted) {
        // Do not emit partial collections when stop was requested mid-cycle.
        collection.traces.clear();
    }

    return collection;
}

}  // namespace radar::acq
