/***************************************************************************************
 *
 * 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 "detect.h"
#include "configuration.h"
#include "video.h"
#include "videodev.h"
#include "histogram.h"
#include "error.h"

#ifdef BUILD_WINDOWS
#include "windows.h"
#endif

extern PosCtl posctl;
extern Simulation simulation;


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


/*
 * axis history chart.
 */
#define AXIS_HISTORY_CNT 400
struct AxisHistory {
	double diff;
	double out;
	struct timeval ts;
};
struct AxisHistory axis_history[2][AXIS_HISTORY_CNT];
int axis_history_pos[2] = { 0, 0 };

struct PosCtl_ThreadData posctl_calib_debug;

void axis_history_add(int axis, double diff, double out) {
	if (axis > 1 || axis < 0) return;

	axis_history_pos[axis]++;
	if (axis_history_pos[axis] < 0) axis_history_pos[axis] = 0;
	if (axis_history_pos[axis] >= AXIS_HISTORY_CNT) axis_history_pos[axis] = 0;

	gettimeofday(&axis_history[axis][axis_history_pos[axis]].ts, NULL);
	axis_history[axis][axis_history_pos[axis]].diff = diff;
	axis_history[axis][axis_history_pos[axis]].out = out;
}

#define CALIB_DURATION_DELTA 	120.0		// max time in sec for the detection of rotation movement
#define CALIB_DURATION_AXIS 	120.0		// max time in sec for the detection of axis movement
#define CALIB_DURATION_DIST 	 20.0		// max distance to detect movement
#define CALIB_STARTSTOP_DELAY	  2.5		// time to wait for start and stop

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_btnstop (GtkWidget *widget, gpointer data) {
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);

	posctl.Stop();
	posctl_gui_update();
}


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

	posctl.StartControl();
	posctl_gui_update();
}


void cb_menu_set_displaycalibdata(GtkCheckMenuItem *checkmenuitem,  gpointer user_data) {
	if (gtk_check_menu_item_get_active(checkmenuitem) == TRUE) config.SetCalibrationShowData(true);
	else config.SetCalibrationShowData(false);
}


void cb_posctl_show (GtkWidget *widget, gpointer data) {
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
	posctl_calib_debug.mode = -1;
	posctl_gui_update();
}


void cb_posctl_btn_axismove (GtkWidget *widget, gpointer data) {
	GtkWidget *btn_a1min = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a1_min"));
	GtkWidget *btn_a1center = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a1_center"));
	GtkWidget *btn_a1max = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a1_max"));
	GtkWidget *btn_a2min = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a2_min"));
	GtkWidget *btn_a2center = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a2_center"));
	GtkWidget *btn_a2max = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a2_max"));
	double a1min, a2min, a1max, a2max;

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

	posctl.GetAxisParam(0, &a1min, &a1max, NULL, NULL, NULL);
	posctl.GetAxisParam(1, &a2min, &a2max, NULL, NULL, NULL);

	if (widget == btn_a1min) posctl.OutputWriteValue(0, a1min);
	else if (widget == btn_a1center) posctl.OutputWriteValue(0, (a1max-a1min)/2.0+a1min);
	else if (widget == btn_a1max) posctl.OutputWriteValue(0, a1max);
	else if (widget == btn_a2min) posctl.OutputWriteValue(1, a1min);
	else if (widget == btn_a2center) posctl.OutputWriteValue(1, (a1max-a1min)/2.0+a1min);
	else if (widget == btn_a2max) posctl.OutputWriteValue(1, a1max);

	posctl_gui_update();
}


void cb_posctl_change_entry (GtkWidget *widget, gpointer data) {
	double a1min, a1max, a1p, a1i, a1d;
	double a2min, a2max, a2p, a2i, a2d;

	GtkWidget *e_posdevice = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_device"));
	GtkWidget *a1_min = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1_min"));
	GtkWidget *a1_max = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1_max"));
	GtkWidget *a1_kp = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1_kp"));
	GtkWidget *a1_ki = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1_ki"));
	GtkWidget *a1_kd = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1_kd"));
	GtkWidget *a2_min = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2_min"));
	GtkWidget *a2_max = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2_max"));
	GtkWidget *a2_kp = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2_kp"));
	GtkWidget *a2_ki = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2_ki"));
	GtkWidget *a2_kd = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2_kd"));
	GtkWidget *resetsim = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_resetsim"));

	const char *s;

	posctl.GetAxisParam(0, &a1min, &a1max, &a1p, &a1i, &a1d);
	posctl.GetAxisParam(1, &a2min, &a2max, &a2p, &a2i, &a2d);

	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
	s = gtk_entry_get_text(GTK_ENTRY(widget));
	if (strncmp(s, "SIMULATION", 11) == 0) 	gtk_widget_show (resetsim);
	else 									gtk_widget_hide (resetsim);

	posctl.LockMutex();
	if (e_posdevice == widget) 			posctl.SetDevice(s);
	else if (a1_min == widget)			a1min = atof(s);
	else if (a1_max == widget)			a1max = atof(s);
	else if (a1_kp == widget)			a1p = atof(s);
	else if (a1_ki == widget)			a1i = atof(s);
	else if (a1_kd == widget)			a1d = atof(s);
	else if (a2_min == widget)			a2min = atof(s);
	else if (a2_max == widget)			a2max = atof(s);
	else if (a2_kp == widget)			a2p = atof(s);
	else if (a2_ki == widget)			a2i = atof(s);
	else if (a2_kd == widget)			a2d = atof(s);

	posctl.SetAxisParam(0, a1min, a1max, a1p, a1i, a1d);
	posctl.SetAxisParam(1, a2min, a2max, a2p, a2i, a2d);

	posctl.UnLockMutex();
}


