#include <QtSerialPort/QSerialPortInfo>
#include <QtWidgets>
#include <time.h>
#include <stdint.h>
#include "project.h"
#include "mainwindow.h"
#include "ui_mainwindow.h"

#ifdef Q_OS_LINUX
#include <wiringPi.h>
#endif

//	----------------------------------------------------------------------------
//	Constants
//	----------------------------------------------------------------------------

	#define PI (3.14159265358979323846)

	const double toRad = PI/180;
	const double toDeg = 180.0/PI;

//	-----------------------------------------------------------
//	Functions
//	-----------------------------------------------------------

inline qint64 elapsed();

//	-----------------------------------------------------------
//	mainX
//	-----------------------------------------------------------

MainWindow *mainX = NULL;
Worker *consoleIO = NULL;

//	-----------------------------------------------------------
//	Geo test
//	-----------------------------------------------------------

	double p1[3]={0,0,0};
	float l1[3]={0,0,0};
	double p2[3];
	float l2[3];
	double t[3];
	double novAtelLat;
	double novAtelLon;
	double novAtelAlt;
	int16_t novAtelCount;
	int16_t novAtelMode;
	void triangulate();
	int autotarget;
	unsigned int targetIndex;
	unsigned int targetSet;
	QElapsedTimer mainTime;
	int controlsEdited;

//	----------------------------------------------------------------------------
//	target_t
//	----------------------------------------------------------------------------

	typedef struct{
		double lat;
		double lon;
		double alt;
		float range;
		char desc[200];
	}target_t;

//	----------------------------------------------------------------------------
//	targets
//	----------------------------------------------------------------------------

	#define NTARS 4
	#define NTARG 13

target_t targets[NTARS][NTARG]={
	{
		{43.351786,-79.798379,112.198,0,"Tower 1"	},
		{43.351641,-79.798130,111.703,0,"Tower 2"	},
		{43.351330,-79.797971,110.932,0,"Tower 3"	},
		{43.350953,-79.797623,110.164,0,"Tower 4"	},
		{43.350603,-79.797355,109.611,0,"Tower 5"	},
		{43.350241,-79.797096,108.221,0,"Tower 6"	},
		{43.350328,-79.796676,105.842,0,"Tower 7"	},
		{43.350533,-79.796343,107.803,0,"Tower 8"	},
		{43.350824,-79.795852,111.834,0,"Tower 9"	},
		{43.351115,-79.795359,110.525,0,"Tower 10"	},
		{43.351426,-79.794865,110.368,0,"Tower 11"	},
		{0,0,0,0,"Free"},
		{0,0,0,0,"Free"}
	},
	{
		{43.351786,-79.798379,112.198,0,"Tower 1"	},
		{43.351330,-79.797971,110.932,0,"Tower 3"	},
		{43.350603,-79.797355,109.611,0,"Tower 5"	},
		{43.350328,-79.796676,105.842,0,"Tower 7"	},
		{43.350824,-79.795852,111.834,0,"Tower 9"	},
		{43.351426,-79.794865,110.368,0,"Tower 11"	},
		{0,0,0,0,"Free"},
		{0,0,0,0,"Free"},
		{0,0,0,0,"Free"},
		{0,0,0,0,"Free"},
		{0,0,0,0,"Free"},
		{0,0,0,0,"Free"},
		{0,0,0,0,"Free"}
	},
	{
		{43.37369820,-79.74583728,61,0,"431 Meadowhill chimney"},
		{43.37358834,-79.74574544,61,0,"427 Meadowhill chimney"},
		{43.37350475,-79.74558071,61,0,"421 Meadowhill chimney"},
		{43.37338433,-79.74541866,61,0,"417 Meadowhill chimney"},
		{43.37326419,-79.74526623,61,0,"411 Meadowhill chimney"},
		{43.37309438,-79.74497872,61,0,"407 Meadowhill chimney"},
		{43.37293040,-79.74483149,61,0,"403 Meadowhill chimney"},
		{43.37290752,-79.74480308,61,0,"399 Meadowhill chimney"},
		{43.37273283,-79.74452639,61,0,"395 Meadowhill peak"},
		{43.37266597,-79.74441072,61,0,"389 Meadowhill chimney"},
		{43.37248230,-79.74417461,61,0,"385 Meadowhill chimney"},
		{43.37234848,-79.74403023,61,0,"379 Meadowhill chimney"},
		{43.37224706,-79.74385452,61,0,"375 Meadowhill chimney"}
	},
	{
		{0,0,0,0,"Free"},
		{0,0,0,0,"Free"},
		{0,0,0,0,"Free"},
		{0,0,0,0,"Free"},
		{0,0,0,0,"Free"},
		{0,0,0,0,"Free"},
		{0,0,0,0,"Free"},
		{0,0,0,0,"Free"},
		{0,0,0,0,"Free"},
		{0,0,0,0,"Free"},
		{0,0,0,0,"Free"},
		{0,0,0,0,"Free"},
		{0,0,0,0,"Free"}
	},
};

int targetIndices[NTARS];
float maxTargetRange = -1;
float minTargetRange = -1;

//	-----------------------------------------------------------
//	Globals
//	-----------------------------------------------------------

	extern const char *DL_filt[];
	extern const char *DL_plot[];
	extern const char *DL_axes[];
	extern const char *DL_bd[];
	extern steering_t steeringOut;

//	-----------------------------------------------------------
//	Globals
//	-----------------------------------------------------------

	extern int newOperatorSettings;
	extern int newOperatorSettingsX;
	extern void setAltitude(double alt);
	extern void setTarget(double lat, double lon, double alt);

//	-----------------------------------------------------------
//	Globals
//	-----------------------------------------------------------

	utility_t util;

//	-----------------------------------------------------------
//	Functions
//	-----------------------------------------------------------

	bool triangulateTarget(double (*position1)[3], float (*los1D)[3], double (*position2)[3], float (*los2D)[3], double (*positiont)[3]);

//	-----------------------------------------------------------
//	Global Functions
//	-----------------------------------------------------------

	extern int latLonToMeters(double lat,double lon,float *xP,float *yP);
	extern void setTarget(double lat, double lon, double alt);
	extern void setAltitude(double alt);

//	-----------------------------------------------------------
//	MainWindow
//	-----------------------------------------------------------

MainWindow::MainWindow(QWidget *parent) :
	QMainWindow(parent),
	ui(new Ui::MainWindow)
{
	mainTime.start();

#ifdef Q_OS_LINUX
	wiringPiSetup();
	for(int i=0;i<7;i++){
		pullUpDnControl(i,PUD_UP);
	}
#endif

	// init attributes
	guiGimIpAddress = "10.0.0.2";			// inits
	guiGimComPort = defaultPORT;			// inits
	guiGimIpPort = 3000;					// inits
	guiGimBaud = UTIL_BAUD;					// inits
	guiGimConnect = CONNECT_232;			// inits
	treeModel = NULL;						// inits
	chart01 = NULL;							// inits
	chart02 = NULL;							// inits
	chart03 = NULL;							// inits
	chart04 = NULL;							// inits
	chart05 = NULL;							// inits
	chart06 = NULL;							// inits
	refreshPar = 1;							// inits
	refreshControls = 1;					// inits
	m_axis = 0;								// inits

	resetIMU = 0;
	resetChart1 = 0;
	resetChart2 = 0;
	resetChart3 = 0;

	charttable1 = gimbalThread.stateAxisTbl;
	charttable2 = gimbalThread.lensFeedbackTbl;
	charttable3 = gimbalThread.lensFeedbackTbl;

	chartcol1 = 0;
	chartcol2 = 1;
	chartcol3 = 1;

	chartrow1 = 1;
	chartrow2 = 1;
	chartrow3 = 2;

	// init operator settings
	operSettings.panGain = 1;
	operSettings.tiltGain = 1;
	operSettings.rollGain = 1;
	operSettings.dynGain = 0.03;

	// gui
	ui->setupUi(this);

	// comms init
	foreach (const QSerialPortInfo &info, QSerialPortInfo::availablePorts()){
		if(info.productIdentifier() == (qint16)7229)continue;
		if(info.productIdentifier() == (qint16)424)continue;
		if(info.portName().toLocal8Bit() == "COM6"){
			displayError(0,0,"Skipping COM6: reserved for Odin");
			continue;
		}
		ui->serialPortComboBox->addItem(info.portName());
	}
	if(0==ui->serialPortComboBox->count()){
		if(guiGimConnect == CONNECT_232)guiGimConnect = CONNECT_NONE;
	}
	//	ui->serialPortComboBox->addItem("10.0.0.2");
	//	ui->serialPortComboBox->addItem("10.0.0.3");
	ui->serialPortComboBox->addItem("disconnect");
	int np = ui->serialPortComboBox->count();
	for(int i=0;i<np;i++){
		if(guiGimConnect == CONNECT_NONE){
			if("disconnect" == ui->serialPortComboBox->itemText(i)){
				ui->serialPortComboBox->setCurrentIndex(i);
				break;
			}
		}else if(guiGimConnect == CONNECT_232){
			if(defaultPORT == ui->serialPortComboBox->itemText(i)){
				ui->serialPortComboBox->setCurrentIndex(i);
				guiGimComPort = ui->serialPortComboBox->itemText(i);
				break;
			}
			if(QString("ttyACM1") == ui->serialPortComboBox->itemText(i)){
				ui->serialPortComboBox->setCurrentIndex(i);
				guiGimComPort = ui->serialPortComboBox->itemText(i);
				break;
			}
			if(i == (np-2)){
				ui->serialPortComboBox->setCurrentIndex(i);
				guiGimComPort = ui->serialPortComboBox->itemText(i);
				break;
			}
		}else{
			if(guiGimIpAddress == ui->serialPortComboBox->itemText(i)){
				ui->serialPortComboBox->setCurrentIndex(i);
				break;
			}
		}
	}

	// graph
	graphScene = new QGraphicsScene(ui->graphicsView);
	ui->graphicsView->setScene(graphScene);
	chart01 = new chartGraphicsItem;
	chart02 = new chartGraphicsItem;
	//	chart03 = new chartGraphicsItem();
	graphScene->addItem(chart01);
	graphScene->addItem(chart02);
	//	graphScene->addItem(chart03);
	int dy = 270;
	chart02->moveBy(0,1*dy);
	//	chart03->moveBy(0,2*dy);

	// strip charts
	graphScene2 = new QGraphicsScene(ui->graphicsView_2);
	ui->graphicsView_2->setScene(graphScene2);
	chart04 = new chartGraphicsItem;
	chart05 = new chartGraphicsItem;
	chart06 = new chartGraphicsItem;
	graphScene2->addItem(chart04);
	graphScene2->addItem(chart05);
	graphScene2->addItem(chart06);
	dy = 180;
	chart05->moveBy(0,1*dy);
	chart06->moveBy(0,2*dy);

	// timer
	timer = new QTimer(this);
	timer2 = new QTimer(this);

#ifdef _OPERATOR_DIALOG
	// operator dialog
	opDlg = new operatorDialog;
#endif
	// signals and slots
	connect(ui->serialPortComboBox, SIGNAL(currentIndexChanged(QString)), this, SLOT(newGimAddress(QString)));
	connect(this, SIGNAL(connectToGimbal(int)),&gimbalThread, SLOT(guiConnectReq(int)));
	//	connect(this, SIGNAL(newJoystick(steering_t *,int,int,int)),&gimbalThread, SLOT(newJoystick(steering_t *,int,int,int)));
	connect(this, SIGNAL(mainStop()),&gimbalThread, SLOT(stop()));
	connect(&gimbalThread, SIGNAL(errorx(int,int,QString)),this, SLOT(displayError(int,int,QString)));
	connect(&gimbalThread, SIGNAL(connectedState(int,QString)),this, SLOT(connectedState(int,QString)));
	connect(&gimbalThread, SIGNAL(lastTable()),this, SLOT(lastTableMain()));
	connect(&gimbalThread, SIGNAL(newTableData(int)),this, SLOT(newTableDataMain(int)));
	connect(&gimbalThread, SIGNAL(newImuData()),this, SLOT(newImuDataMain()));
	connect(&gimbalThread, SIGNAL(newTuneData()),this, SLOT(newTuneDataMain()));
	connect(timer,	SIGNAL(timeout()), this, SLOT(onTimer()));
	connect(timer2,	SIGNAL(timeout()), this, SLOT(onTimer2()));

	// connect Tune widgets
	setupTune();

	// connect to gimbal
	gimbalThread.settings(guiGimIpAddress.toLocal8Bit(),guiGimComPort.toLocal8Bit(),guiGimIpPort,guiGimBaud);
	emit connectToGimbal(guiGimConnect);

	// gui selections
	ui->tabWidget->setCurrentIndex(0);
	ui->radioButton->setChecked(true);

	// execute
	gimbalThread.start();

#ifdef _CONSOLE
	consoleIO = startConsoleIoThread("serial0",115200);
#endif
#ifdef GPS_PORT
	startGpsThread(GPS_PORT,460800);
#endif
	timer->start(33);
	timer2->start(10);
#ifdef _OPERATOR_DIALOG
	opDlg->show();
#endif
	ui->graphicsView_2->setViewportUpdateMode(QGraphicsView::FullViewportUpdate);

	mainX = this;

	QString resstring;
	resstring.sprintf("Startup complete %Li msec",mainTime.elapsed());
	displayError(0,0,resstring);
}

//	----------------------------------------------------------------------------
//	~MainWindow
//	----------------------------------------------------------------------------

MainWindow::~MainWindow()
{
	delete ui;
}

//	----------------------------------------------------------------------------
//	setupTune
//	----------------------------------------------------------------------------

