/***************************************************************************************
 *
 * posctl.cc is part of SimpleSkyCam.
 *
 *****************************************************************************************/

#include <list>
#include <string>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <math.h>
#include "gui.h"
#include "pid.h"
#include "filter.h"
#include "detect.h"
#include "configuration.h"
#include "video.h"
#include "videodev.h"
#include "histogram.h"

extern PosCtl posctl;

extern GtkBuilder *_builder_;			// work around for threads
GtkWidget *posctl_rot_da = NULL;
// GtkWidget *posctl_axis1_da = NULL;
// GtkWidget *posctl_axis1_da = NULL;
double linedash1[] = { 1.0, 4.0 };


void posctl_gui_update();

void cb_posctl_show_window (GtkWidget *widget, gpointer data) {
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);

	GtkWidget *wnd = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "window-posctl"));
	gtk_widget_show(wnd);
}


void cb_posctl_btncalib (GtkWidget *widget, gpointer data) {
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);

//	GtkWidget *btn = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_calib"));
//	gtk_widget_set_sensitive(btn, false);

	posctl.StartCalibration();
	posctl_gui_update();
}

void cb_posctl_angles_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer data) {
	int clientw, clienth;

	position_f_2d lpos[5];
	float lmax;
	position_2d center;

	//
	// rotation da
	GtkWidget *entry_rot_angle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_rotangle"));
	GtkWidget *entry_rot_len = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_rotlen"));
	GtkWidget *entry_axis1_angle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1angle"));
	GtkWidget *entry_axis1_lenmi = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1lenmi"));
	GtkWidget *entry_axis1_lenma = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1lenma"));
	GtkWidget *entry_axis2_angle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2angle"));
	GtkWidget *entry_axis2_lenmi = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2lenmi"));
	GtkWidget *entry_axis2_lenma = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2lenma"));

	float rotangle = atof(gtk_entry_get_text(GTK_ENTRY(entry_rot_angle))) * (M_PI / 180);
	float rotlen = atof(gtk_entry_get_text(GTK_ENTRY(entry_rot_len)));
	float a1_angle = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis1_angle))) * (M_PI / 180);
	float a1_lenmi = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis1_lenmi)));
	float a1_lenma = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis1_lenma)));
	float a2_angle = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis2_angle))) * (M_PI / 180);
	float a2_lenmi = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis2_lenmi)));
	float a2_lenma = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis2_lenma)));