/*
 * draw position relative to target position.
 * draw the axis vectors form the center of the screen.
 * draw the earch movement to the center of the screen.
 */
void cb_posctl_angles_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer data) {
	int clientw, clienth;
	position_f_2d lpos, p;
	position_2d center;
	double pscale = 2.0;

	//
	// 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_len = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1len"));
	GtkWidget *entry_axis2_angle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2angle"));
	GtkWidget *entry_axis2_len = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2len"));

	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_len = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis1_len)));
	float a2_angle = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis2_angle))) * (M_PI / 180);
	float a2_len = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis2_len)));

	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);

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

	// rot
	lpos.x = -sin(rotangle) * rotlen * 4.0;
	lpos.y = -cos(rotangle) * rotlen * 4.0;
	cairo_set_source_rgb(cr, 0.0, 0.0, 0.0);
	cairo_set_line_width(cr, 4.0);
	cairo_move_to(cr, center.x, center.y);
	cairo_line_to(cr, center.x + lpos.x, center.y + lpos.y);
	cairo_stroke(cr);
	cairo_set_line_width(cr, 0.5);
	cairo_move_to(cr, center.x - lpos.x * 100.0, center.y - lpos.y * 100.0);
	cairo_line_to(cr, center.x + lpos.x * 100.0, center.y + lpos.y * 100.0);
	cairo_stroke(cr);

	// axis1
	lpos.x = sin(a1_angle) * a1_len * 4.0;
	lpos.y = cos(a1_angle) * a1_len * 4.0;
	cairo_set_source_rgb(cr, 0.0, 0.75, 0.3);
	cairo_set_line_width(cr, 4.0);
	cairo_move_to(cr, center.x, center.y);
	cairo_line_to(cr, center.x + lpos.x, center.y + lpos.y);
	cairo_stroke(cr);
	cairo_set_line_width(cr, 0.5);
	cairo_move_to(cr, center.x - lpos.x * 100.0, center.y - lpos.y * 100.0);
	cairo_line_to(cr, center.x + lpos.x * 100.0, center.y + lpos.y * 100.0);
	cairo_stroke(cr);

	// axis2
	lpos.x = sin(a2_angle) * a2_len * 4.0;
	lpos.y = cos(a2_angle) * a2_len * 4.0;
	cairo_set_source_rgb(cr, 0.0, 0.3, 0.75);
	cairo_set_line_width(cr, 4.0);
	cairo_move_to(cr, center.x, center.y);
	cairo_line_to(cr, center.x + lpos.x, center.y + lpos.y);
	cairo_stroke(cr);
	cairo_set_line_width(cr, 0.5);
	cairo_move_to(cr, center.x - lpos.x * 100.0, center.y - lpos.y * 100.0);
	cairo_line_to(cr, center.x + lpos.x * 100.0, center.y + lpos.y * 100.0);
	cairo_stroke(cr);

	//
	// draw position relative to the target
	p = posctl.current_pos - posctl.target_pos;
	p.x *= pscale;
	p.y *= pscale;
	cairo_set_source_rgb(cr, 0.8, 0.1, 0.1);
	cairo_set_line_width(cr, 1.0);
	cairo_move_to(cr, center.x-4 + p.x, center.y + p.y);
	cairo_line_to(cr, center.x+4 + p.x, center.y + p.y);
	cairo_stroke(cr);
	cairo_move_to(cr, center.x + p.x, center.y-4 + p.y);
	cairo_line_to(cr, center.x + p.x, center.y+4 + p.y);
	cairo_stroke(cr);
	cairo_move_to(cr, center.x+4 + p.x, center.y-4 + p.y);
	cairo_line_to(cr, center.x-4 + p.x, center.y+4 + p.y);
	cairo_stroke(cr);
	cairo_move_to(cr, center.x+4 + p.x, center.y+4 + p.y);
	cairo_line_to(cr, center.x-4 + p.x, center.y-4 + p.y);
	cairo_stroke(cr);

	// draw distance to axis
	lpos.x = sindeg(posctl.calib_axis1.a) * posctl.axis_pv[0] * pscale;
	lpos.y = cosdeg(posctl.calib_axis1.a) * posctl.axis_pv[0] * pscale;
	cairo_move_to(cr, center.x + p.x, center.y + p.y);
	cairo_line_to(cr, center.x + p.x + lpos.x, center.y + p.y + lpos.y);
	cairo_stroke(cr);

	// draw distance to axis
	lpos.x = sindeg(posctl.calib_axis2.a) * posctl.axis_pv[1] * pscale;
	lpos.y = cosdeg(posctl.calib_axis2.a) * posctl.axis_pv[1] * pscale;
	cairo_move_to(cr, center.x + p.x, center.y + p.y);
	cairo_line_to(cr, center.x + p.x + lpos.x, center.y + p.y + lpos.y);
	cairo_stroke(cr);
};


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);
};


