//
// for testing: make && ./vidoiltank -rotate 270 -samples 1 -debug /mnt/heizoel-data
//

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

#include "vidoiltank.h"

char conf_devname[LEN_FILENAME] = "/dev/video0";
char conf_debugout[LEN_FILENAME] = "";
int conf_width = 1920;
int conf_height = 1080;
int conf_iomode = IOM_MMAP;
int conf_samples = 25;
float conf_filter = 0.1;
int conf_rotate = 0;
Rect conf_rect = { .x = 315, .y = 0, .w = 70, .h = 1910 };
char conf_outdate[LEN_FILENAME];

int detect_level(ImageFloat *imgf);

/*
 * open video device
 * read multiple images into memory
 * close video device
 *
 * calculate images together
 * find position
 *
 * save all data
 */

void help() {
	printf ("\t-dev DEVICE             video device file (default /dev/video0)\n");
	printf ("\t-s w,h                  define with and height\n");
	printf ("\t-debug OUTDIR           debug files output dir\n");
	printf ("\t-ioread                 use read instead of mmap\n");
	printf ("\t-samples NUM            samples to read\n");
	printf ("\t-filter NUM             filter over NUM lines for detection\n");
	printf ("\t-rect X,Y,W,H           part of the screen to search for the level\n");
	printf ("\t-rotate DEG             rotate image by 0,90,180,270\n");
	printf ("\t-help                   display this help\n");
}

int main(int argc, char **argv) {
	Video *video;
	ImageRaw *img = NULL;
	ImageRaw *tmpimg = NULL;
	ImageFloat *imgf = NULL;
	char fn[3*LEN_FILENAME];
	int i, c, b, level, cont, bright, min, max;

	// for debugging only
	time_t curtime = time(NULL);
	struct tm *curtime_tm = NULL;
	curtime_tm = localtime(&curtime);
	strftime(conf_outdate, LEN_FILENAME-1, "%Y%m%d-%H%M%S", curtime_tm);

	for (int i = 1; i < argc; i++) {
		if (strcmp(argv[i], "-dev") == 0) {
			i++;
			strncpy (conf_devname, argv[i], LEN_FILENAME-1);
		}
		else if (strcmp(argv[i], "-s") == 0){
			i++;
			sscanf (argv[i], "%d,%d", &conf_width, &conf_height);
		}
		else if (strcmp(argv[i], "-rect") == 0){
			i++;
			sscanf (argv[i], "%d,%d,%d,%d", &conf_rect.x, &conf_rect.y, &conf_rect.w, &conf_rect.h);
		}
		else if (strcmp(argv[i], "-samples") == 0){
			i++;
			conf_samples = atoi (argv[i]);
		}
		else if (strcmp(argv[i], "-filter") == 0){
			i++;
			conf_filter = 1.0/(float)atoi (argv[i]);
		}
		else if (strcmp(argv[i], "-debug") == 0){
			i++;
			strncpy (conf_debugout, argv[i], LEN_FILENAME-1);
		}
		else if (strcmp(argv[i], "-ioread") == 0){
			conf_iomode = IOM_READ;
		}
		else if (strcmp(argv[i], "-rotate") == 0){
			i++;
			conf_rotate = atoi (argv[i]);
			if (conf_rotate != 0 && conf_rotate != 90 && conf_rotate != 180 && conf_rotate != 270) {
				printf ("rotate can only work for 0°, 90°, 180°, 270°. Rotation of %d° is not supported\n", conf_rotate);
				return 1;
			}
		}
		else if (strcmp(argv[i], "-help") == 0){
			help();
			return 0;
		}
	}

//	printf ("vidoiltank\n");
//	printf ("  device: %s (%d x %d)\n", conf_devname, conf_width, conf_height);

	//
	// open video device
	if ((video = vid_open("/dev/video0")) == NULL) return -1;
	if (vid_start(video, conf_iomode, conf_width, conf_height) == -1) return -1;

	//
	// search for god brightness and contrast
	printf ("check controls\n");
	cont = 200;	// just picked some random numbers
	bright = 170;	// just picked some random numbers
	for (level = 0, c = 32; c <= 250; c += 64)
		for (b = 32; b <= 250; b += 64) {
			vid_setctrls(video, b, c);
			do {} while ((tmpimg = vid_grabimage(video, img)) == NULL && errno == EAGAIN);
			if (tmpimg == NULL) {
				printf ("could not grab image\n");
				exit(1);
			}
			img = tmpimg;
			image_get_minmax (img, &conf_rect, &min, &max);
			if (max-min > level) {
				cont = c;
				bright = b;
				level = max-min;
			}
		}
	printf ("found best control: contrast: %d   brightness: %d\n", cont, bright);
	vid_setctrls(video, bright, cont);

	//
	// get samples
	printf ("Read Samples\n");
	for (i = 0; i < conf_samples; i++) {
		do {} while ((tmpimg = vid_grabimage(video, img)) == NULL && errno == EAGAIN);
		if (tmpimg == NULL) {
			printf ("could not grab image\n");
			exit(1);
		}
		img = tmpimg;
		image_rotate(img, conf_rotate);

		if (i == 0) imgf = imageF_copy(img, &conf_rect);
		else {
			imageF_add (imgf, img, &conf_rect);
		}

		if (i == 0) {
			snprintf (fn, 3*LEN_FILENAME, "%s/%s-img-%02d-%02d.jpg", conf_debugout, conf_outdate, bright, cont);
			image_draw_rect(img, &conf_rect, 0x00FFFF);
			if (strlen(conf_debugout) > 0) image_save(img, fn, 98);
		}
	}
	if (vid_stop(video) == -1) return -1;
	vid_close(video);
	video = NULL;

	tmpimg = image_copyF(imgf);
	snprintf (fn, 3*LEN_FILENAME, "%s/%s-imgf-%03d.jpg", conf_debugout, conf_outdate, conf_samples);
	if (strlen(conf_debugout) > 0) image_save(tmpimg, fn, 98);

	printf ("detect level\n");
	level = detect_level(imgf);
	printf ("got %d\n", level);
	tmpimg = image_copyF(imgf);
	Rect r = {.x = 0,  .y = level, .w = 10, .h = 1};
	image_draw_rect(tmpimg,  &r, 0x0000FF00);
	snprintf (fn, 3*LEN_FILENAME, "%s/%s-img-data.jpg", conf_debugout, conf_outdate);
	if (strlen(conf_debugout) > 0) image_save(tmpimg, fn, 98);

	printf ("%s Found Level: %5.2f    Y-Pos: %d\n", conf_outdate, 100.0 * (1.0-(float)level/(float)imgf->h), level);
	return 0;
};


