/*
 * overlay.c
 *
 * Copyright (C) 2010-2013 Sensoray Co., Inc.
 */

#include <stdio.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/ioctl.h>
#include <stdlib.h>

#include "s2253.h"
#include "s2253ioctl.h"




void usage() {
	fprintf(stdout,
		"Overlay image control. Copyright (C) 2010-2011 Sensoray Co., Inc.\n"
		"Usage: overlay [options] [filename | lines | text]\n"
		"    -v         Verbose\n"
		"    -i <id>    Tell device to remember this image id (default 0)\n"
		"    -x <x>     X offset from left side of display (default 50)\n"
		"    -y <y>     Y offset from top of display (default 50)\n"
		"    -a <n>     Alpha value: 0=hidden .. 7=opaque 8=image 9=special\n"
		"    -e         Erase the image for this id (same as -a 0)\n"
		"    -q         Quick mode, image is loaded but display is not updated\n"
		"    -l         Draw lines instead of an image in the following format:\n"
		"                 [x1 y1 x2 y2 width argb] ...\n"
		"    -t <argb>  Draw text with argb color\n"
#ifdef ENABLE_FONT
		"    -f <font>  Draw text using specified font file (e.g. FreeSans.ttf)\n"
		"               Note: -a option will now set background alpha\n"
		"    -s <size>  Set font size in points (requires -f)\n"
		"    -b         Enable bold text (requires -f)\n"
		"    -I         Enable italic text (requires -f)\n"
		"    -u         Enable text underline (requires -f)\n"
		"    -o         Enable text outline (requires -f)\n"
		"    -O <alpha> Enable text alpha outline 1..8 (requires -f)\n"
		"    -S         Enable text shadow (require -f)\n"
#endif
		"    -d <dev>   Device name (default /dev/video2)\n"
		"    -c <chan>  Override channel id (0=Display 1=VideoA 2=VideoB 3=VideoA+B)\n"
		"    filename   Path to 32-bit or 24-bit PNG or BMP\n"
		"               When filename is omitted, the existing image id\n"
		"               is changed to new settings\n"
		);
}

static const char short_options[] = "i:x:y:va:t:qd:lec:"
#ifdef ENABLE_FONT
	"f:s:bIuoO:S"
#endif
	"";

static void overlayInit(struct s2253_overlay_image *buf, int ch, int id, int xpos, int ypos, int alpha, int size, int update)
{
	memset(buf, 0, sizeof(struct s2253_overlay_image));
	buf->channel = ch;
	buf->id = id;
	buf->xOffset = xpos;
	buf->yOffset = ypos;
	buf->transparent = alpha;
	buf->length = size;
	buf->update = update;
}

static int overlaySend(int fd, struct s2253_overlay_image *buf)
{
	int rc;
	rc = ioctl(fd, S2253_VIDIOC_OVERLAY, buf);
	if (rc < 0)
		perror("S2253_VIDIOC_OVERLAY");
	return rc;
}

#ifdef ENABLE_FONT
static struct s2253_overlay_image *overlayTextAlloc(int len, unsigned int textcolor)
{
	struct s2253_overlay_image *buf;

	buf = malloc(sizeof(struct s2253_overlay_image) + 8 + len);
	if (!buf) {
		fprintf(stderr, "Failed to allocate buffer\n");
	} else {
		memset(buf, 0, sizeof(struct s2253_overlay_image));
		buf->length = 8 + len;
		memcpy(buf->data, "text", 4);
		buf->data[4] = textcolor;
		buf->data[5] = textcolor >> 8;
		buf->data[6] = textcolor >> 16;
		buf->data[7] = (textcolor >> 24) ^ 0xff; // alpha is inverted
	}
	return buf;
}
#endif