void MainWindow::setupTune()
{
	// signals and slots
	connect(ui->pGainE,		SIGNAL(editingFinished()),	this,SLOT(editingFinished_pGain()));
	connect(ui->iGainE,		SIGNAL(editingFinished()),	this,SLOT(editingFinished_iGain()));
	connect(ui->dGainE,		SIGNAL(editingFinished()),	this,SLOT(editingFinished_dGain()));
	connect(ui->limitE,		SIGNAL(editingFinished()),	this,SLOT(editingFinished_limit()));
	connect(ui->kTE,		SIGNAL(editingFinished()),	this,SLOT(editingFinished_kT()));
	connect(ui->kCE,		SIGNAL(editingFinished()),	this,SLOT(editingFinished_kC()));
	connect(ui->kPE,		SIGNAL(editingFinished()),	this,SLOT(editingFinished_kP()));
	connect(ui->kFFE,		SIGNAL(editingFinished()),	this,SLOT(editingFinished_kFF()));
	connect(ui->kTachE,		SIGNAL(editingFinished()),	this,SLOT(editingFinished_kTach()));
	connect(ui->fFreqE,		SIGNAL(editingFinished()),	this, SLOT(editingFinished_fFreq1()));
	connect(ui->fFreqE_2,	SIGNAL(editingFinished()),	this, SLOT(editingFinished_fFreq2()));
	connect(ui->fFreqE_3,	SIGNAL(editingFinished()),	this, SLOT(editingFinished_fFreq3()));
	connect(ui->fFreqE_4,	SIGNAL(editingFinished()),	this, SLOT(editingFinished_fFreq4()));
	connect(ui->fFreqE_5,	SIGNAL(editingFinished()),	this, SLOT(editingFinished_fFreq5()));
	connect(ui->fQE,		SIGNAL(editingFinished()),	this, SLOT(editingFinished_fQ1()));
	connect(ui->fQE_2,		SIGNAL(editingFinished()),	this, SLOT(editingFinished_fQ2()));
	connect(ui->fQE_3,		SIGNAL(editingFinished()),	this, SLOT(editingFinished_fQ3()));
	connect(ui->fQE_4,		SIGNAL(editingFinished()),	this, SLOT(editingFinished_fQ4()));
	connect(ui->fQE_5,		SIGNAL(editingFinished()),	this, SLOT(editingFinished_fQ5()));
	connect(ui->sinA0E,		SIGNAL(editingFinished()),	this, SLOT(editingFinished_sinA0()));
	connect(ui->sinA1E,		SIGNAL(editingFinished()),	this, SLOT(editingFinished_sinA1()));
	connect(ui->sinA2E,		SIGNAL(editingFinished()),	this, SLOT(editingFinished_sinA2()));
	connect(ui->sinF0E,		SIGNAL(editingFinished()),	this, SLOT(editingFinished_sinF0()));
	connect(ui->sinF1E,		SIGNAL(editingFinished()),	this, SLOT(editingFinished_sinF1()));
	connect(ui->sinF2E,		SIGNAL(editingFinished()),	this, SLOT(editingFinished_sinF2()));
	connect(ui->fTypeC,			SIGNAL(currentIndexChanged(QString)), this, SLOT(currentIndexChanged_fType1(QString)));
	connect(ui->fTypeC_2,		SIGNAL(currentIndexChanged(QString)), this, SLOT(currentIndexChanged_fType2(QString)));
	connect(ui->fTypeC_3,		SIGNAL(currentIndexChanged(QString)), this, SLOT(currentIndexChanged_fType3(QString)));
	connect(ui->fTypeC_4,		SIGNAL(currentIndexChanged(QString)), this, SLOT(currentIndexChanged_fType4(QString)));
	connect(ui->fTypeC_5,		SIGNAL(currentIndexChanged(QString)), this, SLOT(currentIndexChanged_fType5(QString)));
	connect(ui->plotVarC,		SIGNAL(currentIndexChanged(QString)), this, SLOT(currentIndexChanged_plotVar(QString)));
	connect(ui->plotAxisC,		SIGNAL(currentIndexChanged(QString)), this, SLOT(currentIndexChanged_plotAxis(QString)));
	connect(ui->backdrive0C,	SIGNAL(currentIndexChanged(QString)), this, SLOT(currentIndexChanged_backdrive0(QString)));
	connect(ui->backdrive1C,	SIGNAL(currentIndexChanged(QString)), this, SLOT(currentIndexChanged_backdrive1(QString)));
	connect(ui->backdrive2C,	SIGNAL(currentIndexChanged(QString)), this, SLOT(currentIndexChanged_backdrive2(QString)));
	connect(ui->radioButton,	SIGNAL(pressed()),		this, SLOT(pressedYaw()));
	connect(ui->radioButton_2,	SIGNAL(pressed()),		this, SLOT(pressedPit()));
	connect(ui->radioButton_3,	SIGNAL(pressed()),		this, SLOT(pressedRol()));
	connect(ui->plotRawCB,		SIGNAL(stateChanged(int)),	this, SLOT(stateChangedRaw(int)));
	connect(ui->enable0CB,		SIGNAL(stateChanged(int)),	this, SLOT(stateChangedEnable0(int)));
	connect(ui->enable1CB,		SIGNAL(stateChanged(int)),	this, SLOT(stateChangedEnable1(int)));
	connect(ui->enable2CB,		SIGNAL(stateChanged(int)),	this, SLOT(stateChangedEnable2(int)));
	connect(ui->remote0CB,		SIGNAL(stateChanged(int)),	this, SLOT(stateChangedRemote0(int)));
	connect(ui->remote1CB,		SIGNAL(stateChanged(int)),	this, SLOT(stateChangedRemote1(int)));
	connect(ui->remote2CB,		SIGNAL(stateChanged(int)),	this, SLOT(stateChangedRemote2(int)));

	// combo boxes for HS filter type
	for(int i = 0; i<NUM_FILTER_TYPES;i++){
		ui->fTypeC->addItem(DL_filt[i]);
		ui->fTypeC_2->addItem(DL_filt[i]);
		ui->fTypeC_3->addItem(DL_filt[i]);
		ui->fTypeC_4->addItem(DL_filt[i]);
		ui->fTypeC_5->addItem(DL_filt[i]);
	}
	// combo boxes for HS plot type
	for(int i = 0; i<NUM_PLOT_TYPES;i++){
		ui->plotVarC->addItem(DL_plot[i]);
	}
	// combo boxes for HS plot axis
	for(int i = 0; i<NiAXES;i++){
		ui->plotAxisC->addItem(DL_axes[i]);
	}
	// combo boxes for backdrive mode
	for(int i = 0; i<NUM_BD_TYPES;i++){
		ui->backdrive0C->addItem(DL_bd[i]);
		ui->backdrive1C->addItem(DL_bd[i]);
		ui->backdrive2C->addItem(DL_bd[i]);
	}
}

//	----------------------------------------------------------------------------
//	pressedXXX
//	----------------------------------------------------------------------------

	void MainWindow::pressedYaw(){m_axis = 0;refreshPar = 1;}
	void MainWindow::pressedPit(){m_axis = 1;refreshPar = 1;}
	void MainWindow::pressedRol(){m_axis = 2;refreshPar = 1;}

//	----------------------------------------------------------------------------
//	stateChangedXXX
//	----------------------------------------------------------------------------

	void MainWindow::stateChangedRaw(int state)			{	gimbalThread.editI(key_plotRaw,		0,0,(state)?(1):0);	controlsEdited=1;}
	void MainWindow::stateChangedEnable0(int state)		{	gimbalThread.editI(key_enable,		0,0,(state)?(1):0);	controlsEdited=1;}
	void MainWindow::stateChangedEnable1(int state)		{	gimbalThread.editI(key_enable,		1,0,(state)?(1):0);	controlsEdited=1;}
	void MainWindow::stateChangedEnable2(int state)		{	gimbalThread.editI(key_enable,		2,0,(state)?(1):0);	controlsEdited=1;}
	void MainWindow::stateChangedRemote0(int state)		{	gimbalThread.editI(key_remote,		0,0,(state)?(1):0);	controlsEdited=1;}
	void MainWindow::stateChangedRemote1(int state)		{	gimbalThread.editI(key_remote,		1,0,(state)?(1):0);	controlsEdited=1;}
	void MainWindow::stateChangedRemote2(int state)		{	gimbalThread.editI(key_remote,		2,0,(state)?(1):0);	controlsEdited=1;}

//	----------------------------------------------------------------------------
//	currentIndexChanged_XXX
//	----------------------------------------------------------------------------

	void MainWindow::currentIndexChanged_fType1(QString )		{	gimbalThread.editI(key_fType,		m_axis,	0,ui->fTypeC->currentIndex());}
	void MainWindow::currentIndexChanged_fType2(QString )		{	gimbalThread.editI(key_fType,		m_axis,	1,ui->fTypeC_2->currentIndex());}
	void MainWindow::currentIndexChanged_fType3(QString )		{	gimbalThread.editI(key_fType,		m_axis,	2,ui->fTypeC_3->currentIndex());}
	void MainWindow::currentIndexChanged_fType4(QString )		{	gimbalThread.editI(key_fType,		m_axis,	3,ui->fTypeC_4->currentIndex());}
	void MainWindow::currentIndexChanged_fType5(QString )		{	gimbalThread.editI(key_fType,		m_axis,	4,ui->fTypeC_5->currentIndex());}
	void MainWindow::currentIndexChanged_plotAxis(QString )		{	gimbalThread.editI(key_plotAxis,	0,		0,ui->plotAxisC->currentIndex());}
	void MainWindow::currentIndexChanged_plotVar(QString )		{	gimbalThread.editI(key_plotVar,		0,		0,ui->plotVarC->currentIndex());	controlsEdited=1;}
	void MainWindow::currentIndexChanged_backdrive0(QString )	{	gimbalThread.editI(key_backdrive,	0,		0,ui->backdrive0C->currentIndex());	controlsEdited=1;}
	void MainWindow::currentIndexChanged_backdrive1(QString )	{	gimbalThread.editI(key_backdrive,	1,		0,ui->backdrive1C->currentIndex());	controlsEdited=1;}
	void MainWindow::currentIndexChanged_backdrive2(QString )	{	gimbalThread.editI(key_backdrive,	2,		0,ui->backdrive2C->currentIndex());	controlsEdited=1;}

//	----------------------------------------------------------------------------
//	editingFinished_XXX
//	----------------------------------------------------------------------------

	void MainWindow::editingFinished_pGain(){	gimbalThread.edit(key_pGain,	m_axis,0,ui->pGainE->text().toLocal8Bit());}
	void MainWindow::editingFinished_iGain(){	gimbalThread.edit(key_iGain,	m_axis,0,ui->iGainE->text().toLocal8Bit());}
	void MainWindow::editingFinished_dGain(){	gimbalThread.edit(key_dGain,	m_axis,0,ui->dGainE->text().toLocal8Bit());}
	void MainWindow::editingFinished_limit(){	gimbalThread.edit(key_limit,	m_axis,0,ui->limitE->text().toLocal8Bit());}
	void MainWindow::editingFinished_kT(){		gimbalThread.edit(key_kT,		m_axis,0,ui->kTE->text().toLocal8Bit());}
	void MainWindow::editingFinished_kC(){		gimbalThread.edit(key_kC,		m_axis,0,ui->kCE->text().toLocal8Bit());}
	void MainWindow::editingFinished_kP(){		gimbalThread.edit(key_kP,		m_axis,0,ui->kPE->text().toLocal8Bit());}
	void MainWindow::editingFinished_kFF(){		gimbalThread.edit(key_kFF,		m_axis,0,ui->kFFE->text().toLocal8Bit());}
	void MainWindow::editingFinished_kTach(){	gimbalThread.edit(key_kTach,	m_axis,0,ui->kTachE->text().toLocal8Bit());}
	void MainWindow::on_kIE_editingFinished(){	gimbalThread.edit(key_kI,		m_axis,0,ui->kIE->text().toLocal8Bit());}
	void MainWindow::on_kILimE_editingFinished(){gimbalThread.edit(key_kILim,	m_axis,0,ui->kILimE->text().toLocal8Bit());}

	void MainWindow::editingFinished_fFreq1(){	gimbalThread.edit(key_fFreq,	m_axis,0,ui->fFreqE->text().toLocal8Bit());}
	void MainWindow::editingFinished_fFreq2(){	gimbalThread.edit(key_fFreq,	m_axis,1,ui->fFreqE_2->text().toLocal8Bit());}
	void MainWindow::editingFinished_fFreq3(){	gimbalThread.edit(key_fFreq,	m_axis,2,ui->fFreqE_3->text().toLocal8Bit());}
	void MainWindow::editingFinished_fFreq4(){	gimbalThread.edit(key_fFreq,	m_axis,3,ui->fFreqE_4->text().toLocal8Bit());}
	void MainWindow::editingFinished_fFreq5(){	gimbalThread.edit(key_fFreq,	m_axis,4,ui->fFreqE_5->text().toLocal8Bit());}

	void MainWindow::editingFinished_fQ1(){		gimbalThread.edit(key_fQ,		m_axis,0,ui->fQE->text().toLocal8Bit());}
	void MainWindow::editingFinished_fQ2(){		gimbalThread.edit(key_fQ,		m_axis,1,ui->fQE_2->text().toLocal8Bit());}
	void MainWindow::editingFinished_fQ3(){		gimbalThread.edit(key_fQ,		m_axis,2,ui->fQE_3->text().toLocal8Bit());}
	void MainWindow::editingFinished_fQ4(){		gimbalThread.edit(key_fQ,		m_axis,3,ui->fQE_4->text().toLocal8Bit());}
	void MainWindow::editingFinished_fQ5(){		gimbalThread.edit(key_fQ,		m_axis,4,ui->fQE_5->text().toLocal8Bit());}

	void MainWindow::editingFinished_sinA0(){	gimbalThread.edit(key_sinA,		0,0,ui->sinA0E->text().toLocal8Bit());}
	void MainWindow::editingFinished_sinA1(){	gimbalThread.edit(key_sinA,		1,0,ui->sinA1E->text().toLocal8Bit());}
	void MainWindow::editingFinished_sinA2(){	gimbalThread.edit(key_sinA,		2,0,ui->sinA2E->text().toLocal8Bit());}
	void MainWindow::editingFinished_sinF0(){	gimbalThread.edit(key_sinF,		0,0,ui->sinF0E->text().toLocal8Bit());}
	void MainWindow::editingFinished_sinF1(){	gimbalThread.edit(key_sinF,		1,0,ui->sinF1E->text().toLocal8Bit());}
	void MainWindow::editingFinished_sinF2(){	gimbalThread.edit(key_sinF,		2,0,ui->sinF2E->text().toLocal8Bit());}

	void MainWindow::on_desiredPos0E_editingFinished(){	gimbalThread.edit(key_desiredPos,0,0,ui->desiredPos0E->text().toLocal8Bit());controlsEdited=1;}
	void MainWindow::on_desiredPos1E_editingFinished(){	gimbalThread.edit(key_desiredPos,1,0,ui->desiredPos1E->text().toLocal8Bit());controlsEdited=1;}
	void MainWindow::on_desiredPos2E_editingFinished(){	gimbalThread.edit(key_desiredPos,2,0,ui->desiredPos2E->text().toLocal8Bit());controlsEdited=1;}

	void MainWindow::on_desiredVel0E_editingFinished(){	gimbalThread.edit(key_desiredRate,0,0,ui->desiredVel0E->text().toLocal8Bit());controlsEdited=1;}
	void MainWindow::on_desiredVel1E_editingFinished(){	gimbalThread.edit(key_desiredRate,1,0,ui->desiredVel1E->text().toLocal8Bit());controlsEdited=1;}
	void MainWindow::on_desiredVel2E_editingFinished(){	gimbalThread.edit(key_desiredRate,2,0,ui->desiredVel2E->text().toLocal8Bit());controlsEdited=1;}

	void MainWindow::on_kPGE_editingFinished(){		gimbalThread.edit(key_geoKP,	m_axis,0,ui->kPGE->text().toLocal8Bit());}
	void MainWindow::on_kPG2E_editingFinished(){	gimbalThread.edit(key_geoKP2,	m_axis,0,ui->kPG2E->text().toLocal8Bit());}
	void MainWindow::on_kIGE_editingFinished(){		gimbalThread.edit(key_geoKI,	m_axis,0,ui->kIGE->text().toLocal8Bit());}
	void MainWindow::on_kDGE_editingFinished(){		gimbalThread.edit(key_geoKD,	m_axis,0,ui->kDGE->text().toLocal8Bit());}
	void MainWindow::on_kILimGE_editingFinished(){	gimbalThread.edit(key_geoKILim,	m_axis,0,ui->kILimGE->text().toLocal8Bit());}
	void MainWindow::on_kMysGE_editingFinished(){	gimbalThread.edit(key_geoKMys2,	m_axis,0,ui->kMysGE->text().toLocal8Bit());}

	void MainWindow::on_kPPE_editingFinished(){		gimbalThread.edit(key_posKP,	m_axis,0,ui->kPPE->text().toLocal8Bit());}
	void MainWindow::on_kPDE_editingFinished(){		gimbalThread.edit(key_posKD,	m_axis,0,ui->kPDE->text().toLocal8Bit());}
	void MainWindow::on_kPLimE_editingFinished(){	gimbalThread.edit(key_posKLim,	m_axis,0,ui->kPLimE->text().toLocal8Bit());}

