#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <linux/fb.h>
#include <linux/videodev2.h>
#include <linux/ivtvfb.h>

#define W 720
#define H 576

static unsigned char frame[720*576*3/2];
static unsigned char framey[W*H];
static unsigned char frameu[W*H / 4];
static unsigned char framev[W*H / 4];
static unsigned int osd[720*576];

static void de_macro_y(unsigned char* dst, unsigned char *src, int dstride, int w, int h)
{
    unsigned int y, x, i;

    // descramble Y plane
    // dstride = 720 = w
    // The Y plane is divided into blocks of 16x16 pixels
    // Each block in transmitted in turn, line-by-line.
    for (y = 0; y < h; y += 16) {
	for (x = 0; x < w; x += 16) {
	    for (i = 0; i < 16; i++) {
		memcpy(dst + x + (y + i) * dstride, src, 16);
		src += 16;
	    }
	}
    }
}

static void de_macro_uv(unsigned char *dstu, unsigned char *dstv, unsigned char *src, int dstride, int w, int h)
{
    unsigned int y, x, i;

    // descramble U/V plane
    // dstride = 720 / 2 = w
    // The U/V values are interlaced (UVUV...).
    // Again, the UV plane is divided into blocks of 16x16 UV values.
    // Each block in transmitted in turn, line-by-line.
    for (y = 0; y < h; y += 16) {
	for (x = 0; x < w; x += 8) {
	    for (i = 0; i < 16; i++) {
		int idx = x + (y + i) * dstride;

		dstu[idx+0] = src[0];  dstv[idx+0] = src[1];
		dstu[idx+1] = src[2];  dstv[idx+1] = src[3];
		dstu[idx+2] = src[4];  dstv[idx+2] = src[5];
		dstu[idx+3] = src[6];  dstv[idx+3] = src[7];
		dstu[idx+4] = src[8];  dstv[idx+4] = src[9];
		dstu[idx+5] = src[10]; dstv[idx+5] = src[11];
		dstu[idx+6] = src[12]; dstv[idx+6] = src[13];
		dstu[idx+7] = src[14]; dstv[idx+7] = src[15];
		src += 16;
	    }
	}
    }
}

/*************************************************************************/
int main(int argc, char **argv)
{
	char *device = "/dev/fb0";
	FILE *fin;
	int fd;
	int x, y;
	struct ivtvfb_dma_frame df;
	struct fb_var_screeninfo vi;

	if (argc > 1) device = argv[1];
	fin = stdin;

	fd = open(device, O_RDWR);

	if (fd == -1) {
		fprintf(stderr, "cannot open %s\n", device);
		exit(-1);
	}

	ioctl(fd, FBIOGET_VSCREENINFO, &vi);
	vi.nonstd = 1;
	vi.bits_per_pixel = 32;
	vi.xres = W;
	vi.yres = H;
	vi.xres_virtual = W;
	vi.yres_virtual = H;
	vi.xoffset = 0;
	vi.yoffset = 0;
	ioctl(fd, FBIOPUT_VSCREENINFO, &vi);

	while (fread(frame, 1, sizeof(frame), fin)) {
		de_macro_y(framey, frame, W, W, H);
		de_macro_uv(frameu, framev, frame + 720 * H, W / 2, W / 2, H / 2);
		for (y = 0; y < H; y++) {
			for (x = 0; x < W; x++) {
				int y2 = y*2;
				int x2 = x * 2;
				if (x < W / 2 && y < H / 2)
					osd[y * W + x] =
					(framey[y2 * W + x2] << 16) |
					(frameu[(y2/2) * (W/2) + x2/2] << 8) |
					(framev[(y2/2) * (W/2) + x2/2]) |
					0x8f000000;
				else
					osd[y * W + x] = 0x00000000;

			}
		}
		df.source = osd;
		df.dest_offset = 0;
		df.count = sizeof(osd);
		ioctl(fd, IVTVFB_IOC_DMA_FRAME, &df);
	}
	fclose(fin);
	return 0;
}