//	float lenmax = max(max(max(a1_lenmi, a1_lenma),max(a2_lenmi, a2_lenma)), rotlen);

	if (posctl_rot_da == NULL) // should only be called once
		posctl_rot_da = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_da_rotation"));
	clienth = gtk_widget_get_allocated_height(posctl_rot_da);
	clientw = gtk_widget_get_allocated_width(posctl_rot_da);
	center.x = clientw >> 1;
	center.y = clienth >> 1;

	//
	// draw the background
	cairo_set_source_rgb(cr, 0.0, 0.0, 0.0);
	cairo_paint(cr);
	cairo_fill (cr);
	cairo_set_source_rgb(cr, 0.8, 0.8, 0.8);
	cairo_rectangle (cr, 1, 1, clientw - 2, clienth - 2);
	cairo_fill (cr);

	cairo_set_source_rgb(cr, 0.0, 0.0, 0.0);
	cairo_set_dash(cr, linedash1, 2, 0);
	cairo_move_to(cr, center.x, center.y - center.y * 0.8);
	cairo_line_to(cr, center.x, center.y + center.y * 0.8);
	cairo_stroke(cr);

	cairo_move_to(cr, center.x - center.x * 0.8, center.y);
	cairo_line_to(cr, center.x + center.x * 0.8, center.y);
	cairo_stroke(cr);
	cairo_set_dash(cr, NULL, 0, 0);

	//
	// calculate angles
	lpos[0].x = sin(rotangle) * rotlen;
	lpos[0].y = cos(rotangle) * rotlen;

	lpos[1].x = lpos[0].x + sin(a1_angle) * a1_lenmi;
	lpos[1].y = lpos[0].y + cos(a1_angle) * a1_lenmi;
	lpos[2].x = lpos[0].x + sin(a1_angle) * a1_lenma;
	lpos[2].y = lpos[0].y + cos(a1_angle) * a1_lenma;

	lpos[3].x = lpos[0].x + sin(a2_angle) * a2_lenmi;
	lpos[3].y = lpos[0].y + cos(a2_angle) * a2_lenmi;
	lpos[4].x = lpos[0].x + sin(a2_angle) * a2_lenma;
	lpos[4].y = lpos[0].y + cos(a2_angle) * a2_lenma;

	// find maximum
	for (int i = 0; i < 5; i++) {
		if (i == 0) {
			lmax = fabs (lpos[0].x);
			if (fabs(lpos[i].y) > lmax) lmax = fabs(lpos[i].y);
		} else {
			if (fabs(lpos[i].x) > lmax) lmax = fabs(lpos[i].x);
			if (fabs(lpos[i].y) > lmax) lmax = fabs(lpos[i].y);
		}
	}

	//
	// draw
	cairo_select_font_face (cr, "sans-serif", CAIRO_FONT_SLANT_NORMAL, CAIRO_FONT_WEIGHT_NORMAL);
	cairo_set_font_size (cr, 11);
	cairo_set_line_width(cr, 4.0);

	// rot
	cairo_set_source_rgb(cr, 0.0, 0.0, 0.0);
	cairo_move_to(cr, center.x, center.y);
	cairo_line_to(cr, center.x + float (lpos[0].x * center.x * 0.8 / lmax),
					  center.y - float (lpos[0].y * center.y * 0.8 / lmax));
	cairo_stroke(cr);
	cairo_move_to(cr, center.x + float (lpos[0].x * center.x * 0.9 / lmax),
					  center.y - float (lpos[0].y * center.y * 0.9 / lmax));
	cairo_show_text(cr, (char *)"E");

	// axis1
	cairo_set_source_rgb(cr, 0.0, 1.0, 0.5);
	cairo_move_to(cr, center.x + float (lpos[1].x * center.x * 0.8 / lmax),
					  center.y - float (lpos[1].y * center.y * 0.8 / lmax));
	cairo_line_to(cr, center.x + float (lpos[2].x * center.x * 0.8 / lmax),
					  center.y - float (lpos[2].y * center.y * 0.8 / lmax));
	cairo_stroke(cr);
	cairo_move_to(cr, center.x + float (lpos[2].x * center.x * 0.9 / lmax),
					  center.y - float (lpos[2].y * center.y * 0.9 / lmax));
	cairo_show_text(cr, (char *)"1");

	// axis2
	cairo_set_source_rgb(cr, 0.0, 0.5, 1.0);
	cairo_move_to(cr, center.x + float (lpos[3].x * center.x * 0.8 / lmax),
					  center.y - float (lpos[3].y * center.y * 0.8 / lmax));
	cairo_line_to(cr, center.x + float (lpos[4].x * center.x * 0.8 / lmax),
					  center.y - float (lpos[4].y * center.y * 0.8 / lmax));
	cairo_stroke(cr);
	cairo_move_to(cr, center.x + float (lpos[4].x * center.x * 0.9 / lmax),
					  center.y - float (lpos[4].y * center.y * 0.9 / lmax));
	cairo_show_text(cr, (char *)"2");
};


void cb_posctl_entryanglelen (GtkWidget *widget, gpointer data) {
	if (posctl_rot_da == NULL) // should only be called once
		posctl_rot_da = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_da_rotation"));
    gdk_window_invalidate_rect(gtk_widget_get_window(posctl_rot_da), NULL, true);
};


void cb_posctl_axis_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer data) {
	GtkWidget *da1 = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_da_axis1"));
	GtkWidget *da2 = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_da_axis2"));
	position_2d center;

	int clienth = gtk_widget_get_allocated_height(area);
	int clientw = gtk_widget_get_allocated_width(area);
	center.x = clientw >> 1;
	center.y = clienth >> 1;

	cairo_set_source_rgb(cr, 0.0, 0.0, 0.0);
	cairo_paint(cr);
	cairo_fill (cr);
	cairo_set_source_rgb(cr, 0.8, 0.8, 0.8);
	cairo_rectangle (cr, 1, 1, clientw - 2, clienth - 2);
	cairo_fill (cr);

	cairo_set_source_rgb(cr, 0.0, 0.0, 0.0);
	cairo_set_dash(cr, linedash1, 2, 0);
	cairo_move_to(cr, center.x, center.y - center.y * 0.8);
	cairo_line_to(cr, center.x, clienth);
	cairo_stroke(cr);
	cairo_set_dash(cr, NULL, 0, 0);

	cairo_set_source_rgb(cr, 0.5, 0.5, 0.5);
	cairo_select_font_face (cr, "sans-serif", CAIRO_FONT_SLANT_NORMAL, CAIRO_FONT_WEIGHT_NORMAL);
	cairo_set_font_size (cr, 10);
	cairo_move_to(cr, 30, 10);
	cairo_show_text(cr, (char *)"offset");
	if (da1 != area && da2 != area) {
		cairo_move_to(cr, 30, 0);
		cairo_show_text(cr, (char *)"unknown");
		return;
	}
};