//	----------------------------------------------------------------------------
//	lastTableMain
//	----------------------------------------------------------------------------

void MainWindow::lastTableMain()
{
	QString line;
	line.sprintf("all tables found");
	ui->textEdit->append(line);
	setupTree();
	readout();

	//	setWindowTitle(gimbalThread.wintitle());
}

//	----------------------------------------------------------------------------
//	displayError
//	----------------------------------------------------------------------------

void MainWindow::displayError(int type, int, const QString &message)
{
	static int latch = 0;
	//	QString line;
	//	line.sprintf("%i %i ",count,socketError);
	//	line += message;
	if(type==3){
		ui->label->setText(message);
	}else{
		qDebug()<<message;
		if(type==0)ui->textEdit->append(message);
		if(!latch){
			int tab = ui->tabWidget->currentIndex();
			QString tabText = ui->tabWidget->tabText(tab);
			if(tabText=="Comms")latch = 1;
		}
		if(latch)ui->textEdit_2->append(message);
	}
}

//	----------------------------------------------------------------------------
//	displayError
//	----------------------------------------------------------------------------

void MainWindow::errorString(QString message)
{
	static int latch = 0;

	ui->textEdit->append(message);
	if(!latch){
		int tab = ui->tabWidget->currentIndex();
		QString tabText = ui->tabWidget->tabText(tab);
		if(tabText=="Comms")latch = 1;
	}
	if(latch)ui->textEdit_2->append(message);
}

//	----------------------------------------------------------------------------
//	newGimAddress
//	----------------------------------------------------------------------------

void MainWindow::newGimAddress(QString gimAddress)
{
	if(gimAddress.contains(".")){
		guiGimConnect = CONNECT_LAN;
		guiGimIpAddress = gimAddress;
	}else if(gimAddress == "disconnect"){
		guiGimConnect = CONNECT_NONE;
		guiGimComPort = "disconnect";
	}else{
		guiGimConnect = CONNECT_232;
		guiGimComPort = gimAddress;
	}
	gimbalThread.settings(
				guiGimIpAddress.toLocal8Bit(),
				guiGimComPort.toLocal8Bit(),
				guiGimIpPort,
				guiGimBaud);
	emit connectToGimbal(guiGimConnect);
}

//	----------------------------------------------------------------------------
//	connectedState
//	----------------------------------------------------------------------------

void MainWindow::connectedState(int clr, QString lbl)
{
	QString iconstring;
	switch(clr){
	default:
	case 0:	iconstring = ":/icons/Led0.bmp"; break;
	case 1:	iconstring = ":/icons/LedG.bmp"; break;
	case 2:	iconstring = ":/icons/LedY.bmp"; break;
	case 3:	iconstring = ":/icons/LedR.bmp"; break;
	}
	QString labeltext;
	labeltext.sprintf("<img src=\"%s\"><strong>%s<\\strong>",iconstring.toStdString().data(),lbl.toStdString().data());
	ui->label_2->setTextFormat(Qt::RichText);
	ui->label_2->setText(labeltext);
	ui->pushButton_7->setText((clr==1)?"Disconnect":"Connect");
}

//	----------------------------------------------------------------------------
//	setupTree
//	----------------------------------------------------------------------------

void MainWindow::setupTree()
{
	//	QString deviceName;
	//	QString moduleName;
	QString tableName;
	QString rowName;
	QStandardItem *rootNode;
	//	QStandardItem *deviceItem;
	//	QStandardItem *moduleItem;
	QStandardItem *tableItem;
	QStandardItem *rowItem;

	if(treeModel != NULL){
		treeModel->deleteLater();
		treeModel = NULL;
	}
	treeModel = new QStandardItemModel;
	rootNode = treeModel->invisibleRootItem();

	//	for(int device=1; device<NUM_DEVICES; device++){
	//		if(!gimbalThread.dbase.devicePresent[device])continue;
	//		deviceName.sprintf("%i.",device);
	//		deviceItem = new QStandardItem(deviceName);
	//		rootNode->appendRow(deviceItem);
	//		for(int module=1; module<NUM_MODULES; module++){
	//			if(!gimbalThread.dbase.moduleExists(device,module))continue;
	//			moduleName.sprintf("%i.%i.",device,module);
	//			if(gimbalThread.dbase.numFields(device,module,255)){
	//				moduleName += " " ;
	//				moduleName += gimbalThread.dbase.dbtToString(device,module,255,2,1,0);
	//			}
	//			moduleItem = new QStandardItem(moduleName);
	//			deviceItem->appendRow(moduleItem);
	//			rootNode->appendRow(moduleItem);
	for(int table=0; table<MAXTABLES; table++){
		if(!gimbalThread.dbase.tableExists(table))continue;
		QString iconpath = tableTypeIconString(0,0,table);
		tableName.sprintf("%i. %s",table,gimbalThread.dbase.tableName(table));
		tableItem = new QStandardItem(QIcon(iconpath),tableName);
		rootNode->appendRow(tableItem);
		for(int row=0; row<MAXROWS; row++){
			if(!gimbalThread.dbase.rowExists(table,row))continue;
			int nc = gimbalThread.dbase.dbTableS[table]->nc;
			rowName = gimbalThread.dbase.rowName(table,row);
			for(int col=0;col<nc;col++){
				rowName += "\t";
				rowName += gimbalThread.dbase.dbToString(table,row,col);
			}
			rowItem = new QStandardItem(rowName);
			tableItem->appendRow(rowItem);
		}
	}
	//		}
	//	}
	ui->treeView->setModel(treeModel);
	if(0)ui->treeView->expandAll();
	QItemSelectionModel *selectionModel = ui->treeView->selectionModel();
	ui->treeView->setHeaderHidden(true);
	connect(selectionModel, SIGNAL(selectionChanged (const QItemSelection &, const QItemSelection &)),
			this, SLOT(selectionChangedSlot(const QItemSelection &, const QItemSelection &)));
	QList<int> sz = ui->splitter->sizes();
	sz[0]=540;
	//sz[1]=110;
	ui->splitter->setSizes(sz);
}

//	----------------------------------------------------------------------------
//	tableTypeIconString
//	----------------------------------------------------------------------------

const char *MainWindow::tableTypeIconString(int /*device*/,int /*module*/,int table)
{
	int tableType = gimbalThread.dbase.tableType(table);

	switch(tableType){
	case DB_RAM:	return ":/icons/DBT_RAM.bmp";
	case DB_FLASH:	return ":/icons/DBT_Flash.bmp";
	default:		return ":/icons/DBT_Instance.bmp";
	}
}

//	----------------------------------------------------------------------------
//	selectionChangedSlot
//	----------------------------------------------------------------------------

void MainWindow::selectionChangedSlot(const QItemSelection & /*newSelection*/, const QItemSelection & /*oldSelection*/)
{
	int table = 0;
	int module = 0;
	int device = 0;

	//get the text of the selected item
	const QModelIndex index = ui->treeView->selectionModel()->currentIndex();
	QString selectedText = index.data(Qt::DisplayRole).toString();
	//find out the hierarchy level of the selected item
	int hierarchyLevel=1;
	QModelIndex seekRoot = index;
	while(seekRoot.parent() != QModelIndex())
	{
		seekRoot = seekRoot.parent();
		hierarchyLevel++;
	}
	if(hierarchyLevel==3){
		QString s;
		QStringList l;
		s = index.data(Qt::DisplayRole).toString();
		l = s.split('.');
		table = l.first().toInt();
		s = index.parent().data(Qt::DisplayRole).toString();
		l = s.split('.');
		l.removeFirst();
		module = l.first().toInt();
		s = index.parent().parent().data(Qt::DisplayRole).toString();
		l = s.split('.');
		device = l.first().toInt();
		setupTableMain(device,module,table);
	}
}

//	----------------------------------------------------------------------------
//	setupTableMain
//	----------------------------------------------------------------------------

void MainWindow::setupTableMain(int /*device*/,int /*module*/,int /*table*/)
{
	//	if(!isOpen(device,module,table)){
	//		createTable(device,module,table);
	//	}

	//	QMdiSubWindow *tableViewSub;
	//	QTableView *tableView;
	//	dbTableModel *tableModel;
	//	if(isOpen(device,module,table,&tableViewSub,&tableView,&tableModel)){
	//		adjustTableSize(tableView,tableViewSub);
	//	}
}

//	----------------------------------------------------------------------------
//	newImuDataMain
//	----------------------------------------------------------------------------

void MainWindow::newImuDataMain()
{
	chart01->chartType = CHART_BODE_TOP;
	chart01->fftPoints(gimbalThread.imuBuf,IMU_BUF_COUNT,resetIMU);

	chart02->chartType = CHART_BODE_BOT;
	chart02->symmY = 1;
	float ymax1 = chart02->maxy;
	chart02->setPoints(gimbalThread.imuBuf,IMU_BUF_COUNT,resetIMU);
	float ymax2 = chart02->maxy;
	if(ymax1 != ymax2){
		char s[120];
		sprintf(s,"%1.4f",ymax2);
		ui->fftMax->setText(s);
	}

	resetIMU = 0;
}

//	----------------------------------------------------------------------------
//	newTuneDataMain
//	----------------------------------------------------------------------------

void MainWindow::newTuneDataMain()
{
	//	chart02->logscaleX = 1;
	chart01->newTuneState(m_axis,&gimbalThread.gimTun,&gimbalThread.refTun);
	chart02->newTuneState(m_axis,&gimbalThread.gimTun,&gimbalThread.refTun);
}
//float filtergains[NiAXES][NiFILT+1][FILTBODEMAX];
//float filterphase[NiAXES][NiFILT+1][FILTBODEMAX];
//float filterfreqs[NiAXES][NiFILT+1][FILTBODEMAX];

//	----------------------------------------------------------------------------
//	colorIndicator
//	----------------------------------------------------------------------------

QString colorIndicator(int clr,const char *lbl)
{
	QString iconstring;
	switch(clr){
	default:
	case 0:	iconstring = ":/icons/Led0.bmp"; break;
	case 1:	iconstring = ":/icons/LedG.bmp"; break;
	case 2:	iconstring = ":/icons/LedY.bmp"; break;
	case 3:	iconstring = ":/icons/LedR.bmp"; break;
	}
	QString labeltext;
	labeltext.sprintf("<img src=\"%s\"><strong>%s<\\strong>",iconstring.toStdString().data(),lbl);
	return labeltext;
}

//	----------------------------------------------------------------------------
//	readout
//	----------------------------------------------------------------------------

	float innerDrive[3];

