#include <atomic>
#include <chrono>
#include <condition_variable>
#include <csignal>
#include <algorithm>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <memory>
#include <mutex>
#include <queue>
#include <unordered_map>
#include <vector>

#include <libcamera/libcamera.h>
#include <libcamera/formats.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <sys/mman.h>
#include <sys/socket.h>
#include <unistd.h>

using namespace libcamera;

static std::atomic<bool> capturing{true};
static std::shared_ptr<Camera> camera;
static std::mutex frameMutex;
static std::condition_variable frameCv;
static std::queue<Request *> done;
static int clientFd = -1;

struct MappedBuffer { void *mem; size_t len; };
static std::unordered_map<FrameBuffer *, MappedBuffer> mapped;

enum class Mode { Motion, Raw };

static Mode parseMode(int argc, char **argv)
{
    if (argc < 2)
        return Mode::Motion;
    if (!strcmp(argv[1], "--raw") || !strcmp(argv[1], "raw"))
        return Mode::Raw;
    if (!strcmp(argv[1], "--motion") || !strcmp(argv[1], "motion"))
        return Mode::Motion;
    std::cerr << "Usage: " << argv[0] << " [--motion|--raw]\n";
    exit(1);
}

static int waitForClient(int port, const char *label)
{
    int server = socket(AF_INET, SOCK_STREAM, 0);
    int yes = 1;
    setsockopt(server, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(yes));
    sockaddr_in addr{};
    addr.sin_family = AF_INET;
    addr.sin_addr.s_addr = INADDR_ANY;
    addr.sin_port = htons(port);
    bind(server, reinterpret_cast<sockaddr *>(&addr), sizeof(addr));
    listen(server, 1);
    std::cerr << "tcp://0.0.0.0:" << port << " waiting...\n";
    int client = accept(server, nullptr, nullptr);
    close(server);
    setsockopt(client, IPPROTO_TCP, TCP_NODELAY, &yes, sizeof(yes));
    std::cerr << "streaming " << label << "\n";
    return client;
}

static bool sendAll(const unsigned char *data, size_t size)
{
    while (size && capturing) {
        ssize_t n = send(clientFd, data, size, MSG_NOSIGNAL);
        if (n <= 0)
            return false;
        data += n;
        size -= n;
    }
    return true;
}

static void requestComplete(Request *request)
{
    if (request->status() == Request::RequestCancelled)
        return;
    std::lock_guard<std::mutex> lock(frameMutex);
    done.push(request);
    frameCv.notify_one();
}

// Tight RGB24 row copy (Pi: BGR888 libcamera format = RGB bytes in memory).
static void packRgb24(const unsigned char *src, unsigned char *dst, int w, int h, int stride)
{
    for (int y = 0; y < h; ++y)
        memcpy(dst + y * w * 3, src + y * stride, w * 3);
}

// Same math as the GLES shader: luminance diff, brighter = more motion.
static void motionRgb24(const unsigned char *prev, const unsigned char *cur, unsigned char *out, int pixels)
{
    for (int i = 0; i < pixels; ++i) {
        int o = i * 3;
        int a = (prev[o] * 77 + prev[o + 1] * 150 + prev[o + 2] * 29) >> 8;
        int b = (cur[o] * 77 + cur[o + 1] * 150 + cur[o + 2] * 29) >> 8;
        unsigned char d = static_cast<unsigned char>(std::min(255, abs(b - a) * 10));
        out[o] = out[o + 1] = out[o + 2] = d;
    }
}