int detect_level(ImageFloat *imgf) {
	int y, x, level;
	float v = 0.0, vmin, vmax;
	FILE * outfile;
	float deltamin = 0;
	float deltamax = 0;
	int deltamin_y = 0;
	int deltamax_y = 0;
	
	if (imgf == NULL) return -1;
	
	float *deltas  = malloc(sizeof(float) * imgf->h);  // deltas
	float *deltasf = malloc(sizeof(float) * imgf->h);  // filtered deltas
	if (deltas == NULL) {
		printf ("%s:%d could not allocate memory. Error:%s\n", __FILE__, __LINE__, strerror(errno));
		return -1;
	}

	//
	// calculate average, and save value in left row
	for (y = 0; y < imgf->h; y++) {
		for (v = 0.0, x = 0; x < imgf->w; x++)
			v += imgf->data[x + y * imgf->w];
		v = v / (float) x;
		if (y == 0) vmin = vmax = v;
		if (vmin > v) vmin = v;
		if (vmax < v) vmax = v;

		// save result in the top left position,
		// together with a bar for display
		imgf->data[y * imgf->w] = v;
	}

	//
	// normalize 0.0 .. 1.0
	for (y = 0; y < imgf->h; y++) for (x = 0; x < imgf->w; x++) {
		imgf->data[x + y * imgf->w] = (imgf->data[x + y * imgf->w]-vmin)/(vmax-vmin);
	}

	vmin = 1.0;
	vmax = 0.0;
	int outavg_with = 8;

	// generate average and delta
	level = 0;
	float delta = 0.0, deltaf = 0.0;
	for (y = 0; y < imgf->h; y++) {
		// average
		v = 0.0;
		for (x = 0; x < imgf->w; x++) v += imgf->data[x + (y) * imgf->w];
		v /= (float)imgf->w;

		// output
		for (x = 0; x < outavg_with; x++) imgf->data[x + (y) * imgf->w] = v;

		// delta
		if (y > 0) {
	        	delta = imgf->data[0 + y * imgf->w] - imgf->data[0 + (y-1) * imgf->w];
			deltaf = deltasf[y-1] * (1.0-conf_filter) + delta * conf_filter;
	        }
		else {
		        delta = 0.0;
			deltamin_y = 0;
			deltamax_y = 0;
			deltamin = delta;
			deltamax = delta;
			deltas[0] = delta;
			deltasf[0] = delta;
		}
		if (deltamin > deltaf) {
			deltamin = deltaf;
			deltamin_y = y;
		}
		if (deltamax > deltaf) {
			deltamax = deltaf;
			deltamax_y = y;
		}
	       	deltas[y] = delta;
	       	deltasf[y] = deltaf;
	}

	// debugging write output
	if (strlen(conf_debugout) > 0) {
		char fn[3*LEN_FILENAME];
		snprintf (fn, 3*LEN_FILENAME, "%s/%s-data.csv", conf_debugout, conf_outdate);
		if ((outfile = fopen(fn, "wb")) == NULL) {
			fprintf(stderr, "%s:%d can't open %s error:%s\n", __FILE__, __LINE__, fn, strerror(errno));
			exit(1);
		}
		for (y = 0; y < imgf->h; y++) {
			fprintf(outfile, "%05d\t%f\t%f\t%f\n", y, imgf->data[y * imgf->w], deltas[y], deltasf[y]);
		}
		fclose (outfile);
	}

	//
	// search for the level from below
	level = deltamin_y;

	free(deltas);
	free(deltasf);
	return level;
};