void MainWindow::readout()
{

	int table = gimbalThread.stateAxisTbl;
	ui->jitterYR->setText(				gimbalThread.dbase.dbToString(table,8,0));
	ui->jitterPR->setText(				gimbalThread.dbase.dbToString(table,8,1));
	ui->jitterRR->setText(				gimbalThread.dbase.dbToString(table,8,2));

	table = gimbalThread.stateNavTbl;
	ui->curLat->setText(				gimbalThread.dbase.dbToString(table, 7,0));
	ui->curLon->setText(				gimbalThread.dbase.dbToString(table, 8,0));
	ui->curAlt->setText(				gimbalThread.dbase.dbToString(table, 9,0));
	ui->curYaw->setText(				gimbalThread.dbase.dbToString(table, 1,0));
	ui->curPit->setText(				gimbalThread.dbase.dbToString(table, 2,0));
	ui->curRol->setText(				gimbalThread.dbase.dbToString(table, 3,0));

	table = gimbalThread.stateGeoTbl;
	ui->targetLat->setText(				gimbalThread.dbase.dbToString(table, 3,0));
	ui->targetLon->setText(				gimbalThread.dbase.dbToString(table, 4,0));
	ui->targetAlt->setText(				gimbalThread.dbase.dbToString(table, 5,0));
	ui->passiveLat->setText(			gimbalThread.dbase.dbToString(table, 6,0));
	ui->passiveLon->setText(			gimbalThread.dbase.dbToString(table, 7,0));
	ui->passiveAlt->setText(			gimbalThread.dbase.dbToString(table, 8,0));
	ui->targetRange->setText(			gimbalThread.dbase.dbToString(table,15,0));

	char s[30];
	float v;

	float kid = (100.0f/0.23f);
	v=(controls.standby)?(0):(kid*innerDrive[0]);		sprintf(s,"%7.0f",v);ui->driveYR->setText(s);
	v=(controls.standby)?(0):(kid*innerDrive[1]);		sprintf(s,"%7.0f",v);ui->drivePR->setText(s);
	v=(controls.standby)?(0):(kid*innerDrive[2]);		sprintf(s,"%7.0f",v);ui->driveRR->setText(s);

	v=float(p1[0]);		sprintf(s,"%13.9f",v);ui->l1Lat->setText(s);
	v=float(p1[1]);		sprintf(s,"%13.9f",v);ui->l1Lon->setText(s);
	v=float(p1[2]);		sprintf(s, "%7.2f",v);ui->l1Alt->setText(s);
	v=l1[0];			sprintf(s, "%7.2f",v);ui->l1Yaw->setText(s);
	v=l1[1];			sprintf(s, "%7.2f",v);ui->l1Pit->setText(s);
	v=l1[2];			sprintf(s, "%7.2f",v);ui->l1Rol->setText(s);
	v=float(p2[0]);		sprintf(s,"%13.9f",v);ui->l2Lat->setText(s);
	v=float(p2[1]);		sprintf(s,"%13.9f",v);ui->l2Lon->setText(s);
	v=float(p2[2]);		sprintf(s, "%7.2f",v);ui->l2Alt->setText(s);
	v=l2[0];			sprintf(s, "%7.2f",v);ui->l2Yaw->setText(s);
	v=l2[1];			sprintf(s, "%7.2f",v);ui->l2Pit->setText(s);
	v=l2[2];			sprintf(s, "%7.2f",v);ui->l2Rol->setText(s);
	v=float(t[0]);		sprintf(s,"%13.9f",v);ui->intersectLat->setText(s);
	v=float(t[1]);		sprintf(s,"%13.9f",v);ui->intersectLon->setText(s);
	v=float(t[2]);		sprintf(s, "%7.2f",v);ui->intersectAlt->setText(s);

	ui->label_5->setTextFormat(Qt::RichText);
	ui->label_6->setTextFormat(Qt::RichText);
	ui->label_16->setTextFormat(Qt::RichText);
	if(gimbalThread.state.nav.TimeGps == (uint64_t)0){
		ui->label_5->setText(colorIndicator(0,"No INS"));
		ui->label_6->setText(colorIndicator(0,"No INS"));
		ui->label_16->setText(colorIndicator(3,"No INS"));
	}else{
		int ii = gimbalThread.state.nav.InsStatus;
		switch(ii&0x03){
		case 0: ui->label_5->setText(colorIndicator(3,"Not tracking")); break;
		case 1: ui->label_5->setText(colorIndicator(2,"Aligning (2deg)")); break;
		case 2: ui->label_5->setText(colorIndicator(1,"Tracking")); break;
		case 3: ui->label_5->setText(colorIndicator(3,"Loss of GPS")); break;
		default: ui->label_5->setText(colorIndicator(0,"")); break;
		};

		switch(ii&0x04){
		case 0x04: ui->label_6->setText(colorIndicator(1,"GpsFix")); break;
		default: ui->label_6->setText(colorIndicator(3,"GpsNoFix")); break;
		};

		switch(ii&0x78){
		case 0x00: ui->label_16->setText(colorIndicator(1,"No Errors")); break;
		case 0x08: ui->label_16->setText(colorIndicator(3,"Reserved Error")); break;
		case 0x10: ui->label_16->setText(colorIndicator(3,"IMU Error")); break;
		case 0x20: ui->label_16->setText(colorIndicator(3,"Mag/Pres Error")); break;
		case 0x40: ui->label_16->setText(colorIndicator(3,"GPS Error")); break;
		default: ui->label_16->setText(colorIndicator(3,"Multiple Errors")); break;
		};
	}

	ui->label_17->setText(colorIndicator(autotarget?1:0,""));
	/*
QString iconstring;
switch(clr){
default:
case 0:	iconstring = ":/icons/Led0.bmp"; break;
case 1:	iconstring = ":/icons/LedG.bmp"; break;
case 2:	iconstring = ":/icons/LedY.bmp"; break;
case 3:	iconstring = ":/icons/LedR.bmp"; break;
}
QString labeltext;
labeltext.sprintf("<img src=\"%s\"><strong>%s<\\strong>",iconstring.toStdString().data(),lbl.toStdString().data());
*/
	/*
0x03
Not tracking
Aligning (2deg)
Tracking
Loss of GPS

0x04 GpsFix
0x08 Reserved Error
0x10 IMU Error
0x20 Mag/Pres Error
0x40 GPS Error
*/
	ranges();
	int itarg = 0;
	ui->targetLabel_1->setText(targetString(itarg++));
	ui->targetLabel_2->setText(targetString(itarg++));
	ui->targetLabel_3->setText(targetString(itarg++));
	ui->targetLabel_4->setText(targetString(itarg++));
	ui->targetLabel_5->setText(targetString(itarg++));
	ui->targetLabel_6->setText(targetString(itarg++));
	ui->targetLabel_7->setText(targetString(itarg++));
	ui->targetLabel_8->setText(targetString(itarg++));
	ui->targetLabel_9->setText(targetString(itarg++));
	ui->targetLabel_10->setText(targetString(itarg++));
	ui->targetLabel_11->setText(targetString(itarg++));
	ui->targetLabel_12->setText(targetString(itarg++));
	ui->targetLabel_13->setText(targetString(itarg++));
	itarg = 0;
	ui->rangeLabel_1->setText(rangeString(itarg++));
	ui->rangeLabel_2->setText(rangeString(itarg++));
	ui->rangeLabel_3->setText(rangeString(itarg++));
	ui->rangeLabel_4->setText(rangeString(itarg++));
	ui->rangeLabel_5->setText(rangeString(itarg++));
	ui->rangeLabel_6->setText(rangeString(itarg++));
	ui->rangeLabel_7->setText(rangeString(itarg++));
	ui->rangeLabel_8->setText(rangeString(itarg++));
	ui->rangeLabel_9->setText(rangeString(itarg++));
	ui->rangeLabel_10->setText(rangeString(itarg++));
	ui->rangeLabel_11->setText(rangeString(itarg++));
	ui->rangeLabel_12->setText(rangeString(itarg++));
	ui->rangeLabel_13->setText(rangeString(itarg++));

	if(controls.standby){
		ui->radioButton_Standby->setChecked(true);
		ui->radioButton_Rate->setChecked(false);
		ui->radioButton_Slave->setChecked(false);
		ui->radioButton_Geo->setChecked(false);
		ui->radioButton_Outer->setChecked(false);
		ui->radioButton_Stow->setChecked(false);
	}else{
		ui->radioButton_Standby->setChecked(false);
		ui->radioButton_Rate->setChecked(controls.mode == MODE_RATE);
		ui->radioButton_Slave->setChecked(controls.mode == MODE_SLAVE);
		ui->radioButton_Geo->setChecked(controls.mode == MODE_GEO);
		ui->radioButton_Outer->setChecked(controls.mode == MODE_OUTER);
		ui->radioButton_Stow->setChecked(controls.mode == MODE_STOW);
	}

	static int oldModeX = gimReadouts.modeX;
	if(oldModeX != gimReadouts.modeX){
		oldModeX = gimReadouts.modeX;
		refreshPar = 1;
	}

	readoutPar();
	readoutControls();
}

//	----------------------------------------------------------------------------
//	readoutPar
//	----------------------------------------------------------------------------

void MainWindow::readoutPar()
{
	if(!refreshPar)return;
	refreshPar = 0;
	refreshControls = 1;

	int table = gimbalThread.pidPTbl;
	int col = m_axis;
	int ipid = pidType(controls.mode);
	ui->pGainE->setText(	gimbalThread.dbase.dbToString(table,0*NiPID+ipid,col));
	ui->iGainE->setText(	gimbalThread.dbase.dbToString(table,1*NiPID+ipid,col));
	ui->dGainE->setText(	gimbalThread.dbase.dbToString(table,2*NiPID+ipid,col));
	ui->limitE->setText(	gimbalThread.dbase.dbToString(table,3*NiPID+ipid,col));
	ui->kTE->setText(		gimbalThread.dbase.dbToString(table,4*NiPID+0,col));
	ui->kCE->setText(		gimbalThread.dbase.dbToString(table,4*NiPID+1,col));

	table = gimbalThread.refPidPTbl;
	col = m_axis;
	ui->pGainR->setText(	gimbalThread.dbase.dbToString(table,0*NiPID+ipid,col));
	ui->iGainR->setText(	gimbalThread.dbase.dbToString(table,1*NiPID+ipid,col));
	ui->dGainR->setText(	gimbalThread.dbase.dbToString(table,2*NiPID+ipid,col));
	ui->limitR->setText(	gimbalThread.dbase.dbToString(table,3*NiPID+ipid,col));
	ui->kTR->setText(		gimbalThread.dbase.dbToString(table,4*NiPID+0,col));
	ui->kCR->setText(		gimbalThread.dbase.dbToString(table,4*NiPID+1,col));

	table = gimbalThread.refOuterPTbl;
	col = m_axis;
	ui->kPR->setText(		gimbalThread.dbase.dbToString(table,0,col));
	ui->kFFR->setText(		gimbalThread.dbase.dbToString(table,1,col));
	ui->kTachR->setText(	gimbalThread.dbase.dbToString(table,2,col));
	ui->kIR->setText(		gimbalThread.dbase.dbToString(table,3,col));
	ui->kILimR->setText(	gimbalThread.dbase.dbToString(table,4,col));

	table = gimbalThread.geoPTbl;
	col = m_axis;
	ui->kPGE->setText(					gimbalThread.dbase.dbToString(table,0,col));
	ui->kPG2E->setText(					gimbalThread.dbase.dbToString(table,1,col));
	ui->kIGE->setText(					gimbalThread.dbase.dbToString(table,2,col));
	ui->kDGE->setText(					gimbalThread.dbase.dbToString(table,3,col));
	ui->kILimGE->setText(				gimbalThread.dbase.dbToString(table,4,col));
	ui->kMysGE->setText(				gimbalThread.dbase.dbToString(table,5,col));

	table = gimbalThread.refGeoPTbl;
	col = m_axis;
	ui->kPGR->setText(					gimbalThread.dbase.dbToString(table,0,col));
	ui->kPG2R->setText(					gimbalThread.dbase.dbToString(table,1,col));
	ui->kIGR->setText(					gimbalThread.dbase.dbToString(table,2,col));
	ui->kDGR->setText(					gimbalThread.dbase.dbToString(table,3,col));
	ui->kILimGR->setText(				gimbalThread.dbase.dbToString(table,4,col));
	ui->kMysGR->setText(				gimbalThread.dbase.dbToString(table,5,col));

	table = gimbalThread.posPTbl;
	col = m_axis;
	ui->kPPE->setText(					gimbalThread.dbase.dbToString(table,0,col));
	ui->kPDE->setText(					gimbalThread.dbase.dbToString(table,1,col));
	ui->kPLimE->setText(				gimbalThread.dbase.dbToString(table,2,col));

	table = gimbalThread.refPosPTbl;
	col = m_axis;
	ui->kPPR->setText(					gimbalThread.dbase.dbToString(table,0,col));
	ui->kPDR->setText(					gimbalThread.dbase.dbToString(table,1,col));
	ui->kPLimR->setText(				gimbalThread.dbase.dbToString(table,2,col));

	switch(m_axis){
	case 0: table = gimbalThread.filtP0Tbl; break;
	case 1: table = gimbalThread.filtP1Tbl; break;
	case 2: table = gimbalThread.filtP2Tbl; break;
	default: table = -1; break;
	}

	ui->fTypeC->setCurrentIndex(	gimbalThread.dbase.dbToInt(table,0,0));
	ui->fTypeC_2->setCurrentIndex(	gimbalThread.dbase.dbToInt(table,0,1));
	ui->fTypeC_3->setCurrentIndex(	gimbalThread.dbase.dbToInt(table,0,2));
	ui->fTypeC_4->setCurrentIndex(	gimbalThread.dbase.dbToInt(table,0,3));
	ui->fTypeC_5->setCurrentIndex(	gimbalThread.dbase.dbToInt(table,0,4));
	ui->fFreqE->setText(			gimbalThread.dbase.dbToString(table,1,0));
	ui->fFreqE_2->setText(			gimbalThread.dbase.dbToString(table,1,1));
	ui->fFreqE_3->setText(			gimbalThread.dbase.dbToString(table,1,2));
	ui->fFreqE_4->setText(			gimbalThread.dbase.dbToString(table,1,3));
	ui->fFreqE_5->setText(			gimbalThread.dbase.dbToString(table,1,4));
	ui->fQE->setText(				gimbalThread.dbase.dbToString(table,2,0));
	ui->fQE_2->setText(				gimbalThread.dbase.dbToString(table,2,1));
	ui->fQE_3->setText(				gimbalThread.dbase.dbToString(table,2,2));
	ui->fQE_4->setText(				gimbalThread.dbase.dbToString(table,2,3));
	ui->fQE_5->setText(				gimbalThread.dbase.dbToString(table,2,4));

	switch(m_axis){
	case 0: table = gimbalThread.refFiltP0Tbl; break;
	case 1: table = gimbalThread.refFiltP1Tbl; break;
	case 2: table = gimbalThread.refFiltP2Tbl; break;
	default: table = -1; break;
	}

	ui->fTypeR->setText(			gimbalThread.dbase.dbToString(table,0,0));
	ui->fTypeR_2->setText(			gimbalThread.dbase.dbToString(table,0,1));
	ui->fTypeR_3->setText(			gimbalThread.dbase.dbToString(table,0,2));
	ui->fTypeR_4->setText(			gimbalThread.dbase.dbToString(table,0,3));
	ui->fTypeR_5->setText(			gimbalThread.dbase.dbToString(table,0,4));
	ui->fFreqR->setText(			gimbalThread.dbase.dbToString(table,1,0));
	ui->fFreqR_2->setText(			gimbalThread.dbase.dbToString(table,1,1));
	ui->fFreqR_3->setText(			gimbalThread.dbase.dbToString(table,1,2));
	ui->fFreqR_4->setText(			gimbalThread.dbase.dbToString(table,1,3));
	ui->fFreqR_5->setText(			gimbalThread.dbase.dbToString(table,1,4));
	ui->fQR->setText(				gimbalThread.dbase.dbToString(table,2,0));
	ui->fQR_2->setText(				gimbalThread.dbase.dbToString(table,2,1));
	ui->fQR_3->setText(				gimbalThread.dbase.dbToString(table,2,2));
	ui->fQR_4->setText(				gimbalThread.dbase.dbToString(table,2,3));
	ui->fQR_5->setText(				gimbalThread.dbase.dbToString(table,2,4));

	table = gimbalThread.outerPTbl;
	col = m_axis;
	ui->kPE->setText(					gimbalThread.dbase.dbToString(table,0,col));
	ui->kFFE->setText(					gimbalThread.dbase.dbToString(table,1,col));
	ui->kTachE->setText(				gimbalThread.dbase.dbToString(table,2,col));
	ui->kIE->setText(					gimbalThread.dbase.dbToString(table,3,col));
	ui->kILimE->setText(				gimbalThread.dbase.dbToString(table,4,col));

	table = gimbalThread.geoPTbl;
	col = m_axis;
	ui->kPGE->setText(					gimbalThread.dbase.dbToString(table,0,col));
	ui->kPG2E->setText(					gimbalThread.dbase.dbToString(table,1,col));
	ui->kIGE->setText(					gimbalThread.dbase.dbToString(table,2,col));
	ui->kDGE->setText(					gimbalThread.dbase.dbToString(table,3,col));
	ui->kILimGE->setText(				gimbalThread.dbase.dbToString(table,4,col));
	ui->kMysGE->setText(				gimbalThread.dbase.dbToString(table,5,col));

	table = gimbalThread.posPTbl;
	col = m_axis;
	ui->kPPE->setText(					gimbalThread.dbase.dbToString(table,0,col));
	ui->kPDE->setText(					gimbalThread.dbase.dbToString(table,1,col));
	ui->kPLimE->setText(				gimbalThread.dbase.dbToString(table,2,col));

}