static void *line_render(int argc, char *argv[], int verbose)
{
	int size = 0;
	int lines = 1;
	struct s2253_overlay_image *buf = NULL;
	struct s2253_line *line;

	while (optind < argc) {
		if (optind + 6 > argc) {
			fprintf(stderr, "Insufficient line inputs for line %d\n", lines);
			free(buf);
			return NULL;
		}
		size = 4 + lines * sizeof(*line);
		buf = realloc(buf, sizeof(struct s2253_overlay_image) + size);
		if (!buf) {
			fprintf(stderr, "Failed to allocate buffer\n");
			return NULL;
		}
		line = (struct s2253_line *)(buf->data + 4) + (lines-1);

		line->x1 = atoi(argv[optind++]);
		line->y1 = atoi(argv[optind++]);
		line->x2 = atoi(argv[optind++]);
		line->y2 = atoi(argv[optind++]);
		line->width = atoi(argv[optind++]);
		line->argb = strtoul(argv[optind++], NULL, 16);
		line->argb ^= 0xff000000; // alpha is inverted
		
		if (verbose)
			printf("line len=%d data %d %d %d %d %d %x\n",
				size, line->x1, line->y1, line->x2, line->y2, 
				line->width, line->argb);
		lines++;
	}
	
	memcpy(buf->data, "line", 4);
	buf->length = size;
	return buf;
}

static void *text_render(int argc, char *argv[], unsigned int text)
{
	int size = 8;
	struct s2253_overlay_image *buf = NULL;

	while (optind < argc) {
		int len = strlen(argv[optind]);
		if (size > 8)
			size += 1; // make room for a space
		buf = realloc(buf, sizeof(struct s2253_overlay_image) + size + len);
		if (!buf) {
			fprintf(stderr, "Failed to allocate buffer\n");
			return NULL;
		}
		buf->data[size-1] = ' ';
		memcpy(buf->data + size, argv[optind], len);
		size += len;
		++optind;
	}
	memcpy(buf->data, "text", 4);
	buf->data[4] = text;
	buf->data[5] = text >> 8;
	buf->data[6] = text >> 16;
	buf->data[7] = (text >> 24) ^ 0xff; // alpha is inverted
	buf->length = size;
	return buf;
}





struct RLE_image {
    unsigned char *data;
    int len; // data length
    int w;
    int h;
    int run; // current run length
    int rgb; // current run color
};

#define MAX_RUN (255 + 16)

int encodeRLE_init(struct RLE_image *rle, int w, int h, int rgb)
{
    rle->w = w;
    rle->h = h;
    rle->len = sizeof(struct s2253_overlay_image) + 10;
    rle->data = malloc(rle->len + w * h);
    if (rle->data) {
	memcpy(rle->data + rle->len - 10, "RLE ", 4);
	rle->data[rle->len-6] = w & 0xff;
	rle->data[rle->len-5] = (w >> 8) & 0xff;
	rle->data[rle->len-4] = h & 0xff;
	rle->data[rle->len-3] = (h >> 8) & 0xff;

	// convert to RGB565
	rgb = ((rgb & 0xf80000) >> 8) |
	      ((rgb & 0x00fc00) >> 5) |
	      ((rgb & 0x0000f8) >> 3);
	rle->data[rle->len-2] = rgb & 0xff;
	rle->data[rle->len-1] =	(rgb >> 8) & 0xff;
    }
    rle->run = 0;
    rle->rgb = 0;
    return rle->data ? 0 : -1;
}

static inline void finish_run(struct RLE_image *rle, int rgb, int run)
{
    if (rle->run > 15) {
        rle->data[rle->len++] = rle->rgb;
        rle->data[rle->len++] = rle->run - 16;
    } else if (rle->run > 0) {
        rle->data[rle->len++] = rle->rgb | rle->run;
    }
    rle->run = run;
    rle->rgb = rgb;
}

