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

#include <EGL/egl.h>
#include <GLES2/gl2.h>
#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();
}

// Pi libcamera RGB888 is BGR in memory; BGR888 gives true RGB for ffplay rgb24.
static void packRgb24(void *src, unsigned char *dst, int w, int h, int stride)
{
    auto *p = static_cast<unsigned char *>(src);
    for (int y = 0; y < h; ++y)
        memcpy(dst + y * w * 3, p + y * stride, w * 3);
}

static void uploadFrame(GLuint tex, const unsigned char *rgb, int w, int h)
{
    glBindTexture(GL_TEXTURE_2D, tex);
    glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, w, h, GL_RGB, GL_UNSIGNED_BYTE, rgb);
}

static GLuint makeProgram()
{
    const char *vs =
        "attribute vec2 pos; attribute vec2 tex; varying vec2 uv;"
        "void main(){uv=tex; gl_Position=vec4(pos,0.0,1.0);}";
    // Luminance diff avoids false motion from AWB/color shifts (common GLES motion trick).
    const char *fs =
        "precision mediump float; varying vec2 uv;"
        "uniform sampler2D prevFrame, currFrame;"
        "const vec3 lum = vec3(0.299, 0.587, 0.114);"
        "void main(){"
        "  float a = dot(texture2D(prevFrame, uv).rgb, lum);"
        "  float b = dot(texture2D(currFrame, uv).rgb, lum);"
        "  float d = clamp(abs(b - a) * 4.0, 0.0, 1.0);"
        "  gl_FragColor = vec4(vec3(d), 1.0);"
        "}";

    auto compile = [](GLenum type, const char *src) {
        GLuint s = glCreateShader(type);
        glShaderSource(s, 1, &src, nullptr);
        glCompileShader(s);
        return s;
    };
    GLuint p = glCreateProgram();
    glAttachShader(p, compile(GL_VERTEX_SHADER, vs));
    glAttachShader(p, compile(GL_FRAGMENT_SHADER, fs));
    glLinkProgram(p);
    return p;
}

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;
    std::cerr << "mode=" << (mode == Mode::Raw ? "raw" : "motion")
              << " " << w << "x" << h << " (half of " << full.toString() << ")"
              << " stride=" << stride << "\n";

    std::vector<unsigned char> rgbFrame(w * h * 3), flipRow(w * 3);

    GLuint program = 0, prevTex = 0, currTex = 0, fbo = 0, vbo = 0;
    GLint uPrev = -1, uCurr = -1, aPos = -1, aTex = -1;
    bool havePrev = false;
    EGLDisplay dpy = EGL_NO_DISPLAY;
    EGLSurface surface = EGL_NO_SURFACE;
    EGLContext ctx = EGL_NO_CONTEXT;

    if (mode == Mode::Motion) {
        dpy = eglGetDisplay(EGL_DEFAULT_DISPLAY);
        eglInitialize(dpy, nullptr, nullptr);
        EGLint cfgAttr[] = { EGL_SURFACE_TYPE, EGL_PBUFFER_BIT, EGL_RENDERABLE_TYPE, EGL_OPENGL_ES2_BIT,
                             EGL_RED_SIZE, 8, EGL_GREEN_SIZE, 8, EGL_BLUE_SIZE, 8, EGL_NONE };
        EGLConfig cfg;
        EGLint n;
        eglChooseConfig(dpy, cfgAttr, &cfg, 1, &n);
        EGLint pb[] = { EGL_WIDTH, w, EGL_HEIGHT, h, EGL_NONE };
        surface = eglCreatePbufferSurface(dpy, cfg, pb);
        EGLint ctxAttr[] = { EGL_CONTEXT_CLIENT_VERSION, 2, EGL_NONE };
        ctx = eglCreateContext(dpy, cfg, EGL_NO_CONTEXT, ctxAttr);
        eglMakeCurrent(dpy, surface, surface, ctx);

        auto makeTex = [&](int tw, int th) {
            GLuint t;
            glGenTextures(1, &t);
            glBindTexture(GL_TEXTURE_2D, t);
            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
            glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, tw, th, 0, GL_RGB, GL_UNSIGNED_BYTE, nullptr);
            return t;
        };
        program = makeProgram();
        prevTex = makeTex(w, h);
        currTex = makeTex(w, h);
        GLuint outTex;
        glGenTextures(1, &outTex);
        glBindTexture(GL_TEXTURE_2D, outTex);
        glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, w, h, 0, GL_RGBA, GL_UNSIGNED_BYTE, nullptr);
        glGenFramebuffers(1, &fbo);
        glBindFramebuffer(GL_FRAMEBUFFER, fbo);
        glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, outTex, 0);

        float quad[] = { -1,-1, 0,1,  1,-1, 1,1,  -1,1, 0,0,  1,1, 1,0 };
        glGenBuffers(1, &vbo);
        glBindBuffer(GL_ARRAY_BUFFER, vbo);
        glBufferData(GL_ARRAY_BUFFER, sizeof(quad), quad, GL_STATIC_DRAW);

        uPrev = glGetUniformLocation(program, "prevFrame");
        uCurr = glGetUniformLocation(program, "currFrame");
        aPos = glGetAttribLocation(program, "pos");
        aTex = glGetAttribLocation(program, "tex");
    }

    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());

    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;
            request = done.front();
            done.pop();
        }

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

            if (mode == Mode::Raw) {
                packRgb24(mem, rgbFrame.data(), w, h, stride);
                if (!sendAll(rgbFrame.data(), rgbFrame.size()))
                    capturing = false;
            } else {
                packRgb24(mem, rgbFrame.data(), w, h, stride);
                uploadFrame(currTex, rgbFrame.data(), w, h);
                if (!havePrev) {
                    uploadFrame(prevTex, rgbFrame.data(), w, h);
                    havePrev = true;
                } else {
                    glViewport(0, 0, w, h);
                    glUseProgram(program);
                    glActiveTexture(GL_TEXTURE0);
                    glBindTexture(GL_TEXTURE_2D, prevTex);
                    glUniform1i(uPrev, 0);
                    glActiveTexture(GL_TEXTURE1);
                    glBindTexture(GL_TEXTURE_2D, currTex);
                    glUniform1i(uCurr, 1);

                    glBindBuffer(GL_ARRAY_BUFFER, vbo);
                    glEnableVertexAttribArray(aPos);
                    glEnableVertexAttribArray(aTex);
                    glVertexAttribPointer(aPos, 2, GL_FLOAT, GL_FALSE, 4 * sizeof(float), (void *)0);
                    glVertexAttribPointer(aTex, 2, GL_FLOAT, GL_FALSE, 4 * sizeof(float), (void *)(2 * sizeof(float)));
                    glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);

                    glReadPixels(0, 0, w, h, GL_RGB, GL_UNSIGNED_BYTE, rgbFrame.data());
                    for (int y = 0; y < h / 2; ++y) {
                        unsigned char *top = rgbFrame.data() + y * w * 3;
                        unsigned char *bot = rgbFrame.data() + (h - 1 - y) * w * 3;
                        memcpy(flipRow.data(), top, w * 3);
                        memcpy(top, bot, w * 3);
                        memcpy(bot, flipRow.data(), w * 3);
                    }
                    if (!sendAll(rgbFrame.data(), rgbFrame.size()))
                        capturing = false;
                    std::swap(prevTex, currTex);
                }
            }
        }

        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;
    if (mode == Mode::Motion) {
        eglDestroyContext(dpy, ctx);
        eglDestroySurface(dpy, surface);
        eglTerminate(dpy);
    }
    camera->release();
    cm->stop();
    return 0;
}