//	----------------------------------------------------------------------------
//	readoutControls
//	----------------------------------------------------------------------------

void MainWindow::readoutControls()
{
	if(!refreshControls)return;
	refreshControls = 0;

	int table = gimbalThread.controlsTbl;
	ui->plotVarC->setCurrentIndex(		gimbalThread.dbase.dbToInt(table,4,0));
	ui->plotAxisC->setCurrentIndex(		gimbalThread.dbase.dbToInt(table,5,0));
	ui->plotRawCB->setChecked((			gimbalThread.dbase.dbToInt(table,6,0))?(true):(false));

	table = gimbalThread.controlsAxisTbl;
	ui->enable0CB->setChecked((			gimbalThread.dbase.dbToInt(table,0,0))?(true):(false));
	ui->enable1CB->setChecked((			gimbalThread.dbase.dbToInt(table,0,1))?(true):(false));
	ui->enable2CB->setChecked((			gimbalThread.dbase.dbToInt(table,0,2))?(true):(false));
	ui->enableO0CB->setChecked((		gimbalThread.dbase.dbToInt(table,1,0))?(true):(false));
	ui->enableO1CB->setChecked((		gimbalThread.dbase.dbToInt(table,1,1))?(true):(false));
	ui->remote0CB->setChecked((			gimbalThread.dbase.dbToInt(table,2,0))?(true):(false));
	ui->remote1CB->setChecked((			gimbalThread.dbase.dbToInt(table,2,1))?(true):(false));
	ui->remote2CB->setChecked((			gimbalThread.dbase.dbToInt(table,2,2))?(true):(false));
	ui->backdrive0C->setCurrentIndex(	gimbalThread.dbase.dbToInt(table,3,0));
	ui->backdrive1C->setCurrentIndex(	gimbalThread.dbase.dbToInt(table,3,1));
	ui->backdrive2C->setCurrentIndex(	gimbalThread.dbase.dbToInt(table,3,2));
	ui->sinA0E->setText(				gimbalThread.dbase.dbToString(table,7,0));
	ui->sinA1E->setText(				gimbalThread.dbase.dbToString(table,7,1));
	ui->sinA2E->setText(				gimbalThread.dbase.dbToString(table,7,2));
	ui->sinF0E->setText(				gimbalThread.dbase.dbToString(table,8,0));
	ui->sinF1E->setText(				gimbalThread.dbase.dbToString(table,8,1));
	ui->sinF2E->setText(				gimbalThread.dbase.dbToString(table,8,2));
	ui->desiredPos0E->setText(			gimbalThread.dbase.dbToString(table,9,0));
	ui->desiredPos1E->setText(			gimbalThread.dbase.dbToString(table,9,1));
	ui->desiredPos2E->setText(			gimbalThread.dbase.dbToString(table,9,2));
	ui->desiredVel0E->setText(			gimbalThread.dbase.dbToString(table,6,0));
	ui->desiredVel1E->setText(			gimbalThread.dbase.dbToString(table,6,1));
	ui->desiredVel2E->setText(			gimbalThread.dbase.dbToString(table,6,2));

	if(controls.standby){
		ui->radioButton_Standby->setChecked(true);
		ui->radioButton_Rate->setChecked(false);
		ui->radioButton_Slave->setChecked(false);
		ui->radioButton_Geo->setChecked(false);
		ui->radioButton_Outer->setChecked(false);
	}else{
		ui->radioButton_Standby->setChecked(false);
		ui->radioButton_Rate->setChecked(controls.mode == MODE_RATE);
		ui->radioButton_Slave->setChecked(controls.mode == MODE_SLAVE);
		ui->radioButton_Geo->setChecked(controls.mode == MODE_GEO);
		ui->radioButton_Outer->setChecked(controls.mode == MODE_OUTER);
	}

	float v;
	char s[80];
	v=float(steeringOut.sim.lla1[0]);		sprintf(s,"%13.9f",v);ui->waypoint1_Lat->setText(s);
	v=float(steeringOut.sim.lla1[1]);		sprintf(s,"%13.9f",v);ui->waypoint1_Lon->setText(s);
	v=float(steeringOut.sim.lla1[2]);		sprintf(s,"%7.2f" ,v);ui->waypoint1_Alt->setText(s);
	v=float(steeringOut.sim.lla2[0]);		sprintf(s,"%13.9f",v);ui->waypoint2_Lat->setText(s);
	v=float(steeringOut.sim.lla2[1]);		sprintf(s,"%13.9f",v);ui->waypoint2_Lon->setText(s);
	v=float(steeringOut.sim.lla2[2]);		sprintf(s,"%7.2f" ,v);ui->waypoint2_Alt->setText(s);
	v=float(steeringOut.sim.speed);			sprintf(s,"%7.2f" ,v);ui->simSpeed->setText(s);

}

//	----------------------------------------------------------------------------
//	macros
//	----------------------------------------------------------------------------

#define _max(a,b)  (((a) > (b)) ? (a) : (b))
#define _min(a,b)  (((a) < (b)) ? (a) : (b))

//	----------------------------------------------------------------------------
//	timeStats2
//	----------------------------------------------------------------------------

inline void timeStats2()
{
	qint64 tnew = mainTime.elapsed();
	static qint64 tOld = tnew;
	util.onTimer2 = tnew - tOld;
	tOld = mainTime.elapsed();
}

//	----------------------------------------------------------------------------
//	onTimer2
//	----------------------------------------------------------------------------

void MainWindow::onTimer2()
{
	timeStats2();

	if(consoleIO == NULL){
		util.rxConsInputFrame = 0;
	}else{
		util.rxConsInputFrame = (consoleIO->receiving) ? (consoleIO->frametime):(0);
	}
}

//	----------------------------------------------------------------------------
//	steering
//	----------------------------------------------------------------------------

double targetM[3];

//	----------------------------------------------------------------------------
//	lensFraction
//	----------------------------------------------------------------------------

float lensFraction(float pos){
	float p = pos/200.0f + 0.5f;
	p = _max(0.0f,_min(1.0f,p));
	return p;
}

//	----------------------------------------------------------------------------
//	digitalReadByte
//	----------------------------------------------------------------------------

#ifndef Q_OS_LINUX
int digitalReadByte(){return 0;}
#endif

//	----------------------------------------------------------------------------
//	opReadouts
//	----------------------------------------------------------------------------

extern gimReadouts_t gimReadouts;

void MainWindow::opReadouts()
{
	gimReadouts.yaw = gimbalThread.state.axis[0].angle.val + gimbalThread.state.axis[0].enc.val;
	gimReadouts.pitch = gimbalThread.state.axis[1].pos;
	gimReadouts.roll = gimbalThread.state.axis[2].pos;
	gimReadouts.wrap = gimReadouts.yaw;
	gimReadouts.losPan = gimReadouts.yaw;
	gimReadouts.focus = lensFraction(gimbalThread.state.lens.lensState[0].pos);
	gimReadouts.iris  = lensFraction(gimbalThread.state.lens.lensState[1].pos);
	gimReadouts.zoom  = lensFraction(gimbalThread.state.lens.lensState[2].pos);
	gimReadouts.losTilt = gimReadouts.pitch;
	gimReadouts.buttons = digitalReadByte();
	gimReadouts.mode = controls.mode;
	gimReadouts.modeX = (gimReadouts.standby)?(MODEX_STANDBY):(gimReadouts.mode);
	gimReadouts.standby = controls.standby;

	if(gimbalThread.state.geo.cameraL[0]){
		gimReadouts.lat = gimbalThread.state.geo.cameraL[0];
		gimReadouts.lon = gimbalThread.state.geo.cameraL[1];
		gimReadouts.alt = gimbalThread.state.geo.cameraL[2];
	}else{
		gimReadouts.lat = 43.30724669;
		gimReadouts.lon = -80.13217796;
		gimReadouts.alt = 400;
	}
	gimReadouts.passiveLat = gimbalThread.state.geo.passiveL[0];
	gimReadouts.passiveLon = gimbalThread.state.geo.passiveL[1];
	gimReadouts.rangeP = gimbalThread.state.geo.rangeP;

	gimReadouts.targetLat = gimbalThread.state.geo.targetL[0];
	gimReadouts.targetLon = gimbalThread.state.geo.targetL[1];
	gimReadouts.targetAlt = gimbalThread.state.geo.targetL[2];

	gimReadouts.ypr[0] = gimbalThread.state.geo.ypr[0];
	gimReadouts.ypr[1] = gimbalThread.state.geo.ypr[1];
	gimReadouts.ypr[2] = gimbalThread.state.geo.ypr[2];

	gimReadouts.yprA[0] = gimbalThread.state.nav.Ypr[0];
	gimReadouts.yprA[1] = gimbalThread.state.nav.Ypr[1];
	gimReadouts.yprA[2] = gimbalThread.state.nav.Ypr[2];

	gimReadouts.insStatus = gimbalThread.state.nav.InsStatus;
	gimReadouts.vnFreq = gimbalThread.state.comm[0].vn200;

	latLonToMeters(gimReadouts.lat,gimReadouts.lon,&gimReadouts.xyCamera[0],&gimReadouts.xyCamera[1]);
	latLonToMeters(gimReadouts.targetLat,gimReadouts.targetLon,&gimReadouts.xyTarget[0],&gimReadouts.xyTarget[1]);
	double dx = gimReadouts.xyCamera[0] - gimReadouts.xyTarget[0];
	double dy = gimReadouts.xyCamera[1] - gimReadouts.xyTarget[1];
	double dz = gimReadouts.alt - gimReadouts.targetAlt;
	gimReadouts.range = sqrt( dx*dx + dy+dy + dz*dz );
	gimReadouts.dxTarget = dx;
	gimReadouts.dyTarget = dy;
	gimReadouts.dzTarget = dz;

	gimReadouts.simOn = steeringOut.sim.mode != SIM_MODE_OFF;

	gimReadouts.angleDif[0] =  gimbalThread.state.axis[0].outerAngleDif;
	gimReadouts.angleDif[1] =  gimbalThread.state.axis[1].outerAngleDif;
	gimReadouts.angleDif[2] =  gimbalThread.state.axis[2].outerAngleDif;

	gimbalThread.dbase.newTableData(gimbalThread.gimReadoutsTbl);
}

//	----------------------------------------------------------------------------
//	opGUI
//	----------------------------------------------------------------------------

void MainWindow::opGUI()
{
	opReadouts();
#ifdef _OPERATOR_DIALOG
	opDlg->timed();
#endif
}

//	----------------------------------------------------------------------------
//	timeStats1
//	----------------------------------------------------------------------------

inline void timeStats1()
{
	qint64 tnew = mainTime.elapsed();
	static qint64 tOld = tnew;
	util.onTimer = tnew - tOld;
	tOld = mainTime.elapsed();
}

//	----------------------------------------------------------------------------
//	onTimer
//	----------------------------------------------------------------------------

void MainWindow::onTimer()
{
	timeStats1();

	static int index = 0;
	switch(index++){
	case 0:		updateTree();	break;
	case 1:		readout();		break;
	case 2:		opGUI();		break;
	default:	index = 0;		break;
	}

	if(newOperatorSettingsX){
		gimbalThread.dbase.newTableData(gimbalThread.operSettingsTbl);
		newOperatorSettingsX = 0;
	}
}

//	----------------------------------------------------------------------------
//	updateChart
//	----------------------------------------------------------------------------

void MainWindow::updateChart(int table,int row)
{
	charttable1 = gimbalThread.stateAxisTbl;	chartcol1 = 0;	chartrow1 = 1;
	charttable2 = gimbalThread.lensFeedbackTbl;	chartcol2 = 1;	chartrow2 = 1;
	charttable3 = gimbalThread.lensFeedbackTbl;	chartcol3 = 1;	chartrow3 = 2;

	clock_t t0 = clock();
	static clock_t t1 = t0;
	static double t = 0;
	t += (t0-t1)*(1.0/CLOCKS_PER_SEC);
	t1 = t0;

	if((table == charttable1)&&(row == chartrow1)){
		chart04->addPoint(0,t,gimbalThread.dbase.dbToFloat(table,row,chartcol1),resetChart1);
		resetChart1 = 0;
	}

	if((table == charttable2)&&(row == chartrow2)){
		chart05->addPoint(0,t,gimbalThread.dbase.dbToFloat(table,row,chartcol2),resetChart2);
		resetChart2 = 0;
	}

	if((table == charttable3)&&(row == chartrow3)){
		chart06->addPoint(0,t,gimbalThread.dbase.dbToFloat(table,row,chartcol3),resetChart3);
		resetChart3 = 0;
	}
}

//	----------------------------------------------------------------------------
//	updateTree
//	----------------------------------------------------------------------------