#define AXIS_DIFF_MIN -20.0
#define AXIS_DIFF_MAX 20.0

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 axis = 0 , i, cnt;
	double dx, dy;
	double aoutmin[2];
	double aoutmax[2];

	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);

	if (da1 == area) axis = 0;
	else if (da2 == area) axis = 1;
	else {
		cairo_move_to(cr, 30, 0);
		cairo_show_text(cr, (char *)"unknown");
		return;
	}
	posctl.GetAxisParam(axis, &aoutmin[axis], &aoutmax[axis], NULL, NULL, NULL);

	// draw diff
	cairo_set_source_rgb(cr, 1.0, 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, 10, 10);
	cairo_show_text(cr, (char *)"diff");
	// draw elements diff
	i = axis_history_pos[axis];
	cnt = 0;
	do {
		dx = ((double) clientw) * (axis_history[axis][i].diff - AXIS_DIFF_MIN) /
				(AXIS_DIFF_MAX-AXIS_DIFF_MIN);
		dy = ((double) clienth) * ((double)cnt) / ((double) AXIS_HISTORY_CNT);

		if (dx >= clientw)	dx = clientw-1;
		if (dy >= clienth) 	dy = clienth-1;
		if (dx < 0) 		dx = 0;
		if (dy < 0)		 	dy = 0;

		if (cnt == 0) 	cairo_move_to(cr, (int)dx, int(dy));
		else 			cairo_line_to(cr, (int)dx, int(dy));

		cnt++;
		i--;
		if (i < 0) i = AXIS_HISTORY_CNT-1;
	} while (i != axis_history_pos[axis]);
	cairo_stroke(cr);

	if (axis == 0) 	cairo_set_source_rgb(cr, 0.0, 0.75, 0.3);
	else 			cairo_set_source_rgb(cr, 0.0, 0.3, 0.75);
	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, 60, 10);
	cairo_show_text(cr, (char *)"out");
	// draw elements diff
	i = axis_history_pos[axis];
	cnt = 0;
	do {
		dx = ((double) clientw) * (axis_history[axis][i].out - aoutmin[axis]) /
				(aoutmax[axis]-aoutmin[axis]);
		dy = ((double) clienth) * ((double)cnt) / ((double) AXIS_HISTORY_CNT);

		if (dx >= clientw)	dx = clientw-1;
		if (dy >= clienth) 	dy = clienth-1;
		if (dx < 0) 		dx = 0;
		if (dy < 0)		 	dy = 0;

		if (cnt == 0) 	cairo_move_to(cr, (int)dx, int(dy));
		else 			cairo_line_to(cr, (int)dx, int(dy));

		cnt++;
		i--;
		if (i < 0) i = AXIS_HISTORY_CNT-1;
	} while (i != axis_history_pos[axis]);
	cairo_stroke(cr);
};


/*
 * posctl gui update
 */