int main(int argc, char **argv)
{
    Mode mode = parseMode(argc, argv);
    std::signal(SIGINT, [](int) { capturing = false; });

    auto cm = std::make_unique<CameraManager>();
    cm->start();
    if (cm->cameras().empty()) {
        std::cerr << "no camera\n";
        return 1;
    }

    camera = cm->get(cm->cameras()[0]->id());
    camera->acquire();

    auto config = camera->generateConfiguration({ StreamRole::Viewfinder });
    StreamConfiguration &sc = config->at(0);
    Size full = sc.size;
    sc.size.width = (full.width / 2) & ~1u;
    sc.size.height = (full.height / 2) & ~1u;
    sc.pixelFormat = formats::BGR888;
    sc.colorSpace = ColorSpace::Sycc;
    sc.bufferCount = 4;
    if (config->validate() == CameraConfiguration::Invalid || sc.pixelFormat != formats::BGR888) {
        std::cerr << "BGR888 config failed\n";
        return 1;
    }
    camera->configure(config.get());

    const int w = sc.size.width, h = sc.size.height, stride = sc.stride;
    const int pixels = w * h;
    const size_t frameBytes = pixels * 3;
    std::cerr << "mode=" << (mode == Mode::Raw ? "raw" : "motion")
              << " " << w << "x" << h << " (half of " << full.toString() << ")"
              << " stride=" << stride << "\n";

    std::vector<unsigned char> cur(frameBytes), prev(frameBytes), out(frameBytes);

    clientFd = waitForClient(5000, mode == Mode::Raw ? "raw rgb24" : "motion rgb24");

    FrameBufferAllocator *allocator = new FrameBufferAllocator(camera);
    Stream *stream = sc.stream();
    if (allocator->allocate(stream) < 0) {
        std::cerr << "buffer alloc failed\n";
        return 1;
    }

    for (const auto &buffer : allocator->buffers(stream)) {
        const auto &plane = buffer->planes()[0];
        mapped[buffer.get()] = {
            mmap(nullptr, plane.length, PROT_READ, MAP_SHARED, plane.fd.get(), plane.offset),
            plane.length
        };
    }

    std::vector<std::unique_ptr<Request>> requests;
    for (const auto &buffer : allocator->buffers(stream)) {
        auto request = camera->createRequest();
        if (!request || request->addBuffer(stream, buffer.get()) < 0)
            return 1;
        requests.push_back(std::move(request));
    }

    camera->requestCompleted.connect(requestComplete);
    camera->start();
    for (auto &r : requests)
        camera->queueRequest(r.get());

    bool havePrev = false;
    unsigned long frames = 0;

    while (capturing) {
        Request *request = nullptr;
        {
            std::unique_lock<std::mutex> lock(frameMutex);
            frameCv.wait_for(lock, std::chrono::milliseconds(50), [] { return !done.empty(); });
            if (done.empty())
                continue;

            // Drop stale frames: only process the newest completed request.
            while (done.size() > 1) {
                Request *stale = done.front();
                done.pop();
                lock.unlock();
                stale->reuse(Request::ReuseBuffers);
                camera->queueRequest(stale);
                lock.lock();
            }
            request = done.front();
            done.pop();
        }

        FrameBuffer *buffer = request->buffers().begin()->second;
        if (buffer->metadata().status == FrameMetadata::FrameSuccess) {
            auto *mem = static_cast<unsigned char *>(mapped[buffer].mem);

            if (mode == Mode::Raw) {
                if (stride == w * 3) {
                    if (!sendAll(mem, frameBytes))
                        capturing = false;
                } else {
                    packRgb24(mem, cur.data(), w, h, stride);
                    if (!sendAll(cur.data(), frameBytes))
                        capturing = false;
                }
            } else {
                packRgb24(mem, cur.data(), w, h, stride);
                if (havePrev) {
                    motionRgb24(prev.data(), cur.data(), out.data(), pixels);
                    if (!sendAll(out.data(), frameBytes))
                        capturing = false;
                }
                prev.swap(cur);
                havePrev = true;
            }

            if (++frames % 60 == 0)
                std::cerr << "frames " << frames << "\n";
        }

        request->reuse(Request::ReuseBuffers);
        camera->queueRequest(request);
    }

    camera->stop();
    camera->requestCompleted.disconnect();
    if (clientFd >= 0)
        close(clientFd);
    for (auto &[_, m] : mapped)
        munmap(m.mem, m.len);
    allocator->free(stream);
    delete allocator;
    camera->release();
    cm->stop();
    return 0;
}