void MainWindow::updateTree()
{

	for(int table=0; table<MAXTABLES; table++){
		if(!gimbalThread.dbase.tableExists(table))break;
		if(!gimbalThread.dbase.tableRefresh(table))continue;
		if(gimbalThread.dbase.tableType(table)==DB_FLASH)refreshPar = 1;
		if(table == gimbalThread.controlsTbl){
			refreshControls = controlsEdited;
			controlsEdited = 0;
		}
		QModelIndex t = treeModel->index(table,0);
		for(int row=0; row<MAXROWS; row++){
			updateChart(table,row);
			if(!gimbalThread.dbase.rowExists(table,row))break;
			int nc = gimbalThread.dbase.dbTableS[table]->nc;
			QString rowName = gimbalThread.dbase.rowName(table,row);
			for(int col=0;col<nc;col++){
				rowName += "\t";
				rowName += gimbalThread.dbase.dbToString(table,row,col);
			}
			QModelIndex r = treeModel->index(row,0,t);
			treeModel->setData(r,rowName,Qt::DisplayRole);
		}
	}
}

//	----------------------------------------------------------------------------
//	newTableDataMain
//	----------------------------------------------------------------------------

void MainWindow::newTableDataMain(int /*cmd*/)
{
}

//	----------------------------------------------------------------------------
//	jsReadingMain
//	----------------------------------------------------------------------------

void MainWindow::jsReadingMain(int /*time*/, int value, int type, int number)
{
	const float k = 1.0f/32767;

	switch(type){

	case JS_EVENT_AXIS:
		switch(number){
		case 0:
			joystickT.jsTilt = k*value;
			break;
		case 1:
			joystickT.jsPan = -k*value;
			break;
		case 2:
			joystickT.jsRoll = k*value;
			break;
		}
		break;

	case JS_EVENT_BUTTON:
		switch(number){
		case 0:
			joystickT.button2 = value;
			break;
		case 1:
			joystickT.button1 = value;
			break;
		}
		break;

	}
	gimbalThread.dbase.newTableData(gimbalThread.joystickTbl);
}

//	----------------------------------------------------------------------------
//	on_MainWindow_destroyed
//	----------------------------------------------------------------------------

void MainWindow::on_MainWindow_destroyed()
{
	//	gimbalThread.quit = 1;
	//joystickThread.quit = 1;
	//opDlg->deleteLater();
}

//	----------------------------------------------------------------------------
//	closing
//	----------------------------------------------------------------------------

void MainWindow::closing()
{
	timer->stop();
	emit mainStop();
#ifdef _OPERATOR_DIALOG
	opDlg->deleteLater();
#endif
}

//	----------------------------------------------------------------------------
//	Worker
//	----------------------------------------------------------------------------

Worker::Worker()
{
	// NEVER allocate heap objects (using new) in the constructor

	init = 0;
	quit = 0;
	cons = NULL;
	numtosend = 0;
	msgtosend = NULL;
	portname = "serial0";
	baudrate = 115200;
	receiving = 0;
	frametime = 0;
	tLastGood = 0;
}

//	----------------------------------------------------------------------------
//	Worker
//	----------------------------------------------------------------------------

Worker::~Worker()
{
}

//	----------------------------------------------------------------------------
//	transmit
//	----------------------------------------------------------------------------

void Worker::transmit(unsigned char *message, int len)
{
	msgLensCommand_t *m = (msgLensCommand_t*)message;
	m->tail.checksum++;
	cons->writePort(message, len);
}
//	----------------------------------------------------------------------------
//	Sleep
//	----------------------------------------------------------------------------

void _sleep(int millisec){
	usleep(millisec*1000);
}

//	----------------------------------------------------------------------------
//	timeStats3
//	----------------------------------------------------------------------------
/*
inline void timeStats3()
{
qint64 tnew = mainTime.elapsed();
static qint64 tOld = tnew;
util.rxConsInputFrame = tnew - tOld;
tOld = mainTime.elapsed();
}
*/
//	----------------------------------------------------------------------------
//	Worker
//	----------------------------------------------------------------------------

void Worker::process()
{
	QString resstring;
	QSerialPort *serial = new QSerialPort;
	cons = new comm(NULL,serial);
	int res = cons->openPort(portname.toLocal8Bit(),baudrate,0);
	if(res){
		resstring = "Open "+ portname + " fail, quitting thread";
		//		quit = 1;
	}else{
		resstring = "Open "+ portname + " succeeded";
	}
	qDebug()<<resstring;
	emit error(resstring);

	while(!quit){
		int numread = cons->getMessage();
		if(numread){
			emit newMsg(cons->message,numread);
			qint64 tnew = mainTime.elapsed();
			frametime = tnew - tLastGood;
			tLastGood = tnew;
			receiving = 1;
			//			tOld = mainTime.elapsed();
			//			timeStats3();
		}
		if(receiving){
			qint64 dt = mainTime.elapsed() - tLastGood;
			if(dt > 20000){
				receiving = 0;
			}
		}
		if(numtosend){
			transmit(msgtosend,numtosend);
			numtosend = 0;
		}

	}
	cons->closePort();

	emit finished();
}

//	----------------------------------------------------------------------------
//	startConsoleIoThread
//	----------------------------------------------------------------------------

Worker* MainWindow::startConsoleIoThread(const char *portname,int baudrate)
{
	QThread* thread = new QThread;
	Worker* worker = new Worker;
	worker->moveToThread(thread);
	connect(worker, SIGNAL (error(QString)), this, SLOT (errorString(QString)));
	connect(thread, SIGNAL (started()), worker, SLOT (process()));
	connect(worker, SIGNAL (finished()), thread, SLOT (quit()));
	connect(worker, SIGNAL (finished()), worker, SLOT (deleteLater()));
	connect(thread, SIGNAL (finished()), thread, SLOT (deleteLater()));
	connect(worker, SIGNAL (newMsg(unsigned char *,int)), &gimbalThread, SLOT (newMsg(unsigned char *,int)));
	worker->portname = portname;
	worker->baudrate = baudrate;
	thread->start();
	return worker;
}

//	----------------------------------------------------------------------------
//	GpsThread
//	----------------------------------------------------------------------------

GpsThread::GpsThread()
{
	// NEVER allocate heap objects (using new) in the constructor

	init = 0;
	quit = 0;
	mstate = 0;
	index = 0;
	memset(message,0,sizeof(message));
	gpscomm = NULL;
	numtosend = 0;
	msgtosend = NULL;
#ifdef GPS_PORT
	portname = GPS_PORT;
#else
	portname = "";
#endif

	baudrate = 115200;
	latitude = 0;
	longitude = 0;
	altitude = 0;
}

//	----------------------------------------------------------------------------
//	GpsThread
//	----------------------------------------------------------------------------

GpsThread::~GpsThread()
{
}

//	----------------------------------------------------------------------------
//	transmit
//	----------------------------------------------------------------------------

void GpsThread::transmit(unsigned char *message, int len)
{
	gpscomm->writePort(message, len);
}

//	----------------------------------------------------------------------------
//	GpsThread
//	----------------------------------------------------------------------------

#define ptrToDouble(ptr) *(double *) (ptr)

void GpsThread::process()
{
	const char header[3]={(char)0xaa,0x44,0x12};
	int headerLength;
	int length;
	int totalLength;

	QString resstring;
	QSerialPort *serial = new QSerialPort;
	gpscomm = new comm(NULL,serial);
	int res = gpscomm->openPort(portname.toLocal8Bit(),baudrate,0);
	if(res){
		resstring = "Open "+ portname + " fail, looking for uBlox";
		qDebug()<<resstring;
		emit error(resstring);
		resstring = "No uBlox found, quitting thread";
		foreach (const QSerialPortInfo &info, QSerialPortInfo::availablePorts()){
			if(info.productIdentifier() == (qint16)424){		// uBlox
				res = gpscomm->openPort(info.portName().toLocal8Bit(),baudrate,0);
				if(res){
					resstring = "Open "+ portname + " fail, quitting thread";
				}else{
					resstring = "Open "+ info.portName() + " succeeded";
				}
				break;
			}
		}
		//		quit = 1;
	}else{
		resstring = "Open "+ portname + " succeeded";
	}
	qDebug()<<resstring;
	emit error(resstring);

	while(!quit){
		char c;
		int numread = gpscomm->readByte(&c);
		if(numread==1){
			if(index<maxlen){
				message[index++] = c;
			}else{
				mstate = 0;
				index = 0;
			}
			switch(mstate){
			case 0:
			case 1:
			case 2:
				if(mstate==0){
					index = 0;
					message[index++] = c;
				}
				if(c==header[mstate]){
					mstate++;
				}else{
					mstate = 0;
					index = 0;
				}
				break;
			case 3:
				mstate++;
				break;
			case 4:
				if(c==42){
					mstate++;
				}else{
					mstate = 0;

				}
				break;
			case 5:
				if(index>28){
					headerLength = message[3];
					length =  *(int *)(&message[8]);
					if(headerLength<28)break;
					if(length!=72)break;
					totalLength = headerLength + length;
					if(index>totalLength){
						novAtelLat = ptrToDouble(&message[headerLength + 8]);
						novAtelLon = ptrToDouble(&message[headerLength + 16]);
						novAtelAlt = (float)(ptrToDouble(&message[headerLength + 24]));
						novAtelCount++;
						novAtelMode = 1;
						mstate++;
					}
				}
				break;
			case 6:
				headerLength = message[3];
				length =  *(int *)(&message[8]);
				totalLength = headerLength + length + 4;
				if(index >= totalLength){
					mstate = 0;
					index = 0;
				}
				break;
			default:
				mstate = 0;
				index = 0;
				break;
			}
		}
		//		if(numread==1){
		//			emit newMsg(cons->message,numread);
		//		}
		//		if(numtosend){
		//			transmit(msgtosend,numtosend);
		//			numtosend = 0;
		//		}
	}
	gpscomm->closePort();

	emit finished();
}

//	----------------------------------------------------------------------------
//	startGpsThread
//	----------------------------------------------------------------------------

GpsThread* MainWindow::startGpsThread(const char *portname,int baudrate)
{
	QThread* thread = new QThread;
	GpsThread* gps = new GpsThread;
	gps->moveToThread(thread);
	connect(gps, SIGNAL (error(QString)), this, SLOT (errorString(QString)));
	connect(thread, SIGNAL (started()), gps, SLOT (process()));
	connect(gps, SIGNAL (finished()), thread, SLOT (quit()));
	connect(gps, SIGNAL (finished()), gps, SLOT (deleteLater()));
	connect(thread, SIGNAL (finished()), thread, SLOT (deleteLater()));
	connect(gps, SIGNAL (newMsg(unsigned char *,int)), &gimbalThread, SLOT (newMsg(unsigned char *,int)));
	gps->portname = portname;
	gps->baudrate = baudrate;
	thread->start();
	return gps;
}

//	----------------------------------------------------------------------------
//	buttons
//	----------------------------------------------------------------------------

void MainWindow::on_pushButton_clicked(){	resetIMU = 1;}
void MainWindow::on_pushButton_2_clicked(){	resetChart1 = 1;}
void MainWindow::on_pushButton_3_clicked(){	resetChart2 = 1;}
void MainWindow::on_pushButton_4_clicked(){	resetChart3 = 1;}

//	----------------------------------------------------------------------------
//	checkboxes
//	----------------------------------------------------------------------------

void MainWindow::on_enableO0CB_clicked(){ steeringOut.axis[0].enableO = (steeringOut.axis[0].enableO)?(0):(1); cmdSteering = 1; }
void MainWindow::on_enableO1CB_clicked(){ steeringOut.axis[1].enableO = (steeringOut.axis[1].enableO)?(0):(1); cmdSteering = 1; }

//	----------------------------------------------------------------------------
//	Geo
//	----------------------------------------------------------------------------

enum { X, Y, Z };
enum { N, E, D, U = D };
enum { PSI, THETA, PHI };
enum { YAW, PIT, ROL };

#define eF 0.0033528107
#define eA 6378137
#define eB 6356752.3142
#define Eccentricity2 0.00669437999013
#define EccentricityPrime2 (0.0820944379496 * 0.0820944379496)

//	----------------------------------------------------------------------------
//	Geo
//	----------------------------------------------------------------------------


//	----------------------------------------------------------------------------
//	ecefToGeodetic
//	----------------------------------------------------------------------------
#if 0
void ecefToGeodetic(double (*ecef)[3], double (*position)[3])
{
	double p = sqrt((*ecef)[0] * (*ecef)[0] + (*ecef)[1] * (*ecef)[1]);
	double theta = atan2((*ecef)[2] * eA, p * eB);
	double sinTheta = sin(theta);
	double sinTheta3 = sinTheta * sinTheta * sinTheta;
	double cosTheta = cos(theta);
	double cosTheta3 = cosTheta * cosTheta * cosTheta;
	double pn = atan2((*ecef)[2] + EccentricityPrime2 * eB * sinTheta3, p - Eccentricity2 * eA * cosTheta3);
	double pe = atan2((*ecef)[1], (*ecef)[0]);
	double sinLatitude = sin((*position)[N]);
	double cosLatitude = cos((*position)[N]);
	(*position)[N] = pn * toDeg;
	(*position)[E] = pe * toDeg;
	(*position)[U] = p / cosLatitude - eA / sqrt(1 - Eccentricity2 * sinLatitude * sinLatitude);
}
#else
#define C_E2		(0.006694379990141)
#define C_EPSILON   (0.996647189335253)
#define C_ABAR	  (42.69767270717997)
#define C_BBAR	  (42.84131151331357)
#define C_A		 (6378.137)
#define VNC_PI_D	(3.141592653589793238)

//vec3d ecef_to_lla_v3d(vec3d ecef)
void ecefToGeodetic(double (*ecef)[3], double (*position)[3])
{
	double x, y, z, r, c_phi, c_phi0, s_phi,
			s_phi0, tau, lat, lon, /*alt,*/ eta, h;

	const double Rthresh = 0.001;   /* Limit on distance from pole in km to switch calculation. */

	x = (*ecef)[0]/1000;
	y = (*ecef)[1]/1000;
	z = (*ecef)[2]/1000;

	r = sqrt(x*x + y*y);

	if (r < Rthresh)
	{
		c_phi0 = 0;
		s_phi0 = (z > 0) - (z < 0); /* computes sign */
	}
	else
	{
		double tau0 = z / (C_EPSILON * r);
		c_phi0 = 1 / sqrt(1 + tau0 * tau0);
		s_phi0 = tau0 * c_phi0;
	}

	tau = (z + C_BBAR * s_phi0 * s_phi0 * s_phi0) / (r - C_ABAR * c_phi0 * c_phi0 * c_phi0);
	lat = atan(tau);

	if (r < Rthresh)
	{
		c_phi = 0;
		s_phi = (z > 0) - (z < 0); /* computes sign */
	}
	else
	{
		c_phi = 1 / sqrt(1 + tau * tau);
		s_phi = tau * c_phi;
	}

	eta = sqrt(1 - C_E2 * s_phi * s_phi);
	h = r * c_phi + z * s_phi - C_A * eta;

	lon = atan2(y, x);


	(*position)[0] = lat * 180 / VNC_PI_D;
	(*position)[1] = lon * 180 / VNC_PI_D;
	(*position)[2] = h * 1000;

}
#endif