void posctl_gui_update() {
	char txt[255];
	double kp, ki, kd, mi, ma;

	GtkWidget *caliblabel = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_calib_label"));

	GtkWidget *btnclib = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_calib"));
	GtkWidget *btnstop = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_stop"));
	GtkWidget *btncontrol = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_control"));
	GtkWidget *btnsimreset = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_resetsim"));

	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_a1len = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1len"));

	GtkWidget *e_cal_a2angle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2angle"));
	GtkWidget *e_cal_a2len = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2len"));
	GtkWidget *lb_earthaxis = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_lb_earthaxis"));
	GtkWidget *lb_axisaxis = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_lb_axisaxis"));

	GtkWidget *e_posdevice = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_device"));
	GtkWidget *btn_a1min = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a1_min"));
	GtkWidget *btn_a1center = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a1_center"));
	GtkWidget *btn_a1max = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a1_max"));
	GtkWidget *btn_a2min = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a2_min"));
	GtkWidget *btn_a2center = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a2_center"));
	GtkWidget *btn_a2max = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a2_max"));


	GtkWidget *a1_min = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1_min"));
	GtkWidget *a1_max = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1_max"));
	GtkWidget *a1_kp = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1_kp"));
	GtkWidget *a1_ki = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1_ki"));
	GtkWidget *a1_kd = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1_kd"));
	GtkWidget *a1_out = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_lb_outx"));
	GtkWidget *a1_pv = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_lb_dx"));
	GtkWidget *a2_min = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2_min"));
	GtkWidget *a2_max = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2_max"));
	GtkWidget *a2_kp = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2_kp"));
	GtkWidget *a2_ki = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2_ki"));
	GtkWidget *a2_kd = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2_kd"));
	GtkWidget *a2_out = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_lb_outy"));
	GtkWidget *a2_pv = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_lb_dy"));

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

	if (m == POSCTL_MODE_OFF) {
		gtk_widget_set_sensitive(btnclib, true);
		gtk_widget_set_sensitive(btncontrol, true);
		gtk_widget_set_sensitive(btnstop, false);
		gtk_widget_set_sensitive(btn_a1min, true);
		gtk_widget_set_sensitive(btn_a1center, true);
		gtk_widget_set_sensitive(btn_a1max, true);
		gtk_widget_set_sensitive(btn_a2min, true);
		gtk_widget_set_sensitive(btn_a2center, true);
		gtk_widget_set_sensitive(btn_a2max, true);
		gtk_widget_set_sensitive(e_posdevice, true);
		gtk_label_set_label(GTK_LABEL(caliblabel), (char *) "");
	}
	else {
		if (m == POSCTL_MODE_CALIB) {
			gtk_label_set_label(GTK_LABEL(caliblabel), (char *) "running");
		}
		gtk_widget_set_sensitive(btnclib, false);
		gtk_widget_set_sensitive(btncontrol, false);
		gtk_widget_set_sensitive(btnstop, true);
		gtk_widget_set_sensitive(btn_a1min, false);
		gtk_widget_set_sensitive(btn_a1center, false);
		gtk_widget_set_sensitive(btn_a1max, false);
		gtk_widget_set_sensitive(btn_a2min, false);
		gtk_widget_set_sensitive(btn_a2center, false);
		gtk_widget_set_sensitive(btn_a2max, false);
		gtk_widget_set_sensitive(e_posdevice, false);
	}

	strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.calib_rot.a);
	gtk_entry_set_text (GTK_ENTRY(e_cal_rotangle), txt);
	strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.calib_rot.l);
	gtk_entry_set_text (GTK_ENTRY(e_cal_rotlen), txt);

	strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.calib_axis1.a);
	gtk_entry_set_text (GTK_ENTRY(e_cal_a1angle), txt);
	strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.calib_axis1.l);
	gtk_entry_set_text (GTK_ENTRY(e_cal_a1len), txt);

	strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.calib_axis2.a);
	gtk_entry_set_text (GTK_ENTRY(e_cal_a2angle), txt);
	strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.calib_axis2.l);
	gtk_entry_set_text (GTK_ENTRY(e_cal_a2len), txt);

	strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.calib_rot.a - posctl.calib_axis1.a);
	gtk_label_set_text (GTK_LABEL(lb_earthaxis), txt);
	strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.calib_axis1.a - posctl.calib_axis2.a);
	gtk_label_set_text (GTK_LABEL(lb_axisaxis), txt);

	gtk_entry_set_text (GTK_ENTRY(e_posdevice), posctl.GetDevice().c_str());
	strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.axis_op[0]);
	gtk_label_set_text(GTK_LABEL(a1_out), txt);
	strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.axis_op[1]);
	gtk_label_set_text(GTK_LABEL(a2_out), txt);

	strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.axis_pv[0]);
	gtk_label_set_text(GTK_LABEL(a1_pv), txt);
	strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.axis_pv[1]);
	gtk_label_set_text(GTK_LABEL(a2_pv), txt);

	posctl.GetAxisParam(0, &mi, &ma, &kp, &ki, &kd);
	strfromd (txt, sizeof(txt-1), (char *)"%f", mi);
	gtk_entry_set_text_nofocus (a1_min, txt);
	strfromd (txt, sizeof(txt-1), (char *)"%f", ma);
	gtk_entry_set_text_nofocus (a1_max, txt);
	strfromd (txt, sizeof(txt-1), (char *)"%f", kp);
	gtk_entry_set_text_nofocus (a1_kp, txt);
	strfromd (txt, sizeof(txt-1), (char *)"%f", ki);
	gtk_entry_set_text_nofocus (a1_ki, txt);
	strfromd (txt, sizeof(txt-1), (char *)"%f", kd);
	gtk_entry_set_text_nofocus (a1_kd, txt);

	posctl.GetAxisParam(1, &mi, &ma, &kp, &ki, &kd);
	strfromd (txt, sizeof(txt-1), (char *)"%f", mi);
	gtk_entry_set_text_nofocus (a2_min, txt);
	strfromd (txt, sizeof(txt-1), (char *)"%f", ma);
	gtk_entry_set_text_nofocus (a2_max, txt);
	strfromd (txt, sizeof(txt-1), (char *)"%f", kp);
	gtk_entry_set_text_nofocus (a2_kp, txt);
	strfromd (txt, sizeof(txt-1), (char *)"%f", ki);
	gtk_entry_set_text_nofocus (a2_ki, txt);
	strfromd (txt, sizeof(txt-1), (char *)"%f", kd);
	gtk_entry_set_text_nofocus (a2_kd, txt);

	const gchar *txtptr = gtk_entry_get_text(GTK_ENTRY(e_posdevice));
	if (strncmp(txtptr, "SIMULATION", 11) == 0) 	gtk_widget_show (btnsimreset);
	else											gtk_widget_hide (btnsimreset);

	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) {
	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"));
	GtkWidget *tg = NULL;

	posctl_gui_update();

	gtk_widget_queue_draw(da1);
	gtk_widget_queue_draw(da2);

	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);

    if (data != NULL) {
    	posctl_calib_debug = *(struct PosCtl_ThreadData*) data;
    	free (data);

    	switch (posctl_calib_debug.mode) {
    	case (POSCTL_CALIB_MODE_OFF):
 	    			tg = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "cal_mode_off"));
  	    			gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(tg), true);
   	    			break;
    	case (POSCTL_CALIB_MODE_STOP1):
 	 	 			tg = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "cal_mode_rs"));
   	    	    	gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(tg), true);
   	    	    	break;
    	case (POSCTL_CALIB_MODE_DELTAM):
 	 	 			tg = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "cal_mode_r"));
   	    	    	gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(tg), true);
   	    	    	break;
    	case (POSCTL_CALIB_MODE_AXIS1START):
 	 	 			tg = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "cal_mode_a1s"));
   	    	    	gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(tg), true);
   	    	    	break;
    	case (POSCTL_CALIB_MODE_AXIS1M):
 	 	 			tg = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "cal_mode_a1"));
   	    	    	gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(tg), true);
   	    	    	break;
    	case (POSCTL_CALIB_MODE_AXIS2START):
 	 	 			tg = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "cal_mode_a2s"));
   	    	    	gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(tg), true);
   	    	    	break;
    	case (POSCTL_CALIB_MODE_AXIS2M):
 	 	 			tg = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "cal_mode_a2"));
   	    	    	gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(tg), true);
   	    	    	break;
    	case (POSCTL_CALIB_MODE_FINISH):
 	 	 			tg = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "cal_mode_f"));
   	    	    	gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(tg), true);
   	    	    	break;
    	default:
    				break;
    	}
    }

	return false;
};