void encodeRLE_rgba(struct RLE_image *rle, unsigned char *data)
{
    int i = 0;
    int x;

    for (x = 0; x < rle->w; ++x) {
        int rgb = 0;
        unsigned char alpha = 0;

        rgb += (int)data[i++] * (0x10000 * 299 / 1000);   // red
        rgb += (int)data[i++] * (0x10000 * 587 / 1000);   // green
        rgb += (int)data[i++] * (0x10000 * 114 / 1000);   // blue
        rgb >>= 17; // rgb now 0x00..0x7f
        alpha = data[i++];   // alpha

        if (rgb <= 0xf || alpha == 0) {
            rgb = (alpha >> 1) & 0x70;
        } else {
            rgb = (rgb & 0x70) + 0x80;
        }
        if (rle->rgb != rgb || rle->run == MAX_RUN)
            finish_run(rle, rgb, 1);
        else
            rle->run++;
    }
    if (rle->rgb != 0)
        finish_run(rle, 0, 0);
}

void encodeRLE_done(struct RLE_image *rle)
{
    finish_run(rle, 0, 0);
    rle->data = realloc(rle->data, rle->len);
}

#ifdef ENABLE_FONT

static char *font_file = NULL;
static int font_bold = 0;
static int font_italic = 0;
static int font_underline = 0;
static int font_outline = 0;
static int font_size = 0;
static int font_shadow = 0;
static int font_alpha = 0;

#include <wand/magick_wand.h>

static void *font_render(int argc, char *argv[], int rgb, int verbose)
{
	MagickWand *wand = NULL;
	MagickWand *c_wand = NULL;
	DrawingWand *d_wand = NULL;
	PixelWand *p_wand = NULL;
	double *fm = NULL;
	unsigned char *pixels;
	int width, height, y;
	double pad = 1;
	int len = 0;
	int i;
	char *text;
	static int initialized = 0;
	
	if (font_shadow) pad += 1;
	if (font_outline) pad += (double)font_size/8;
	
	
	for (i = optind; i < argc; i++)
		len += strlen(argv[i]) + 1;
	text = alloca(len);
	len = 0;
	for (i = optind; i < argc; i++) {
		int l = strlen(argv[i]);
		memcpy(text + len, argv[i], l);
		len += l;
		text[len++] = ' ';
	}
	text[len-1] = 0;

	if (!initialized) {
		MagickWandGenesis();
		initialized = 1;
	}

	wand = NewMagickWand();
	d_wand = NewDrawingWand();
	p_wand = NewPixelWand();

	PixelSetColor(p_wand, "none");
	MagickNewImage(wand, 1, 1, p_wand);

	DrawSetFont(d_wand, font_file) ;
	DrawSetFontSize(d_wand, font_size);
	DrawSetTextAntialias(d_wand, MagickTrue);
	DrawSetStrokeWidth(d_wand, pad);

	DrawSetFontWeight(d_wand, font_bold ? 900 : 100);
	DrawSetFontStyle(d_wand, font_italic ? ItalicStyle : NormalStyle);
	// Underline bug: Stroke width ignored? Looks bad
	DrawSetTextDecoration(d_wand, font_underline ? UnderlineDecoration : NoDecoration);

	fm = MagickQueryFontMetrics(wand, d_wand, text);
	if (!fm) {
		fprintf(stderr, "Failed to initialize font\n");
		return NULL;
	}
	//printf("metrics: %g %g %g %g %g %g %g %g %g %g %g %g %g\n",
	//	fm[0], fm[1], fm[2], fm[3], fm[4], fm[5], fm[6], 
	//	fm[7], fm[8], fm[9], fm[10], fm[11], fm[12]);
	width = (int)fm[4] + 3 * pad + (font_shadow ? 5 : 0);
	height = (int)fm[5] + 2 * pad + (font_shadow ? 5 : 0);
	y = (int)(fm[5] + fm[3] + pad);
	free(fm);
	fm = NULL;

	MagickRemoveImage(wand);
	MagickNewImage(wand, width, height, p_wand);

	PixelSetColor(p_wand, "white");
	DrawSetFillColor(d_wand, p_wand);


	if (font_outline) {
		PixelSetColor(p_wand, "black");
		PixelSetAlpha(p_wand, (double)font_outline/8);
		DrawSetStrokeColor(d_wand, p_wand);
		DrawAnnotation(d_wand, pad, y, (unsigned char*) text);
	}

	PixelSetColor(p_wand, "none");
	DrawSetStrokeColor(d_wand, p_wand);
	DrawAnnotation(d_wand, pad, y, (unsigned char*) text);
	MagickDrawImage(wand, d_wand);

	if (font_shadow) {
		MagickResetImagePage(wand, "");
		c_wand = CloneMagickWand(wand);
		PixelSetColor(p_wand, "none");
		PixelSetAlpha(p_wand, 1.0);
		MagickSetImageBackgroundColor(wand, p_wand);
		MagickShadowImage(wand, 70, 4, 5, 5);
		MagickCompositeImage(wand, c_wand, OverCompositeOp, 5, 5);

		DestroyMagickWand(c_wand);
	}

	c_wand = NewMagickWand();
	PixelSetColor(p_wand, "none");
	PixelSetAlpha(p_wand, (double)font_alpha/8);
	MagickNewImage(c_wand, width, height, p_wand);
	MagickCompositeImage(c_wand, wand, OverCompositeOp, 0, 0);

	pixels = malloc(width * height * 4);
	if (!pixels) {
		fprintf(stderr, "Failed to allocate buffer\n");
		return NULL;
	}

	MagickExportImagePixels(c_wand, 0, 0, width, height, "RGBA",
		CharPixel, pixels);

	MagickRemoveImage(wand);
	DestroyMagickWand(wand);
	MagickRemoveImage(c_wand);
	DestroyMagickWand(c_wand);
	DestroyDrawingWand(d_wand);
	DestroyPixelWand(p_wand);

	struct RLE_image rle;
	if (encodeRLE_init(&rle, width, height, rgb) != 0) {
		fprintf(stderr, "Failed to allocate memory\n");
		return NULL;
	}

	for (i = 0; i < height; i++)
		encodeRLE_rgba(&rle, pixels + i*width*4);

	encodeRLE_done(&rle);
	free(pixels);
	if (verbose)
		fprintf(stderr, "Encoded %dx%d RLE data length=%d\n", width, height, rle.len - 8 - (int)sizeof(struct s2253_overlay_image));

	struct s2253_overlay_image *buf = (struct s2253_overlay_image *)rle.data;
	buf->length = rle.len - sizeof(struct s2253_overlay_image);
	return buf;
}
#endif