//	----------------------------------------------------------------------------
//	geodeticToEcef
//	----------------------------------------------------------------------------

#if 0

void geodeticToEcef(double (*position)[3], double (*ecef)[3])
{
	double sinLatitude = sin(toRad*((*position)[N]));
	double rEW = eA / sqrt(1.0 - Eccentricity2 * sinLatitude * sinLatitude);
	double temp = (rEW + (*position)[U]) * cos(toRad*((*position)[N]));
	(*ecef)[0] = temp * cos(toRad*((*position)[E]));
	(*ecef)[1] = temp * sin(toRad*((*position)[E]));
	(*ecef)[2] = ((1.0 - Eccentricity2) * rEW + (*position)[U]) * sinLatitude;
}

#else

//	----------------------------------------------------------------------------
//	geodeticToEcef
//	----------------------------------------------------------------------------

//#define C_E2		(0.006694379990141)
//#define C_EPSILON   (0.996647189335253)
//#define C_ABAR	  (42.69767270717997)
//#define C_BBAR	  (42.84131151331357)
//#define C_A		 (6378.137)

void geodeticToEcef(double (*position)[3], double (*ecef)[3])
{
	double lat = (*position)[0] * toRad;   /* Geodetic latitude in radians. */
	double lon = (*position)[1] * toRad;   /* Longitude in radians. */
	double alt = (*position)[2] / 1000;	/* Altitude above WGS84 in km. */

	double t1 = sin(lat);
	double n = C_A / sqrt(1 - C_E2 * t1 * t1);

	t1 = alt + n;
	double x = t1 * cos(lat) * cos(lon);
	double y = t1 * cos(lat) * sin(lon);
	double z = (t1 - C_E2 * n) * sin(lat);

	(*ecef)[0] = x * 1000;
	(*ecef)[1] = y * 1000;
	(*ecef)[2] = z * 1000;
}
#endif

//	----------------------------------------------------------------------------
//	earthToRe2t
//	----------------------------------------------------------------------------

// create transformation matrix from ecef co-ords to tangent plane co-ords,
// given the lat and long of the origin of the tangent plane

void earthToRe2t(double (*position)[3], double (*Re2t)[3][3])
{
	double lat = toRad*((*position)[N]);
	double lon = toRad*((*position)[E]);

	double sinLatitude = sin(lat);
	double cosLatitude = cos(lat);
	double sinLongitude = sin(lon);
	double cosLongitude = cos(lon);

	(*Re2t)[0][0] = -sinLatitude * cosLongitude;
	(*Re2t)[0][1] = -sinLatitude * sinLongitude;
	(*Re2t)[0][2] = cosLatitude;
	(*Re2t)[1][0] = -sinLongitude;
	(*Re2t)[1][1] = cosLongitude;
	(*Re2t)[1][2] = 0;
	(*Re2t)[2][0] = -cosLatitude * cosLongitude;
	(*Re2t)[2][1] = -cosLatitude * sinLongitude;
	(*Re2t)[2][2] = -sinLatitude;
}

//	----------------------------------------------------------------------------
//	mult3by3times3by1
//	----------------------------------------------------------------------------

void mult3by3times3by1(double (*a)[3][3], double (*b)[3], double (*c)[3])
{
	double p[3];
	double (*pPtr)[3];

	pPtr = (b == c)?(&p):(c);

	for (int row=0; row<3; row++){
		(*pPtr)[row] = (*a)[row][0]*(*b)[0];
		for (int k = 1; k<3; k++)
			(*pPtr)[row] += (*a)[row][k] * (*b)[k];
	}
	if(pPtr != c)memcpy(c, p, sizeof(p));
}

//	----------------------------------------------------------------------------
//	multT3by3times3by1
//	----------------------------------------------------------------------------

void multT3by3times3by1(double (*a)[3][3], double (*b)[3], double (*c)[3])
{
	double p[3];
	double (*pPtr)[3];
	int row, k;

	if (b == c){
		pPtr = &p;
	}else{
		pPtr = c;
	}
	for (row = 0; row < 3; row++) {
		(*pPtr)[row] = (*a)[0][row] * (*b)[0];
		for (k = 1; k < 3; k++){
			(*pPtr)[row] += (*a)[k][row] * (*b)[k];
		}
	}
	if (pPtr != c)memcpy(c, p, sizeof(p));
}

//	----------------------------------------------------------------------------
//	multT3by3times3by3
//	----------------------------------------------------------------------------

void multT3by3times3by3(double (*a)[3][3], double (*b)[3][3], double (*c)[3][3])
{
	double p[3][3];
	double (*pPtr)[3][3];
	int row, col, k;

	if (a == c || b == c){
		pPtr = &p;
	}else{
		pPtr = c;
	}

	for(row = 0; row < 3; row++){
		for(col = 0; col < 3; col++){
			(*pPtr)[row][col] = (*a)[0][row] * (*b)[0][col];
			for (k = 1; k < 3; k++){
				(*pPtr)[row][col] += (*a)[k][row] * (*b)[k][col];
			}
		}
	}

	if (pPtr != c)memcpy(c, p, sizeof(p));
}

//	----------------------------------------------------------------------------
//	yprEulerToRot
//	----------------------------------------------------------------------------

void yprEulerToRot(double (*eulers)[3], double (*rotPtr)[3][3])
{

	double psi = toRad*((*eulers)[PSI]);
	double theta = toRad*((*eulers)[THETA]);
	double phi = toRad*((*eulers)[PHI]);

	double sinPsi = sin(psi);
	double cosPsi = cos(psi);
	double sinTheta = sin(theta);
	double cosTheta = cos(theta);
	double sinPhi = sin(phi);
	double cosPhi = cos(phi);

	(*rotPtr)[0][0] = cosTheta * cosPsi;
	(*rotPtr)[0][1] = cosTheta * sinPsi;
	(*rotPtr)[0][2] = -sinTheta;
	(*rotPtr)[1][0] = sinPhi * sinTheta * cosPsi - cosPhi * sinPsi;
	(*rotPtr)[1][1] = sinPhi * sinTheta * sinPsi + cosPhi * cosPsi;
	(*rotPtr)[1][2] = sinPhi * cosTheta;
	(*rotPtr)[2][0] = cosPhi * sinTheta * cosPsi + sinPhi * sinPsi;
	(*rotPtr)[2][1] = cosPhi * sinTheta * sinPsi - sinPhi * cosPsi;
	(*rotPtr)[2][2] = cosPhi * cosTheta;
}
/*
//	----------------------------------------------------------------------------
//	rotToYprEuler
//	----------------------------------------------------------------------------

void rotToYprEuler(double (*rotPtr)[3][3], double (*eulers)[3])
{
(*eulers)[PHI] = atan2((*rotPtr)[1][2], (*rotPtr)[2][2]);

if((*rotPtr)[0][2]<=-1.0){
(*eulers)[THETA]=PI/2.0;
}else if((*rotPtr)[0][2]>=1.0){
(*eulers)[THETA]=-PI/2.0;
}else{
(*eulers)[THETA] = asin(-(*rotPtr)[0][2]);
}
(*eulers)[PSI] = atan2((*rotPtr)[0][1], (*rotPtr)[0][0]);
}
*/
//	---------------------
//  geo
//	---------------------
/*
void geo()
{
double ofs[3] = { 0.0,-0.1, -2000.0};

for(int i=0;i<3;i++){
state.geo.cameraL[i] = state.nav.position[i];
geoMisc.ypr[i] = state.nav.ypr[i];
}

if((state.geo.targetL[0]==0)&&(state.geo.cameraL[0]>43)){
for(int i=0;i<3;i++){
state.geo.targetL[i] = state.geo.cameraL[i] + ofs[i];
}
}

geodeticToEcef(&state.geo.cameraL,&geoMisc.cameraE);
geodeticToEcef(&state.geo.targetL,&geoMisc.targetE);
for(int i=0;i<3;i++){
geoMisc.vectorE[i] = geoMisc.targetE[i] - geoMisc.cameraE[i];
}
state.geo.range = (float)sqrt(
geoMisc.vectorE[N]*geoMisc.vectorE[N] +
geoMisc.vectorE[E]*geoMisc.vectorE[E] +
geoMisc.vectorE[D]*geoMisc.vectorE[D]);

earthToRe2t(&state.geo.cameraL,&geoMisc.Re2t);
mult3by3times3by1(&geoMisc.Re2t,&geoMisc.vectorE,&geoMisc.vectorT);

yprEulerToRot(&geoMisc.ypr,&geoMisc.c);
mult3by3times3by1(&geoMisc.c,&geoMisc.vectorT,&geoMisc.vectorC);
state.geo.losError[0] = (float)(toDeg*atan2(-geoMisc.vectorC[1],geoMisc.vectorC[0]));
state.geo.losError[1] = (float)(toDeg*atan2( geoMisc.vectorC[2],geoMisc.vectorC[0]));
state.geo.losError[2] = state.nav.ypr[2];
}
*/

//	----------------------------------------------------------------------------
//	LineLineIntersect
//	----------------------------------------------------------------------------

#define EPS 0.01

int LineLineIntersect(
		double (*p1)[3],double (*p2)[3],double (*p3)[3],double (*p4)[3],
double (*pa)[3],double (*pb)[3])
{
	double p13[3],p43[3],p21[3];
	double d1343,d4321,d1321,d4343,d2121;
	double numer,denom;
	double mua,mub;

	p13[X] = (*p1)[X] - (*p3)[X];
	p13[Y] = (*p1)[Y] - (*p3)[Y];
	p13[Z] = (*p1)[Z] - (*p3)[Z];
	p43[X] = (*p4)[X] - (*p3)[X];
	p43[Y] = (*p4)[Y] - (*p3)[Y];
	p43[Z] = (*p4)[Z] - (*p3)[Z];
	if (fabs(p43[X])  < EPS && fabs(p43[Y])  < EPS && fabs(p43[Z])  < EPS)
		return(false);
	p21[X] = (*p2)[X] - (*p1)[X];
	p21[Y] = (*p2)[Y] - (*p1)[Y];
	p21[Z] = (*p2)[Z] - (*p1)[Z];
	if (fabs(p21[X])  < EPS && fabs(p21[Y])  < EPS && fabs(p21[Z])  < EPS)
		return(false);

	d1343 = p13[X] * p43[X] + p13[Y] * p43[Y] + p13[Z] * p43[Z];
	d4321 = p43[X] * p21[X] + p43[Y] * p21[Y] + p43[Z] * p21[Z];
	d1321 = p13[X] * p21[X] + p13[Y] * p21[Y] + p13[Z] * p21[Z];
	d4343 = p43[X] * p43[X] + p43[Y] * p43[Y] + p43[Z] * p43[Z];
	d2121 = p21[X] * p21[X] + p21[Y] * p21[Y] + p21[Z] * p21[Z];

	denom = d2121 * d4343 - d4321 * d4321;
	if (fabs(denom) < EPS)
		return(false);
	numer = d1343 * d4321 - d1321 * d4343;

	mua = numer / denom;
	mub = (d1343 + d4321 * (mua)) / d4343;

	(*pa)[X] = (*p1)[X] + mua * p21[X];
	(*pa)[Y] = (*p1)[Y] + mua * p21[Y];
	(*pa)[Z] = (*p1)[Z] + mua * p21[Z];
	(*pb)[X] = (*p3)[X] + mub * p43[X];
	(*pb)[Y] = (*p3)[Y] + mub * p43[Y];
	(*pb)[Z] = (*p3)[Z] + mub * p43[Z];

	return(true);
}

//	----------------------------------------------------------------------------
//	triangulateTarget
//	----------------------------------------------------------------------------

bool triangulateTargetX(double (*position1)[3], float (*los1D)[3],
double (*position2)[3], float (*los2D)[3],
double (*positiont)[3])
{
	double ecef1[3];											// position1 ecef
	double ecef2[3];											// position2 ecef
	double point1[3],point3[3];									// line1 in tangent plane
	double point2[3],point4[3];									// line2 in tangent plane
	double pointa[3],pointb[3];									// line1_2 intersection tangent plane: 2 types
	double Re2tV[3][3];											// matrix for ecef_to_tangent

	double targetecef[3];

	double los1[3]={	toRad*(*los1D)[YAW],
						toRad*(*los1D)[PIT],
						toRad*(*los1D)[ROL]	};					// los1D in radians

	double los2[3]={	toRad*(*los2D)[YAW],
						toRad*(*los2D)[PIT],
						toRad*(*los2D)[ROL] };					// los2D in radians

	double tanYaw1 = tan(los1[YAW]);
	double tanYaw2 = tan(los2[YAW]);
	if(fabs(tanYaw1-tanYaw2)<(1.0*toRad)){
		return false;											// fail if lines nearly parallel
	}

	earthToRe2t(position1, &Re2tV);								// get matrix for ecef_to_tangent

	point1[X] = 0;												// point1 = pos1-pos1
	point1[Y] = 0;
	point1[Z] = 0;

	point2[X] = point1[X] + cos(los1[YAW])*cos(los1[PIT]);		// point2 = point1 + los1 unit vector
	point2[Y] = point1[X] + sin(los1[YAW])*cos(los1[PIT]);
	point2[Z] = point1[X] - sin(los1[PIT]);

	geodeticToEcef( position1, &ecef1 );						// point3 = pos2-pos1
	geodeticToEcef( position2, &ecef2 );
	double diff[3] = { ecef2[0]-ecef1[0],
					   ecef2[1]-ecef1[1],
					   ecef2[2]-ecef1[2] };						// ..subtract in ecef
	mult3by3times3by1(&Re2tV, &diff, &point3);					// ..convert to tangent

	point4[X] = point3[X] + cos(los2[YAW])*cos(los2[PIT]);		// point4 = point3 + los2 unit vector
	point4[Y] = point3[Y] + sin(los2[YAW])*cos(los2[PIT]);
	point4[Z] = point3[Z] - sin(los2[PIT]);

	LineLineIntersect( &point1, &point2, &point3, &point4, &pointa, &pointb );
	double target[3] = { ( pointa[0] + pointb[0] ) / 2.0,
						 ( pointa[1] + pointb[1] ) / 2.0,
						 ( pointa[2] + pointb[2] ) / 2.0 };		// average the 2 intersection types

	multT3by3times3by1(&Re2tV, &target, &targetecef);			// back to ECEF (transpose and multiply)

	for(int i=0;i<3;i++){
		targetecef[i] += ecef1[i];								// bring back datum pos1
	}
	ecefToGeodetic( &targetecef, positiont );					// back to geodetic

	return true;
}