PosCtl::PosCtl() {
	mode = POSCTL_MODE_OFF;

#ifdef DEBUG_POSCTL
	debug_tofile((char*)"posctl.log", 1, (char*)"mode , calEV,calEL , posX,posY,targetX,targetY,dx,dy , cal1V,cal1L , axis1.pv,axis1.op,axis1.kp,axis1.ki,axis1.kd , cal2V,cal2L , axis2.pv,axis2.op,axis2.kp,axis2.ki,axis2.kd\n");
#endif

	calib_mode = POSCTL_CALIB_MODE_OFF;
#if BUILD_WINDOWS
	device_fd = INVALID_HANDLE_VALUE;
#else
	device_fd = -1;
#endif
	device = "";
	device_type = POSCTL_DEVTYPE_TTY;
};

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


void PosCtl::NotifyGtk() {
	int i;
	struct PosCtl_ThreadData *data = (struct PosCtl_ThreadData*) malloc(sizeof(struct PosCtl_ThreadData));
	threaddata.mode = calib_mode;
	switch (calib_mode) {
	case(POSCTL_CALIB_MODE_STOP1):
			for (i = 0; i < 2; i++) {
				threaddata.c[i].x = threaddata.c[i].y = -1;
				threaddata.a1[1].x = threaddata.a1[i].y = -1;
				threaddata.a2[1].x = threaddata.a2[i].y = -1;
			}
			break;
	case(POSCTL_CALIB_MODE_DELTAM):
			threaddata.c[0].x = calib_pos.x;
			threaddata.c[0].y = calib_pos.y;
			break;
	case(POSCTL_CALIB_MODE_AXIS1START):
			threaddata.c[1].x = calib_pos.x;
			threaddata.c[1].y = calib_pos.y;
			break;
	case(POSCTL_CALIB_MODE_AXIS1M):
			threaddata.a1[0].x = calib_pos.x;
			threaddata.a1[0].y = calib_pos.y;
			break;
	case(POSCTL_CALIB_MODE_AXIS2START):
			threaddata.a1[1].x = calib_pos.x;
			threaddata.a1[1].y = calib_pos.y;
			break;
	case(POSCTL_CALIB_MODE_AXIS2M):
			threaddata.a2[0].x = calib_pos.x;
			threaddata.a2[0].y = calib_pos.y;
			break;
	case(POSCTL_CALIB_MODE_FINISH):
			threaddata.a2[1].x = calib_pos.x;
			threaddata.a2[1].y = calib_pos.y;
			break;
	default:
			break;
	}
	*(struct PosCtl_ThreadData*)data = threaddata;
	gdk_threads_add_idle(cb_thread_posctl, data);
}


/*
 * Start Calibration with the max axis output (a1, a2)
 */
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_STOP1;
	gettimeofday (&calib_timestamp, NULL);
	NotifyGtk();
}

void PosCtl::StartControl() {
	if (mode != POSCTL_MODE_OFF) {
		printf ("%s:%d mode is not off, can't start control.\n", __FILE__, __LINE__);
		return;
	}
	printf ("%s:%d %s start controlling Target:%f , %f\n", __FILE__, __LINE__, __FUNCTION__,
			target_pos.x, target_pos.y);

	pid_axis[0].Start();
	pid_axis[1].Start();
	mode = POSCTL_MODE_CONTROL;
	NotifyGtk();
}


/*
 * 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 functionsdouble a1, double a2
 */
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_STOP1;
	if (OutputWriteStop()) mode = POSCTL_MODE_OFF;
	gettimeofday (&calib_timestamp, NULL);
	NotifyGtk();
};


