#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <unistd.h>
#include <sys/mman.h>
#include <fcntl.h>
#include <linux/fb.h>

#ifdef __ARM_NEON
#include <arm_neon.h>
#endif
#include <linux/watchdog.h>

#define WIDTH 256
#define HEIGHT 256

void update_screen(unsigned int *f, unsigned int *c)
{
	int x, y;
	uint32x2_t minpos = { 4096, 4096 }, maxpos = { 0, 0 };
	uint8x8_t table =  { 0, 4, 8, 12, 0, 4, 8, 12 };
	uint32x2_t ypos = { 0, 0 }, xinc = { 16, 0 }, yinc = { 0, 1 };

	for (y = 0; y < HEIGHT; y++) {
		uint32x2_t xpos = ypos;

		for (x = 0; x < WIDTH; x += 16) {
			uint32x4_t f0 = vld1q_u32(f);
			uint32x4_t f1 = vld1q_u32(f+4);
			uint32x4_t f2 = vld1q_u32(f+8);
			uint32x4_t f3 = vld1q_u32(f+12);
			uint32x4_t c0 = vld1q_u32(c);
			uint32x4_t c1 = vld1q_u32(c+4);
			uint32x4_t c2 = vld1q_u32(c+8);
			uint32x4_t c3 = vld1q_u32(c+12);
			uint32x4_t d0 = veorq_u32(f0, c0);
			uint32x4_t d1 = veorq_u32(f1, c1);
			uint32x4_t d2 = veorq_u32(f2, c2);
			uint32x4_t d3 = veorq_u32(f3, c3);
			uint32x4_t d = vtstq_u32(vorrq_u32(d0, d1), vorrq_u32(d2, d3));
			uint32x2_t mask = (uint32x2_t) vtbl2_u8(*((uint8x8x2_t*)&d), table);
			mask = vtst_u32(mask, mask);
			f += 16;
			c += 16;

			minpos = vbsl_u32(vand_u32(mask, vclt_u32(xpos, minpos)), xpos, minpos);
			maxpos = vbsl_u32(vand_u32(mask, vcgt_u32(xpos, maxpos)), xpos, maxpos);
			vst1q_u32(c, f0);
			vst1q_u32(c+4, f1);
			vst1q_u32(c+8, f2);
			vst1q_u32(c+12, f3);
			xpos = vadd_u32(xpos, xinc);
		}
		
		ypos = vadd_u32(ypos, yinc);
	}


	if (minpos[0] <= maxpos[0] && minpos[1] <= maxpos[1]) {
		fprintf(stderr, "Dirty page: %dx%d:%dx%d...\n", minpos[0], minpos[1], maxpos[0], maxpos[1]);
	}
}

int main(int argc, char **argv)
{
	int wfd = open("/dev/watchdog", O_WRONLY);
	int new_timeout = 15;
	int i=0;
	ioctl(wfd, WDIOC_SETTIMEOUT, &new_timeout);

	int fbfd = open(argv[1], O_RDONLY);
	unsigned int *fbmmap = mmap(NULL, WIDTH*HEIGHT*4, PROT_READ, MAP_SHARED, fbfd, 0);
	unsigned int * fbbuf = malloc(WIDTH*HEIGHT*4);

	for(i=0;;i++)
	{
		ioctl(wfd, WDIOC_KEEPALIVE, 0);
		update_screen(fbmmap, fbbuf);
		printf("Still alive %d\n", i);
	}
}