static void drawLine(unsigned char *data, int w, struct s2253_line *line)
{
    int x, y, x2, y2, dx, c, h;
    unsigned int *p;

    if (line->y1 < line->y2) {
        y = line->y1;
        y2 = line->y2;
        h = y2 - y;
        x = line->x1;
        dx = line->x2 - x;
        c = h / 2;
    } else {
        y = line->y2;
        y2 = line->y1;
        h = y2 - y;
        x = line->x2;
        dx = line->x1 - x;
        c = h / 2;
    }
    p = (unsigned int *)(data + y * w*4);
    
    if (dx != 0) {
        // draw a normal line
        while (y <= y2) {
            if (y >= 0 && y < 576) {
                if (x >= 0 && x < 720) {
                    int i = line->width;
                    while (i--) {
                        p[x++] = line->argb;
                    }
                    x -= line->width;
                }
            }
            c += dx;
            if (c <= -h) {
                x -= 1;
                c += h;
                while (c <= -h) {
                    if (y >= 0 && y <= 576 && x >= 0 && x < 720) {
                        p[x] = line->argb;
                    }
                    x -= 1;
                    c += h;
                }
            } else if (c >= h) {
                x += 1;
                c -= h;
                while (c >= h) {
                    if (y >= 0 && y <= 576 && x >= 0 && x < 720) {
                        p[x] = line->argb;
                    }
                    x += 1;
                    c -= h;
                }
            }
            y++;
            p += w;
        }
        return;
    }

    // special case, draw straight line or rectangle
    
    if (x < 0)
        x = 0;
    else if (x >= 720)
        return;
    if (y < 0) {
        p -= y * w*4;
    } else if (y >= 576)
        return;
    if (y + h > 576)
        h = 576 - y;
    x2 = x + line->width;
    if (x2 > 719)
        x2 = 719;
    do {
        int i = x;
        while (i < x2) {
            p[i++] = line->argb;
        }
	p += w;
    } while (h--);
    
}