void PosCtl::CalibModeWait(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 > CALIB_STARTSTOP_DELAY) {
		printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);

		calib_mode++;
		calib_pos.x = x;
		calib_pos.y = y;

		gettimeofday (&calib_timestamp, NULL);
		NotifyGtk();
	}
}


void PosCtl::CalibModeDelta(int x, int y) {
	struct timeval tv;
	float timediff;
	double a1min, a1max;
	float dist = hypot ((float)(x - calib_pos.x), (float)(y - calib_pos.y));

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

	if (timediff > CALIB_DURATION_DELTA || dist > CALIB_DURATION_DIST) {
		position_f_2d fp;

		printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
		pid_axis[0].GetParam(&a1min, &a1max, NULL, NULL, NULL);

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

		LockMutex();
		calib_rot_v = fp;
		calc_vec2anglelen(&fp, &calib_rot);
		printf ("%s:%d %s   calib_rot_v.x:%f calib_rot_v.y:%f\n", __FILE__, __LINE__, __FUNCTION__, calib_rot_v.x, calib_rot_v.y);
		UnLockMutex();

		calib_mode = POSCTL_CALIB_MODE_AXIS1START;

		if (OutputWriteStop()) mode = POSCTL_MODE_OFF;
		if (OutputWriteValue(0, pid_axis[0].GetMax())) mode = POSCTL_MODE_OFF;

		gettimeofday (&calib_timestamp, NULL);
		calib_pos.x = x;
		calib_pos.y = y;
		NotifyGtk();
	}
};


void PosCtl::CalibModeAxis(int x, int y) {
	struct timeval tv;
	float timediff;
	position_f_2d fp;
	float dist = hypot ((float)(x - calib_pos.x), (float)(y - calib_pos.y));

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


	if (timediff > CALIB_DURATION_AXIS || dist > CALIB_DURATION_DIST) {
		printf ("%s:%d %s calib_mode: %d\n", __FILE__, __LINE__, __FUNCTION__, calib_mode);
		printf ("%s:%d x: %d y: %d cpos.x:%d cpos.y:%d rot.x:%f  rot.y:%f\n", __FILE__, __LINE__, x, y, calib_pos.x, calib_pos.y, calib_rot_v.x, calib_rot_v.y);

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

		if (calib_mode == POSCTL_CALIB_MODE_AXIS1M)	{
			calib_axis1_v = fp;
			calc_vec2anglelen(&calib_axis1_v, &calib_axis1);
			if (OutputWriteStop()) mode = POSCTL_MODE_OFF;
			else if (OutputWriteValue(1, pid_axis[1].GetMax())) mode = POSCTL_MODE_OFF;
		}
		else if (calib_mode ==  POSCTL_CALIB_MODE_AXIS2M)	{
			calib_axis2_v = fp;
			calc_vec2anglelen(&calib_axis2_v, &calib_axis2);
		}

		if (calib_mode != POSCTL_CALIB_MODE_OFF) calib_mode++;
		gettimeofday (&calib_timestamp, NULL);

		calib_pos.x = x;
		calib_pos.y = y;
		NotifyGtk();
	}
};


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

	mode = POSCTL_MODE_OFF;
	calib_mode = POSCTL_CALIB_MODE_OFF;
	if (OutputWriteStop()) mode = POSCTL_MODE_OFF;
	if (OutputWriteValue(0, 0.5 * (pid_axis[0].GetMin() + pid_axis[0].GetMax()))) mode = POSCTL_MODE_OFF;
	if (OutputWriteValue(1, 0.5 * (pid_axis[1].GetMin() + pid_axis[1].GetMax()))) mode = POSCTL_MODE_OFF;
	gettimeofday (&calib_timestamp, NULL);
	NotifyGtk();
};


/*
 * Loop, if new data is aviable{
 */
