#include <algorithm>
#include <chrono>
#include <csignal>
#include <iostream>
#include <optional>
#include <string>
#include <thread>
#include <vector>

#include "components/ffmsep/cpstream_core.hh"
#include "components/ffmsep/tactile/tacdec.hh"

using namespace std::chrono_literals;

static volatile std::sig_atomic_t g_running = 1;

void handle_sigint(int) {
    g_running = 0;
}

int main(int argc, char** argv) {
    std::signal(SIGINT, handle_sigint);

    // Register tactile codec so the stream can resolve it by ID or name.
    ffmsep::tactile::register_tactile_codec();

    const auto ports = ffmsep::CPStreamCore::list_available_ports();
    const auto describe_ports = [&ports]() {
        if (ports.empty()) {
            std::cout << "No serial ports detected.\n";
            return;
        }
        std::cout << "Available serial ports:\n";
        for (const auto& p : ports) {
            std::cout << " - " << p.port.c_str() << " (" << p.description.c_str() << ")\n";
        }
    };

    std::string port;
    if (argc >= 2) {
        const std::string requested = argv[1];
        if (ports.empty()) {
            std::cout << "No serial ports detected, attempting requested '" << requested << "'.\n";
            port = requested;
        } else {
            const auto it = std::find_if(ports.begin(), ports.end(),
                [&](const auto& info) { return info.port == requested; });
            if (it == ports.end()) {
                std::cerr << "Requested port '" << requested << "' not found among detected ports.\n";
                describe_ports();
                return 1;
            }
            port = it->port;
        }
    } else {
        describe_ports();
        if (!ports.empty()) {
            port = ports.front().port;
            std::cout << "Auto-selecting: " << port << "\n";
        } else {
            std::cerr << "No serial ports found. Exiting.\n";
            return 1;
        }
    }

    ffmsep::CPStreamConfig cfg;
    cfg.port = port;
    cfg.baudrate = 115200;
    cfg.codec_id = ffmsep::CPCodecID::Tactile; // resolve tactile decoder
    cfg.read_chunk_size = 256;
    cfg.packet_queue_capacity = 128;
    cfg.frame_queue_capacity = 32;

    ffmsep::CPStreamCore core(cfg);
    if (!core.open()) {
        std::cerr << "Open failed: " << core.last_error() << "\n";
        return 2;
    }

    // Optional: receive frames via callback
    core.set_frame_callback([](const ffmsep::DecodedFrame& df) {
        const auto maybe = ffmsep::tactile::parse_frame(df.frame);
        if (!maybe) {
            return;
        }
        const auto& tf = *maybe;
        const auto pv = ffmsep::tactile::parse_pressure_values(tf);
        std::cout << "Frame pts=" << df.pts
                  << " addr=" << int(tf.device_address)
                  << " func=" << int(static_cast<std::uint8_t>(tf.function))
                  << " len=" << int(tf.data_length)
                  << " pressures=" << pv.size() << "\n";
    });

    if (!core.start()) {
        std::cerr << "Start failed: " << core.last_error() << "\n";
        return 3;
    }

    std::cout << "Streaming from " << port << ". Press Ctrl+C to stop...\n";

    // Also demonstrate polling API (in case users don't want callbacks)
    while (g_running) {
        ffmsep::DecodedFrame df;
        if (core.wait_for_frame(df, 200ms)) {
            const auto maybe = ffmsep::tactile::parse_frame(df.frame);
            if (maybe) {
                const auto sz = ffmsep::tactile::parse_matrix_size_payload(*maybe);
                if (sz) {
                    std::cout << " matrix=" << int(sz->long_edge) << "x" << int(sz->short_edge) << "\n";
                }
            }
        }
    }

    core.stop();
    core.close();
    std::cout << "Stopped.\n";
    return 0;
}