static void* convert_line_to_rle(struct s2253_overlay_image *buf)
{
	int i, minx = 720, miny = 576, maxx = 0, maxy = 0;
	struct s2253_line *line = (struct s2253_line *)(buf->data + 4);

	// get extents of rectangle
	while ((unsigned char*)line < buf->data + buf->length) {
		int w = line->width;
		if (line->x1 < minx) minx = line->x1;
		if (line->x1+w-1 > maxx) maxx = line->x1+w-1;
		if (line->x2 < minx) minx = line->x2;
		if (line->x2+w-1 > maxx) maxx = line->x2+w-1;
		if (line->y1 < miny) miny = line->y1;
		if (line->y1 > maxy) maxy = line->y1;
		if (line->y2 < miny) miny = line->y2;
		if (line->y2 > maxy) maxy = line->y2;
		line++;
	}
	int width = maxx-minx+1, height = maxy-miny+1;
	unsigned char *pixels = malloc(width * 4 * height);
	if (!pixels) {
		fprintf(stderr, "Failed to allocate memory\n");
		return NULL;
	}
	memset(pixels, 0, width * 4 * height);
	
	line = (struct s2253_line *)(buf->data + 4);
	while ((unsigned char*)line < buf->data + buf->length) {
		// adjust the line to be inside the rectangle and draw it
		line->x1 -= minx;
		line->y1 -= miny;
		line->x2 -= minx;
		line->y2 -= miny;
		drawLine(pixels, width, line);
		line++;
	}
	
	struct RLE_image rle;
	if (encodeRLE_init(&rle, width, height, 0xffffff) != 0) {
		fprintf(stderr, "Failed to allocate memory\n");
		free(pixels);
		return NULL;
	}

	for (i = 0; i < height; i++)
		encodeRLE_rgba(&rle, pixels + i*width*4);

	encodeRLE_done(&rle);
	free(pixels);

	free(buf);  // free the "line" buffer
	
	buf = (struct s2253_overlay_image *)rle.data;
	buf->xOffset = minx;
	buf->yOffset = miny;
	buf->length = rle.len - sizeof(struct s2253_overlay_image);

	return buf;
}
	
	
	