void PosCtl::Loop (int posx, int posy) {
#ifdef DEBUG_POSCTL
	static int lastmode = -1;
	double kp0,ki0,kd0, kp1,ki1,kd1;
	if (lastmode != mode && mode != POSCTL_MODE_CONTROL) {
		debug_tofile((char*)"posctl.log", 0, (char*)"%d,,,,,,,,,,,,,,,,,,,,,,\n", mode);
	}
#endif
	//
	// calibration mode?
	if (mode == POSCTL_MODE_CALIB) {
		switch (calib_mode) {
		case (POSCTL_CALIB_MODE_STOP1):
		case (POSCTL_CALIB_MODE_AXIS1START):
		case (POSCTL_CALIB_MODE_AXIS2START):
			CalibModeWait(posx, posy);
			break;
		case (POSCTL_CALIB_MODE_DELTAM):
			CalibModeDelta(posx, posy);
			break;
		case (POSCTL_CALIB_MODE_AXIS1M):
		case (POSCTL_CALIB_MODE_AXIS2M):
			CalibModeAxis(posx, posy);
			break;
		case (POSCTL_CALIB_MODE_FINISH):
			CalibModeFinish();
			break;
		default:
			calib_mode = POSCTL_CALIB_MODE_OFF;
			mode = POSCTL_MODE_OFF;
			NotifyGtk();
			break;
		}
	}

	//
	// dist_axis1+axis2 should contain the lot distance to the
	// of the calibrated axis vector with the starting point at target_pos
	// if the current position is below the axis line, the distance must be negative.
	//
	// this distance must be controlled by the PID axis controller.
	// dist_axis1 --> will control pid_axis2
	// dist_axis2 --> will control pis_axis1
	else if (mode == POSCTL_MODE_CONTROL) {
		position_f_2d p, target_p;

		LockMutex();

		// calculate
		// dist_axis < 0.0 pid should increase the output.
		//           > 0.0 pid should decrease the output
		target_p.x = target_pos.x; target_p.y = target_pos.y;
		p.x = posx; p.y = posy;
		axis_pv[0] = calib_axis2_v.perpendicular(p, target_pos);
		axis_pv[1] = calib_axis1_v.perpendicular(p, target_pos);
		axis_op[0] = pid_axis[0].Update(0.0, axis_pv[0]);
		axis_op[1] = pid_axis[1].Update(0.0, axis_pv[1]);

#ifdef DEBUG_POSCTL
		pid_axis[0].GetParam(NULL, NULL, &kp0, &ki0, &kd0);
		pid_axis[1].GetParam(NULL, NULL, &kp1, &ki1, &kd1);
		debug_tofile((char*)"posctl.log", 0, (char*)"%d , %g,%g , %d,%d,%g,%g,%g,%g , %g,%g , %g,%g,%g,%g,%g , %g,%g , %g,%g,%g,%g,%g\n",
				mode, calib_rot.a, calib_rot.l,
				posx, posy, target_pos.x, target_pos.y, posx - target_pos.x, posy - target_pos.y,
				calib_axis1.a, calib_axis1.l, axis_pv[0], axis_op[0],kp0,ki0,kd0,
				calib_axis2.a, calib_axis2.l, axis_pv[1], axis_op[1],kp1,ki1,kd1);
#endif

		if (OutputWriteValue(0, axis_op[0]) || OutputWriteValue(1, axis_op[1]))
			mode = POSCTL_MODE_OFF;

		axis_history_add(0, axis_pv[0], axis_op[0]);
		axis_history_add(1, axis_pv[1], axis_op[1]);

		UnLockMutex();
		NotifyGtk();
	}

	else {
		LockMutex();
		target_pos.x = posx;
		target_pos.y = posy;
		UnLockMutex();
	}

	current_pos.x = posx;
	current_pos.y = posy;
#ifdef DEBUG_POSCTL
	lastmode = mode;
#endif
};


void PosCtl::SetDevice (std::string d) {
	printf ("%s:%d %s new device:%s\n", __FILE__, __LINE__, __FUNCTION__, d.c_str());

	OutputClose();
	device = d;

	if (d.compare ("SIMULATION") == 0) device_type = POSCTL_DEVTYPE_SIM;
	else  device_type = POSCTL_DEVTYPE_TTY;
};


int PosCtl::OutputClose() {
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
#ifdef BUILD_WINDOWS
	if (device_fd != INVALID_HANDLE_VALUE) CloseHandle(device_fd);
	device_fd = INVALID_HANDLE_VALUE;
#else
	if (device_fd > 0) close(device_fd);
	device_fd = -1;
#endif
	return 0;
};


