#include <algorithm>
#include <chrono>
#include <csignal>
#include <iomanip>
#include <iostream>
#include <memory>
#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;
    cfg.slave_request_command = {
        0x55, 0xAA, 0x09, 0x00, 0x34, 0x00, 0xFB,
        0x00, 0x1C, 0x00, 0x00, 0x18, 0x00, 0x7A
    };
    cfg.slave_request_interval = 200ms;

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

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

    auto print_hex = [](const std::vector<std::uint8_t>& data) {
        if (data.empty()) {
            std::cout << "(empty)";
            return;
        }
        const auto old_flags = std::cout.flags();
        const auto old_fill = std::cout.fill();
        std::cout << std::hex << std::uppercase;
        for (std::size_t idx = 0; idx < data.size(); ++idx) {
            if (idx != 0U) {
                std::cout << ' ';
            }
            std::cout << std::setw(2) << std::setfill('0')
                      << static_cast<int>(data[idx]);
        }
        std::cout.flags(old_flags);
        std::cout.fill(old_fill);
    };

    std::cout << "Streaming from " << port << ". Press Ctrl+C to stop...\n";
    if (!cfg.slave_request_command.empty()) {
        std::cout << "Slave mode active; request interval "
                  << cfg.slave_request_interval.count() << " ms.\n";
    }

    // Also demonstrate polling API (in case users don't want callbacks)
    while (g_running) {
        std::shared_ptr<ffmsep::DecodedFrame> df;
        if (core.wait_for_frame(df, 200ms) && df) {
            std::cout << "Frame pts=" << df->pts
                      << " bytes=" << df->frame.data.size();
            if (df->tactile) {
                const auto& tf = *df->tactile;
                std::cout << " addr=" << int(tf.device_address)
                          << " func=0x" << std::hex << std::uppercase << int(tf.response_function)
                          << std::dec;
                if (df->tactile_matrix_size) {
                    const auto& ms = *df->tactile_matrix_size;
                    std::cout << " matrix=" << int(ms.long_edge)
                              << "x" << int(ms.short_edge);
                }
                if (!df->tactile_pressures.empty()) {
                    std::cout << " pressures=" << df->tactile_pressures.size()
                              << " values=[";
                    const std::size_t preview = std::min<std::size_t>(df->tactile_pressures.size(), 8);
                    for (std::size_t idx = 0; idx < preview; ++idx) {
                        if (idx != 0U) {
                            std::cout << ", ";
                        }
                        std::cout << df->tactile_pressures[idx];
                    }
                    if (preview < df->tactile_pressures.size()) {
                        std::cout << ", ...";
                    }
                    std::cout << "]";
                }
                std::cout << "\n    raw=";
                print_hex(df->frame.data);
            } else {
                std::cout << " raw=";
                print_hex(df->frame.data);
            }
            std::cout << "\n";
        }
    }

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