int main(int argc, char *argv[])
{
	int fd;
	struct s2253_overlay_image overlay, *buf = NULL;
	FILE *fp;
	int size = 0;
	int id = 0;
	int xpos = 0;
	int ypos = 0;
	int alpha = 0;
	int update = OVERLAY_UPDATE_DISPLAY;
	char *dev = "/dev/video2";
	char *filename = NULL;
	int lines = 0;
	unsigned int text = 0xffffffff;
	int verbose = 0;
	int ch = 0;
	struct v4l2_capability cap;

	// process command line arguments
	for (;;) {
		int c;

		c = getopt(argc, argv, short_options);

		if (c == -1)
			break;
		switch (c) {
		case 'v':
			verbose = 1;
			break;
		case 'i':
			id = atoi(optarg);
			break;
		case 'x':
			xpos = atoi(optarg);
			update |= OVERLAY_UPDATE_POSITION;
			break;
		case 'y':
			ypos = atoi(optarg);
			update |= OVERLAY_UPDATE_POSITION;
			break;
		case 'a':
			alpha = atoi(optarg);
			if (alpha >= 0 && alpha <= 8)
				update |= OVERLAY_UPDATE_TRANSPARENT;
			break;
		case 'e':
			alpha = 0;
			update |= OVERLAY_UPDATE_TRANSPARENT;
			break;
		case 'q':
			update &= ~OVERLAY_UPDATE_DISPLAY;
			break;
		case 'l':
			lines = 1;
			break;
		case 't':
			text = strtoul(optarg, NULL, 16);
			break;
		case 'd':
			dev = optarg;
			break;
		case 'c':
			ch = atoi(optarg);
			if (ch < 0 || ch > 3)
				printf("invalid channel id: %d\n", ch);
			break;
#ifdef ENABLE_FONT
		case 'f':
			font_file = optarg;
			break;
		case 'b':
			font_bold = 1;
			break;
		case 'I':
			font_italic = 1;
			break;
		case 'u':
			font_underline = 1;
			break;
		case 'o':
			font_outline = 8;
			break;
		case 'O':
			font_outline = atoi(optarg);
			break;
		case 's':
			font_size = atoi(optarg);
			break;
		case 'S':
			font_shadow = 1;
			break;
#endif
		}
	}
	if ((argc < 2 || optind >= argc) && (update == OVERLAY_UPDATE_DISPLAY)) {
		usage();
		return 0;
	}
	if (lines) {
		buf = line_render(argc, argv, verbose);
		if (!buf)
			return 1;
		size = buf->length;

#ifdef ENABLE_FONT
	} else if (font_file) {
		font_alpha = alpha; // font background alpha 0..8
		buf = font_render(argc, argv, text, verbose);
		if (!buf)
			return 1;
		size = buf->length;
#endif	
	} else if (text != 0xffffffff) {
		buf = text_render(argc, argv, text);
		if (!buf)
			return 1;
		size = buf->length;

	} else if (optind < argc) {
		filename = argv[optind];
	}

	fd = open(dev, O_RDWR);
	if (fd == -1) {
		fprintf(stderr, "unable to open video device %s\n", dev);
		return 1;
	}
	if (ioctl(fd, VIDIOC_QUERYCAP, &cap) < 0) {
		perror("VIDIOC_QUERYCAP");
		close(fd);
		return 1;
	}
	
	if (filename) {
		// open file
		fp = fopen(filename, "rb");
		if (fp == NULL) {
			fprintf(stderr, "unable to open file \"%s\"\n", filename);
			exit (EXIT_FAILURE);
		}
		// determine the size
		fseek(fp, 0, SEEK_END);
		size = ftell(fp);
		buf = malloc(sizeof(overlay) + size);
		if (buf == NULL) {
			fprintf(stderr, "failed to allocate buffer\n");
			exit (EXIT_FAILURE);
		}
		// load it into memory
		rewind(fp);
		fread(buf + 1, 1, size, fp);
		fclose(fp);

	} else if (!buf) {
		buf = &overlay;
	} else if (strncmp((char*)buf->data, "line", 4) == 0) {
		if ((((cap.capabilities & V4L2_CAP_DEVICE_CAPS) ? 
				cap.device_caps : cap.capabilities)
		    & V4L2_CAP_VIDEO_OUTPUT) == 0) {
			if (verbose) fprintf(stderr, "converting line to rle\n");
			buf = convert_line_to_rle(buf);
			size = buf->length;
			xpos = buf->xOffset;
			ypos = buf->yOffset;
			update |= OVERLAY_UPDATE_POSITION;
		}
	}


	overlayInit(buf, ch, id, xpos, ypos, alpha, size, update);
	
	if (verbose)
		fprintf(stderr, "ch=%d id=%d x=%d y=%d alpha=%d update=%d len=%d filename=%s\n", 
			ch, id, xpos, ypos, alpha, update, size, filename ? filename : "<none>");

	overlaySend(fd, buf);
	

	close(fd);

	if (buf != &overlay)
		free(buf);

	return 0;
}