/*
 * posctl gui update
 */
void posctl_gui_update() {
	char txt[255];

	GtkWidget *btnclib = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_calib"));
	GtkWidget *cbenable = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_enablectl"));

	GtkWidget *e_cal_rotangle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_rotangle"));
	GtkWidget *e_cal_rotlen = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_rotlen"));

	GtkWidget *e_cal_a1angle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1angle"));
	GtkWidget *e_cal_a1lenmi = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1lenmi"));
	GtkWidget *e_cal_a1lenma = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1lenma"));

	GtkWidget *e_cal_a2angle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2angle"));
	GtkWidget *e_cal_a2lenmi = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2lenmi"));
	GtkWidget *e_cal_a2lenma = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2lenma"));

	posctl.LockMutex();
	int m = posctl.GetMode();

	if (m == POSCTL_MODE_OFF) {
		gtk_widget_set_sensitive(btnclib, true);
		gtk_widget_set_sensitive(cbenable, true);
	}
	else {
		gtk_widget_set_sensitive(btnclib, false);
		gtk_widget_set_sensitive(cbenable, false);
	}

	strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_rot_angle);
	gtk_entry_set_text (GTK_ENTRY(e_cal_rotangle), txt);
	strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_rot_len);
	gtk_entry_set_text (GTK_ENTRY(e_cal_rotlen), txt);

	strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis1_angle);
	gtk_entry_set_text (GTK_ENTRY(e_cal_a1angle), txt);
	strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis1_lenmi);
	gtk_entry_set_text (GTK_ENTRY(e_cal_a1lenmi), txt);
	strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis1_lenma);
	gtk_entry_set_text (GTK_ENTRY(e_cal_a1lenma), txt);

	strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis2_angle);
	gtk_entry_set_text (GTK_ENTRY(e_cal_a2angle), txt);
	strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis2_lenmi);
	gtk_entry_set_text (GTK_ENTRY(e_cal_a2lenmi), txt);
	strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis2_lenma);
	gtk_entry_set_text (GTK_ENTRY(e_cal_a2lenma), txt);

	posctl.UnLockMutex();
}


/*
 * callback from the detect thread.
 * the gtk/gui updates must and will be processed in a separate gui thread
 */
gboolean cb_thread_posctl (gpointer data) {
	posctl_gui_update();
	return false;
};


PosCtl::PosCtl() {
	mode = POSCTL_MODE_OFF;

	calib_mode = POSCTL_CALIB_MODE_OFF;
	calib_rot_len = 0.0;
	calib_rot_angle = 0.0;
	calib_axis1_angle = 0.0;
	calib_axis1_lenmi = 0.0;
	calib_axis1_lenma = 0.0;
	calib_axis2_angle = 0.0;
	calib_axis2_lenmi = 0.0;
	calib_axis2_lenma = 0.0;
};


/*
 * stop the control or the calibration
 */
void PosCtl::Stop() {
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
	mode = POSCTL_MODE_OFF;
	gdk_threads_add_idle(cb_thread_posctl, NULL);
}


void PosCtl::StartCalibration() {
	printf ("%s:%d\n", __FILE__, __LINE__);
	if (mode != POSCTL_MODE_OFF) {
		printf ("%s:%d mode is not off, can't start calibration.\n", __FILE__, __LINE__);
		return;
	}
	mode = POSCTL_MODE_CALIB;
	calib_mode = POSCTL_CALIB_MODE_START;
	gettimeofday (&calib_timestamp, NULL);
	gdk_threads_add_idle(cb_thread_posctl, NULL);
}