//	----------------------------------------------------------------------------
//	triangulate
//	----------------------------------------------------------------------------

void triangulate()
{
	if(triangulateTarget(&p1,&l1,&p2,&l2,&t)){
	}else{
		t[0]=0;
		t[0]=1;
		t[0]=2;
	}
}

//	----------------------------------------------------------------------------
//	on_pushButton_5_clicked
//	----------------------------------------------------------------------------

void MainWindow::on_pushButton_5_clicked()
{
	for(int i=0;i<3;i++){
		//		p1[i] = gimbalThread.state.nav.position[i];
		p1[i] = gimbalThread.state.geo.cameraL[i];
		l1[i] = gimbalThread.state.nav.Ypr[i];
	}
}

//	----------------------------------------------------------------------------
//	on_pushButton_6_clicked
//	----------------------------------------------------------------------------

void MainWindow::on_pushButton_6_clicked()
{
	static int shotindex = 1;

	for(int i=0;i<3;i++){
		//		p2[i] = gimbalThread.state.nav.position[i];
		p2[i] = gimbalThread.state.geo.cameraL[i];
		l2[i] = gimbalThread.state.nav.Ypr[i];
	}
	triangulate();
	setTarget(t[0],t[1],t[2]);

	targetSet = 2;
	if(strcmp(targets[targetSet][targetIndex].desc,"Free")){
		targetIndex = (targetIndex<(NTARG-1))?(targetIndex+1):(0);
	}
	targets[targetSet][targetIndex].lat = t[0];
	targets[targetSet][targetIndex].lon = t[1];
	targets[targetSet][targetIndex].alt = t[2];
	sprintf(targets[targetSet][targetIndex].desc,"Sighted %i",shotindex++);

}

//	----------------------------------------------------------------------------
//	ranges
//	----------------------------------------------------------------------------

void MainWindow::ranges()
{

//	static int fpass = 1;
	static double ecefT[NTARS][NTARG][3];
	double ecefC[3];
	double vector[3];

	//  geodeticToEcef(&gimbalThread.state.nav.position,&ecefC);
	geodeticToEcef(&gimbalThread.state.geo.cameraL,&ecefC);
	for(int itarg=0;itarg<NTARG;itarg++){
		double pos[3]={targets[targetSet][itarg].lat,targets[targetSet][itarg].lon,targets[targetSet][itarg].alt};
		geodeticToEcef(&pos,&ecefT[targetSet][itarg]);
		for(int iaxis=0;iaxis<3;iaxis++){
			vector[iaxis] = ecefT[targetSet][itarg][iaxis] - ecefC[iaxis];
		}
		targets[targetSet][itarg].range = (float)sqrt(
					vector[0]*vector[0] +
				vector[1]*vector[1] +
				vector[2]*vector[2]);
		if(itarg==0){
			maxTargetRange = targets[targetSet][itarg].range;
			minTargetRange = targets[targetSet][itarg].range;
		}else{
			maxTargetRange = std::max(maxTargetRange,targets[targetSet][itarg].range);
			minTargetRange = std::min(minTargetRange,targets[targetSet][itarg].range);
		}
	}
//	if(autotarget || fpass){
	if(autotarget){
		for(int itarg=0;itarg<NTARG;itarg++){
			if(targets[targetSet][itarg].range == minTargetRange){
				targetIndex = itarg;
				target_t *t = &targets[targetSet][targetIndex];
				setTarget(t->lat,t->lon,t->alt);
				break;
			}
		}
	}
//	fpass = 0;
}

//	----------------------------------------------------------------------------
//	targetString
//	----------------------------------------------------------------------------

QString MainWindow::rangeString(unsigned int index)
{
	int highlight = targets[targetSet][index].range == minTargetRange;

	QString labeltext = "";
	if(index>=NTARG)return labeltext;
	if(highlight){
		labeltext.sprintf("<strong>%8.0f<\\strong>",targets[targetSet][index].range);
	}else{
		labeltext.sprintf("%8.0f",targets[targetSet][index].range);
	}
	return labeltext;
}

//	----------------------------------------------------------------------------
//	targetString
//	----------------------------------------------------------------------------

QString MainWindow::targetString(unsigned int index)
{
	int highlight = targetIndex == index;

	QString labeltext = "";
	if(index>=NTARG)return labeltext;
	if(highlight){
		labeltext.sprintf("<strong>%s<\\strong>",targets[targetSet][index].desc);
	}else{
		labeltext.sprintf("%s",targets[targetSet][index].desc);
	}
	return labeltext;
}


//	----------------------------------------------------------------------------
//	on_autoTargetPB_clicked
//	----------------------------------------------------------------------------

void MainWindow::on_autoTargetPB_clicked()
{
	autotarget = autotarget?0:1;
}

//	----------------------------------------------------------------------------
//	selectTarget
//	----------------------------------------------------------------------------

void selectTarget()
{
	autotarget = 0;
	target_t *t = &targets[targetSet][targetIndex];
	setTarget(t->lat, t->lon, t->alt);
}

//	----------------------------------------------------------------------------
//	on_autoTargetPB_clicked
//	----------------------------------------------------------------------------

void MainWindow::on_nextTargetPB_clicked()
{
	while(1){
		targetIndex = (targetIndex<(NTARG-1))?(targetIndex+1):(0);
		if(targetIndex == 0)break;
		if(targets[targetSet][targetIndex].lat != 0)break;
	}
	selectTarget();
}

//	----------------------------------------------------------------------------
//	on_autoTargetPB_clicked
//	----------------------------------------------------------------------------

void MainWindow::on_prevTargetPB_clicked()
{
	while(1){
		targetIndex = (targetIndex>0)?(targetIndex-1):(NTARG-1);
		if(targetIndex == 0)break;
		if(targets[targetSet][targetIndex].lat != 0)break;
	}
	selectTarget();
}

//	----------------------------------------------------------------------------
//	on_prevSetPB_clicked
//	----------------------------------------------------------------------------

void MainWindow::on_prevSetPB_clicked()
{
	if(targetSet>0){
		targetIndices[targetSet] = targetIndex;
		targetSet--;
		targetIndex = targetIndices[targetSet];
	}
	selectTarget();
}

//	----------------------------------------------------------------------------
//	on_nextSetPB_clicked
//	----------------------------------------------------------------------------

void MainWindow::on_nextSetPB_clicked()
{
	if(targetSet<(NTARS-1)){
		targetIndices[targetSet] = targetIndex;
		targetSet++;
		targetIndex = targetIndices[targetSet];
	}
	selectTarget();
}

//	----------------------------------------------------------------------------
//	on_standby_Button_clicked
//	----------------------------------------------------------------------------

void MainWindow::on_standby_Button_clicked()
{
	steeringOut.standby = (steeringOut.standby)?(0):(1);
	cmdSteering = 1;
}

//	----------------------------------------------------------------------------
//	on_null_Button_pressed
//	----------------------------------------------------------------------------

void MainWindow::on_null_Button_pressed()
{
	gimbalThread.cmdAutonull = 2;
}

//	----------------------------------------------------------------------------
//	on_null_Button_released
//	----------------------------------------------------------------------------

void MainWindow::on_null_Button_released()
{
	gimbalThread.cmdAutonull = 1;
}

//	----------------------------------------------------------------------------
//	on_radioButton_Standby_clicked
//	----------------------------------------------------------------------------

void MainWindow::on_radioButton_Standby_clicked()
{
	setModeX(MODEX_STANDBY);
}

//	----------------------------------------------------------------------------
//	on_radioButton_Rate_clicked
//	----------------------------------------------------------------------------

void MainWindow::on_radioButton_Rate_clicked()
{
	setModeX(MODE_RATE);
}

//	----------------------------------------------------------------------------
//	on_radioButton_Slave_clicked
//	----------------------------------------------------------------------------

void MainWindow::on_radioButton_Slave_clicked()
{
	setModeX(MODE_SLAVE);
}

//	----------------------------------------------------------------------------
//	on_radioButton_Outer_clicked
//	----------------------------------------------------------------------------

void MainWindow::on_radioButton_Outer_clicked()
{
	setModeX(MODE_OUTER);
}

//	----------------------------------------------------------------------------
//	on_radioButton_Outer_clicked
//	----------------------------------------------------------------------------


void MainWindow::on_radioButton_Geo_clicked()
{
	setModeX(MODE_GEO);
}

//	----------------------------------------------------------------------------
//	on_radioButton_Outer_clicked
//	----------------------------------------------------------------------------

void MainWindow::on_radioButton_Stow_clicked()
{
	setModeX(MODE_STOW);
}

//	----------------------------------------------------------------------------
//	on_pushButton_7_clicked
//	----------------------------------------------------------------------------

void MainWindow::on_pushButton_7_clicked()
{
	guiGimConnect = (guiGimConnect==CONNECT_NONE)?(CONNECT_232):(CONNECT_NONE);
	emit connectToGimbal(guiGimConnect);
}

//	----------------------------------------------------------------------------
//	on_fftMax_editingFinished
//	----------------------------------------------------------------------------

void MainWindow::on_fftMax_editingFinished()
{
	float f = 0;
	int res = sscanf(ui->fftMax->text().toLocal8Bit(),"%a",&f);
	if(res==1){
		f = fabs(f);
		chart02->maxy =  f;
		chart02->miny = -f;
	}

}

//	----------------------------------------------------------------------------
//	pollGimbalTx
//	----------------------------------------------------------------------------

void pollGimbalTx(){
	if(NULL==mainX)return;
	mainX->gimbalThread.pollGimbalTx();
}


//	----------------------------------------------------------------------------
//	on_xxxeditingFinished
//	----------------------------------------------------------------------------

void MainWindow::on_waypoint1_Lat_editingFinished() { steeringOut.sim.lla1[0] = ui->waypoint1_Lat->text().toDouble(); cmdSteering = 1; refreshControls = 1; }
void MainWindow::on_waypoint1_Lon_editingFinished() { steeringOut.sim.lla1[1] = ui->waypoint1_Lon->text().toDouble(); cmdSteering = 1; refreshControls = 1; }
void MainWindow::on_waypoint1_Alt_editingFinished() { steeringOut.sim.lla1[2] = ui->waypoint1_Alt->text().toDouble(); cmdSteering = 1; refreshControls = 1; }
void MainWindow::on_waypoint2_Lat_editingFinished() { steeringOut.sim.lla2[0] = ui->waypoint2_Lat->text().toDouble(); cmdSteering = 1; refreshControls = 1; }
void MainWindow::on_waypoint2_Lon_editingFinished() { steeringOut.sim.lla2[1] = ui->waypoint2_Lon->text().toDouble(); cmdSteering = 1; refreshControls = 1; }
void MainWindow::on_waypoint2_Alt_editingFinished() { steeringOut.sim.lla2[2] = ui->waypoint2_Alt->text().toDouble(); cmdSteering = 1; refreshControls = 1; }
void MainWindow::on_simSpeed_editingFinished()		{ steeringOut.sim.speed =   ui->simSpeed->text().toDouble();	  cmdSteering = 1; refreshControls = 1; }

//	----------------------------------------------------------------------------
//	sim start/stop
//	----------------------------------------------------------------------------

void MainWindow::on_pushButton_14_clicked()
{
	switch(steeringOut.sim.mode){

		case SIM_MODE_OFF:
			steeringOut.sim.mode = SIM_MODE_RUN;
			break;

		case SIM_MODE_RUN:
		default:
			steeringOut.sim.mode = SIM_MODE_OFF;
			break;

	}
	cmdSteering = 1;
	refreshControls = 1;
}

//	----------------------------------------------------------------------------
//	triangulateTarget
//	----------------------------------------------------------------------------

bool triangulateTarget(double (*position1)[3], float (*los1D)[3], double (*position2)[3], float (*los2D)[3], double (*positiont)[3])
{									// line1_2 intersection tangent plane: 2 types
	float yaw1 =  (*los1D)[0];
	float yaw2 =  (*los2D)[0];

	const float minyaw = 1.0f;
	if( fabs(yaw2-yaw1) < minyaw ){
		return false;
	}

	double a1 = 90.0-yaw1;
	double a2 = 90.0-yaw2;

	double m1 = tan(toRad*a1);
	double m2 = tan(toRad*a2);

	double Re2tV[3][3];	earthToRe2t(	position1, &Re2tV);
	double ecef1[3];	geodeticToEcef( position1, &ecef1 );
	double ecef2[3];	geodeticToEcef( position2, &ecef2 );

	double ned1[3];		mult3by3times3by1(&Re2tV, &ecef1, &ned1);
	double ned2[3];		mult3by3times3by1(&Re2tV, &ecef2, &ned2);

	double y1 = ned1[0];
	double x1 = ned1[1];
	double y2 = ned2[0];
	double x2 = ned2[1];

	double b1 = y1 - m1*x1;
	double b2 = y2 - m2*x2;

	double x = (b2-b1)/(m1-m2);
	double y = m1*x + b1;

	double dx = x - x1;
	double dy = y - y1;
	double range = sqrt(dx*dx + dy*dy);
	double pit = (*los1D)[1];
	double dz = -range*tan(toRad*pit);

	double targetned[3] = {
		ned1[0] + dy,
		ned1[1] + dx,
		ned1[2] + dz
	};

	double targetecef[3];
	multT3by3times3by1(&Re2tV, &targetned, &targetecef);		// back to ECEF (transpose and multiply)
	ecefToGeodetic( &targetecef, positiont );					// back to geodetic

	return true;
}

//	----------------------------------------------------------------------------
//	on_pushButton_8_clicked
//	----------------------------------------------------------------------------

void MainWindow::on_pushButton_8_clicked()
{
	extern int opMinimize;
	opMinimize++;

}
