/***********************************************************************
**
**  v4l2_cap.c
**
**  Copyright (C) 2024 Herbert Poetzl
**
**  gcc -Wall -O3 -march=armv8-a+crc -o v4l2_cap v4l2_cap.c
**
**  This program is free software; you can redistribute it and/or modify
**  it under the terms of the GNU General Public License version 2 as
**  published by the Free Software Foundation.
**
***********************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/poll.h>

#include <linux/videodev2.h>
#include <arm_neon.h>
#include <arm_acle.h>

#define FORMAT	V4L2_PIX_FMT_RGB24

#define NR_BUF	4

static void errno_exit(const char *s)
{
    fprintf(stderr, "%s error %d, %s\n",
	s, errno, strerror(errno));
    exit(EXIT_FAILURE);
}

uint32_t crc32(const uint8_t *data, size_t len)
{
    uint32_t crc = 0;

    len >>= 3;

    while (len--)
	crc = __crc32d(crc, *(uint64_t *)data++);

    return crc;
}

uint64_t xor64(const uint8_t *data, size_t len)
{
    uint64x2_t vzero = (uint64x2_t){0, 0};
    register uint64x2_t v0, v1, v2, v3;

    v0 = v1 = v2 = v3 = vzero;
    len >>= 5;

    while (len--) {
	v0 = vld1q_u64((uint64_t *)(data));
	v2 = veorq_u64(v0, v2);
	v1 = vld1q_u64((uint64_t *)(data + 16));
	v3 = veorq_u64(v1, v3);

	data += 32;
    }

    uint64_t result = 0;

    result ^= vgetq_lane_u64(v2, 0);
    result ^= vgetq_lane_u64(v2, 1);
    result ^= vgetq_lane_u64(v3, 0);
    result ^= vgetq_lane_u64(v3, 1);

    return result;
}

uint64_t fletcher4(const uint8_t *data, size_t len)
{
    uint32x4_t vzero = (uint32x4_t){0, 0, 0, 0};
    register uint32x4_t v0, v1, v2, v3, v4;

    v0 = v1 = v2 = v3 = v4 = vzero;
    len >>= 4;

    while (len--) {
	v0 = vld1q_u32((uint32_t *)(data));
	v1 = vaddq_u32(v1, v0);
	v2 = vaddq_u32(v2, v1);
	v3 = vaddq_u32(v3, v2);
	v4 = vaddq_u32(v4, v3);

	data += 16;
    }

    uint32_t a, b, c, d;

    a = 1*vgetq_lane_u32(v1, 0) +
	1*vgetq_lane_u32(v1, 1) +
	1*vgetq_lane_u32(v1, 2) +
	1*vgetq_lane_u32(v1, 3);

    b = 4*vgetq_lane_u32(v2, 0) +
	4*vgetq_lane_u32(v2, 1) +
	4*vgetq_lane_u32(v2, 2) +
	4*vgetq_lane_u32(v2, 3) -
	1*vgetq_lane_u32(v1, 1) -
	2*vgetq_lane_u32(v1, 2) -
	3*vgetq_lane_u32(v1, 3);

    c = 16*vgetq_lane_u32(v3, 0) +
	16*vgetq_lane_u32(v3, 1) +
	16*vgetq_lane_u32(v3, 2) +
	16*vgetq_lane_u32(v3, 3) -
        6*vgetq_lane_u32(v2, 0) -
	10*vgetq_lane_u32(v2, 1) -
	14*vgetq_lane_u32(v2, 2) -
	18*vgetq_lane_u32(v2, 3) +
	1*vgetq_lane_u32(v1, 2) +
	3*vgetq_lane_u32(v1, 3);

    d = 64*vgetq_lane_u32(v4, 0) +
	64*vgetq_lane_u32(v4, 1) +
	64*vgetq_lane_u32(v4, 2) +
	64*vgetq_lane_u32(v4, 3) -
        48*vgetq_lane_u32(v3, 0) -
	64*vgetq_lane_u32(v3, 1) -
	80*vgetq_lane_u32(v3, 2) -
	96*vgetq_lane_u32(v3, 3) +
        4*vgetq_lane_u32(v2, 0) +
	10*vgetq_lane_u32(v2, 1) +
	20*vgetq_lane_u32(v2, 2) +
	34*vgetq_lane_u32(v2, 3) -
	1*vgetq_lane_u32(v1, 3);

    uint64_t result = 0;

    result = a ^ c;
    result <<= 32;
    result ^= b ^ d;

    return result;
}

static inline
uint32_t marker(uint8_t *data)
{
    uint32_t mark = *data;

    *data++ = 0;
    mark = (mark << 8) | *data;
    *data++ = 0;
    mark = (mark << 8) | *data;
    *data = 0;

    return mark;
}

int main(int argc, char* argv[])
{
    const char *dev_path = argv[1];

    unsigned frames = 60;
    unsigned width = 1920;
    unsigned height = 1080;
    char *endptr;
    
    if (argc > 2)
        frames = strtoull(argv[2], &endptr, 10);

    if (argc > 3)
        width = strtoull(argv[3], &endptr, 10);

    if (argc > 4)
        height = strtoull(argv[4], &endptr, 10);
      
    size_t frame_size = width*height*3;

    int fd;
    struct v4l2_capability cap;
    struct v4l2_format fmt = { 0 };
    struct v4l2_requestbuffers req = { 0 };

    struct buffer {
	void *start;
	size_t length;
    };
    struct buffer buffers[NR_BUF];
    
    // Open the video device
    fd = open(dev_path, O_RDWR);
    if (fd == -1)
        errno_exit("open");
    
    // Query device capabilities
    if (ioctl(fd, VIDIOC_QUERYCAP, &cap) == -1)
        errno_exit("VIDIOC_QUERYCAP");
    
    // Check if device supports capturing
    if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
        fprintf(stderr,
	    "%s is not a video capture device\n",
	    dev_path);
        exit(EXIT_FAILURE);
    }
    
    // Set the video format
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.width = width;
    fmt.fmt.pix.height = height;
    fmt.fmt.pix.pixelformat = FORMAT;
    fmt.fmt.pix.field = V4L2_FIELD_ANY;
    
    if (ioctl(fd, VIDIOC_S_FMT, &fmt) == -1)
        errno_exit("VIDIOC_S_FMT");
    
    // Request buffers for capturing
    req.count = NR_BUF;
    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory = V4L2_MEMORY_MMAP;
    
    if (ioctl(fd, VIDIOC_REQBUFS, &req) == -1)
        errno_exit("VIDIOC_REQBUFS");

    // get buffer mappings
    for (int n = 0; n < req.count; n++) {
	struct v4l2_buffer buf = { 0 };

	buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	buf.memory = V4L2_MEMORY_MMAP;
	buf.index = n;

	if (-1 == ioctl(fd, VIDIOC_QUERYBUF, &buf))
	    errno_exit("VIDIOC_QUERYBUF");

	buffers[n].length = buf.length;
	buffers[n].start = mmap(
		NULL /* start anywhere */,
	        buf.length,
	        PROT_READ | PROT_WRITE /* required */,
	        MAP_SHARED /* recommended */,
	        fd, buf.m.offset);

	if (MAP_FAILED == buffers[n].start)
		errno_exit("mmap");
    }

    // schedule buffers
    for (int n = 0; n < req.count; n++) {
	struct v4l2_buffer buf = { 0 };

	buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	buf.memory = V4L2_MEMORY_MMAP;
	buf.index = n;

	if (-1 == ioctl(fd, VIDIOC_QBUF, &buf))
		errno_exit("VIDIOC_QBUF");

	fprintf(stderr, 
	    "queued buffer [%d]\n", n);
    }

    fprintf(stderr, 
        "expected frame size [%lu] [%dx%d]\n",
	frame_size, width, height);

    enum v4l2_buf_type type;
    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    if (-1 == ioctl(fd, VIDIOC_STREAMON, &type))
	errno_exit("VIDIOC_STREAMON");

    uint32_t hoff = 3*(width - 1);
    uint32_t voff = 3*width*(height - 1);
    struct pollfd fds;

    fds.fd = fd;
    fds.events = POLLIN;
    
    // Loop for capturing frames (replace with your processing logic)
    for (int i = 0; i < frames; i++) {
	int ret = poll(&fds, 1, -1);
	if (ret == -1)
	    errno_exit("poll");

        struct v4l2_buffer buf = { 0 };

	buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;

        // Dequeue the captured buffer
        if (ioctl(fd, VIDIOC_DQBUF, &buf) == -1)
            errno_exit("VIDIOC_DQBUF");

	uint8_t *data = buffers[buf.index].start;

	write(1, data, buf.bytesused);

	uint32_t mark[4] = {
	    marker(&data[0]),
	    marker(&data[hoff]),
	    marker(&data[voff]),
	    marker(&data[hoff+voff]) };

	// uint32_t crc crc32(data, buf.bytesused);
	// uint64_t xor xor64(data, buf.bytesused);
	uint64_t fl4 = fletcher4(data, buf.bytesused);
        
        // Process the captured frame (data is in `buffer` at offset `buf.offset`)
        fprintf(stderr,
	    "%06X %016lX %06X %06X %06X %06X # [%d] [%d]\n",
	    i, fl4, mark[0], mark[1], mark[2], mark[3],
	    buf.bytesused, buf.index);

        if (ioctl(fd, VIDIOC_QBUF, &buf) == -1)
            errno_exit("VIDIOC_QBUF");
    }
    
    // Unmap memory and close device
    // munmap(buffer, req.count * fmt.fmt.pix.sizeimage);
    close(fd);
    
    return 0;
}