int PosCtl::OutputOpen() {
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
#ifdef BUILD_WINDOWS
	if (device_fd != INVALID_HANDLE_VALUE) return 0;
#else
	if (device_fd > 0) return 0;
#endif

#ifdef BUILD_WINDOWS
	device_fd = CreateFile(device.c_str(), GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
	if (device_fd == INVALID_HANDLE_VALUE) {
		printf ("%s:%d could not open device:%s Error:%ld\n", __FILE__, __LINE__,
			device.c_str(), GetLastError());
		errormessage_display ((char *)"window-posctl", (char *)"OutputOpen",
				      (char*)"%s:%d could not open device:%s Error:%ld\n", __FILE__, __LINE__, device.c_str(), GetLastError());
		return -1;
	}
	COMMTIMEOUTS comTimeOut;
	comTimeOut.ReadIntervalTimeout = 100;
	comTimeOut.ReadTotalTimeoutMultiplier = 1;
	comTimeOut.ReadTotalTimeoutConstant = 100;
	comTimeOut.WriteTotalTimeoutMultiplier = 0;
	comTimeOut.WriteTotalTimeoutConstant = 0;
	SetCommTimeouts(device_fd, &comTimeOut);
#else
	device_fd = open (device.c_str(), O_RDWR);
	if (device_fd < 0) {
		printf ("%s:%d could not open device:%s Error:%s\n", __FILE__, __LINE__,
				device.c_str(), strerror(errno));
		errormessage_display ((char *)"window-posctl", (char *)"OutputOpen",
				(char*)"%s:%d could not open device:%s Error:%s\n", __FILE__, __LINE__, device.c_str(), strerror(errno));
		return -1;
	}
#endif
	return 0;
};

int PosCtl::WriteTTY (char * outbuf) {
        //printf ("%s:%d %s send: '%s'\n", __FILE__, __LINE__, __FUNCTION__, outbuf);

#ifdef BUILD_WINDOWS
	DWORD len;
	int res;
	res = WriteFile(device_fd, outbuf, strlen(outbuf), &len, NULL);
	if (res == 0 || len != strlen(outbuf)) {
		printf ("%s:%d could not write data to device:%s Error:%ld\n", __FILE__, __LINE__,
			device.c_str(), GetLastError());
		errormessage_display ((char *)"window-posctl", (char *)"WriteTTY",
				      (char *) "%s:%d could not write data to device:%s Error:%ld\n", __FILE__, __LINE__,
				      device.c_str(), GetLastError());
	}
#else
	ssize_t len;
	len = write (device_fd, outbuf, strlen(outbuf));
	if ((size_t) len != strlen(outbuf) || len < 0) {
		printf ("%s:%d could not write data to device:%s Error:%s\n", __FILE__, __LINE__,
				device.c_str(), strerror(errno));
		errormessage_display ((char *)"window-posctl", (char *)"WriteTTY",
				(char *) "%s:%d could not write data to device:%s Error:%s\n", __FILE__, __LINE__,
				device.c_str(), strerror(errno));
	}
#endif
	return 0;
}

int PosCtl::ReadTTY (char * inbuf, int length) {
#ifdef BUILD_WINDOWS
        DWORD len;
	int res;
	res = ReadFile(device_fd, inbuf, length, &len, NULL);
	
	if (res == 0) {
		printf ("%s:%d could not read data from device:%s Error:%ld\n", __FILE__, __LINE__,
			device.c_str(), GetLastError());
		errormessage_display ((char *)"window-posctl", (char *)"ReadTTY",
				      (char *) "%s:%d could not read data from device:%s Error:%ld\n", __FILE__, __LINE__,
				      device.c_str(), GetLastError());
		inbuf[0] = 0;
	}
	else {
	  inbuf[len] = 0;
	  //printf ("%s:%d %s receive: '%s'\n", __FILE__, __LINE__, __FUNCTION__, inbuf);
	}
#else
	ssize_t len;
	// make device non-blocking
	fcntl(device_fd, F_SETFL, fcntl(device_fd, F_GETFL) | O_NONBLOCK);
	
	len = read (device_fd, inbuf, length);

	// somehow the first read sometimes fails, no idea why
	if (len < 0 && (errno == EAGAIN || errno == EWOULDBLOCK)) {
	  usleep(100000);
	  len = read (device_fd, inbuf, length);
	}
	
	if (len < 0) {
		if (errno != EAGAIN) {

			printf ("%s:%d could not read data from device:%s Error:(%d) %s\n", __FILE__, __LINE__,
					device.c_str(), errno, strerror(errno));
			errormessage_display ((char *)"window-posctl", (char *)"ReadTTY",
					(char *) "%s:%d could not read data from device:%s Error:%s\n", __FILE__, __LINE__,
					device.c_str(), strerror(errno));
		}
		inbuf[0] = 0;
	}
	else {
	  inbuf[len] = 0;
	  printf ("%s:%d %s receive: '%s'\n", __FILE__, __LINE__, __FUNCTION__, inbuf);
	}

	// make device blocking
	fcntl(device_fd, F_SETFL, fcntl(device_fd, F_GETFL) & ~O_NONBLOCK);
#endif
	return 0;
}

int PosCtl::OutputWriteValue (int axis, double value) {
	char outbuf[255];
	// printf ("%s:%d %s   Axis %d   Value:%f\n", __FILE__, __LINE__, __FUNCTION__, axis, value);

	if (device_type == POSCTL_DEVTYPE_SIM) {
		simulation.AxisSetValue(axis, value);
		return 0;
	}

#ifdef BUILD_WINDOWS
	if (device_fd == INVALID_HANDLE_VALUE) if (OutputOpen() != 0) return -1;
#else
	if (device_fd <= 0) if (OutputOpen() != 0) return -1;
#endif
	//
	// save language setting and set to plain C
	std::string s = setlocale(LC_ALL, NULL);
	setlocale (LC_ALL, "C");

	snprintf(outbuf, 254, ":R%c%+09.4f#", (axis == 0 ? 'D' : 'R'), value);
	outbuf[254] = 0;

	// reset language setting to default
	setlocale (LC_ALL, s.c_str());

	WriteTTY(outbuf);
	ReadTTY(outbuf, sizeof(outbuf) - 1);

	return 0;
};

int PosCtl::OutputWriteStop () {
	char outbuf[255];

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

	if (device_type == POSCTL_DEVTYPE_SIM) {
		simulation.AxisStop();
		return 0;
	}

	snprintf (outbuf, 255, ":Q#");
#ifdef BUILD_WINDOWS
	if (device_fd == INVALID_HANDLE_VALUE) if (OutputOpen() != 0) return -1;
#else
	if (device_fd <= 0) if (OutputOpen() != 0) return -1;
#endif
	WriteTTY(outbuf);

	return 0;
}

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

	OutputWriteValue(0, 0.0);
	OutputWriteValue(1, 0.0);

	return 0;
}