void PosCtl::StartControl() {
	if (mode != POSCTL_MODE_OFF) {
		printf ("%s:%d mode is not off, can't start control.\n", __FILE__, __LINE__);
		return;
	}
	pid_axis[0].Start();
	pid_axis[1].Start();
	mode = POSCTL_MODE_CONTROL;
	gdk_threads_add_idle(cb_thread_posctl, NULL);
}


/*
 * get and set PID parameter, no mutex lock
 * the access to the data should be already locked by the curren tthread
 */
void PosCtl::SetAxisParam (	int axis, double min, double max,
							double k, double i, double d) {

	if (axis < 0 || axis > 1) return;
	pid_axis[axis].SetParam(min, max, k, i, d);
};

void PosCtl::GetAxisParam (	int axis, double *min, double *max,
							double *k, double *i, double *d) {

	if (axis < 0 || axis > 1) return;
	pid_axis[axis].GetParam(min, max, k, i, d);
};


/*
 * calibration functions
 */
void PosCtl::CalibModeStart(int x, int y) {
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);

	calib_pos.x = x;
	calib_pos.y = y;

	calib_mode = POSCTL_CALIB_MODE_DELTA;
	gettimeofday (&calib_timestamp, NULL);
	gdk_threads_add_idle(cb_thread_posctl, NULL);
};


void PosCtl::CalibModeDelta(int x, int y) {
	struct timeval tv;
	float timediff;

	gettimeofday (&tv, NULL);
	timediff = (float)(tv.tv_sec - calib_timestamp.tv_sec) + ((tv.tv_usec - calib_timestamp.tv_usec) / 1000000.0);

	if (timediff > 10.0) {
		position_f_2d fp;

		printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);

		fp.x = x - calib_pos.x;
		fp.x /= (float)timediff;
		fp.y = y - calib_pos.y;
		fp.y /= (float)timediff;

		LockMutex();
		calib_rot_angle = atan2(fp.x, fp.y) * 180.0 / M_PI;
		calib_rot_len = sqrt(fp.x * fp.x + fp.y * fp.y);
		UnLockMutex();

		calib_mode = POSCTL_CALIB_MODE_AXIS1_MA;
		gettimeofday (&calib_timestamp, NULL);
		gdk_threads_add_idle(cb_thread_posctl, NULL);

	}
};


void PosCtl::CalibModeAxis(int x, int y) {
	struct timeval tv;
	float timediff;

	gettimeofday (&tv, NULL);
	timediff = (float)(tv.tv_sec - calib_timestamp.tv_sec) + ((tv.tv_usec - calib_timestamp.tv_usec) / 1000000.0);

	if (timediff > 10.0) {
		printf ("%s:%d %s calib_mode: %d\n", __FILE__, __LINE__, __FUNCTION__, calib_mode);

		calib_mode++;
		gettimeofday (&calib_timestamp, NULL);
		gdk_threads_add_idle(cb_thread_posctl, NULL);
	}
};


void PosCtl::CalibModeFinish() {
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);

	mode = POSCTL_MODE_OFF;
	calib_mode = POSCTL_CALIB_MODE_OFF;
	gettimeofday (&calib_timestamp, NULL);
	gdk_threads_add_idle(cb_thread_posctl, NULL);
};


/*
 * Loop, if new data is aviable
 */
void PosCtl::Loop (int posx, int posy) {
	//
	// calibration mode?
	if (mode == POSCTL_MODE_CALIB) {
		switch (calib_mode) {
		case (POSCTL_CALIB_MODE_START):
			CalibModeStart(posx, posy);
			break;
		case (POSCTL_CALIB_MODE_DELTA):
			CalibModeDelta(posx, posy);
			break;
		case (POSCTL_CALIB_MODE_AXIS1_MI):
		case (POSCTL_CALIB_MODE_AXIS1_MA):
		case (POSCTL_CALIB_MODE_AXIS2_MI):
		case (POSCTL_CALIB_MODE_AXIS2_MA):
			CalibModeAxis(posx, posy);
			break;
		case (POSCTL_CALIB_MODE_FINISH):
			CalibModeFinish();
			break;
		default:
			calib_mode = POSCTL_CALIB_MODE_OFF;
			mode = POSCTL_MODE_OFF;
			gdk_threads_add_idle(cb_thread_posctl, NULL);
		}
	}
};