//	-------------------------------------------------
//	geoTuneDlg.cpp
//	-------------------------------------------------

#include "stdafx.h"
#include "project.h"
#include "gimbalDefs.h"
#include "gimbalDef.h"
#include "geoTune.h"
#include "geoTuneDlg.h"
#include "graphDlg.h"
#include "serial.h"
#include "gps.h"
#include "gimbalState.h"
#include "camView.h"
#include "Resource.h"
#include "afxdialogex.h"
#include <ctime>

#ifdef _VIMBA
#include "VimbaCPP/Include/VimbaCPP.h"
#include "VimbaImageTransform/Include/VmbTransform.h"

AVT::VmbAPI::VimbaSystem& vimbaSys = AVT::VmbAPI::VimbaSystem::GetInstance();
AVT::VmbAPI::CameraPtr pCamera = AVT::VmbAPI::CameraPtr();
#endif

#ifdef _DEBUG
#define new DEBUG_NEW
#endif

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

enum {
	LED_GREY,
	LED_GREEN,
	LED_YELLOW,
	LED_RED
};

DEFINE_GUID(ImageFormatJPEG, 0xb96b3cae,0x0728,0x11d3,0x9d,0x7b,0x00,0x00,0xf8,0x1e,0xf3,0x2e);

//	-----------------------------------------------------------
//	Settings
//	-----------------------------------------------------------

#define TIMER_FREQ 15

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

	int npixx;			// display X size [pixels]
	int npixy;			// display Y size [pixels]
	CRect rect;			// display bounds [pixels]
	int dcOK;
	int painting;
	CDC dc;
	CBitmap dbitmap;
	CString logText;
	CString logText2;
	int newlog;

	int portIndex = COM_PORT;
	int gwshow = 0;

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

	int connected = 0;
	int frames = 0;
	CBitmap leds[4];
	int cameraConnected = 0;
	int gpsConnected = 0;
	int vn200Connected = 0;
	int gyroConnected = 0;
	int refreshpars = 1;

//	-----------------------------------------------------------
//	Globals
//	-----------------------------------------------------------
	
#ifdef _VIMBA
	AVT::VmbAPI::FeaturePtr focusFeature;
	AVT::VmbAPI::FeaturePtr exposureFeature;
	AVT::VmbAPI::FeaturePtr gainFeature;
	AVT::VmbAPI::FeaturePtr fstopFeature;
	AVT::VmbAPI::FeaturePtr lensInitializeFeature;
#endif

	double exposureTimeAbs = 0;
	double cameraGain = 0;
	double fstop = 0;
	int focusInt = 0;
	int vimbaFocus = 0;
	int focusMax = 0;
	int focusMin = 0;

	int quit = 0;
	int needFrame = 0;
	int saveFrame = 0;
	int settleCount = 0;
	CImage m_Image;
	CImage m_Imagx;
	int m_ClearBackground = 0;
	int newImage = 1;
	int imageOk = 0;
	int m_fitWidth = 0;
	clock_t focTime;

	graphDlg graphD;
	int m_pause = 0;
	int m_camera = 1;

//	-----------------------------------------------------------
//	Slave
//	-----------------------------------------------------------

	typedef struct {
		int focus;
		float y;
		float p;
		float r;
	} slavePos_t;

	slavePos_t slavePos[2]={
//		{1880,   17.3f, 4.60f, 0},
//		{2005,  44.95f, 3.20f, 0}
		{1900,   17.3f, 4.60f, 0},
		{2020,  44.95f, 3.20f, 0}
	};

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

double p1[3] = { 0,0,0 };
float l1[3] = { 0,0,0 };
double p2[3];
float l2[3];
double t[3];
double targetLat;
double targetLon;
double targetAlt;
double novAtelLat;
double novAtelLon;
float novAtelAlt;
float novAtelhMsl;
int16_t novAtelCount;
int16_t novAtelMode;
void triangulate();
int autotarget;
unsigned int targetIndex;
unsigned int targetSet;
char *fname(int dt,char *type,int inx, char *ext);

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

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

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

	#define NTARS 4
	#define NTARG 11

	target_t targets[NTARS][NTARG] = {
		{
			{43.351786,-79.798379,117.8,0,"Tower 1 top"	},
			{43.351641,-79.798130,117.8,0,"Tower 2 top"	},
			{43.351330,-79.797971,116.5,0,"Tower 3 top"	},
			{43.350953,-79.797623,116.5,0,"Tower 4 top"	},
			{43.350603,-79.797355,116.0,0,"Tower 5 top"	},
			{43.350241,-79.797096,116.0,0,"Tower 6 top"	},
			{43.350328,-79.796676,116.0,0,"Tower 7 top"	},
			{43.350533,-79.796343,116.0,0,"Tower 8 top"	},
			{43.350824,-79.795852,116.5,0,"Tower 9 top"	},
			{43.351115,-79.795359,116.0,0,"Tower 10 top"},
			{43.351426,-79.794865,117.0,0,"Tower 11 top"}
		},{
			{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"	}
		},
		{
			{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"	},
			{43.347721,-79.795518,103.000,0,"WinNers"	},
			{43.351316,-79.799178,109.700,0,"Office, 0"	},
			{43.350241,-79.799178,109.700,0,"Office, S"	},
			{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"}
		},

	};

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

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

	extern int gimbalConnected;
	extern int gimLogCount;
	extern controls_t controls;
	extern state_t state;
	extern steering_t steering;

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

	extern int readGimbalM();
	extern void	setStandby(int standby);
	extern void	setMode(int mode);
	extern void	setAutonull(int autonull);
	extern void	setSlave(float y, float p, float r);
	extern void sendControls(controls_t *c);
	extern void sendSteering(steering_t *s);
	extern void setConnect(int val);
	extern void setHeading();
	extern void setHSLog(int val);

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

	void startGimbalThread();
	void ranges();
	char *targetString(unsigned int index);
	char *rangeString(unsigned int index);
	char *focus2MeterS(int f);

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

#ifdef _VIMBA
	void grabFrame();
	void readCameraSettings();
	void startVimbaThread();
	VmbErrorType setupCamera();
#endif

//	-----------------------------------------------------------
//	CgeoTuneDlg dialog
//	-----------------------------------------------------------


CgeoTuneDlg::CgeoTuneDlg(CWnd* pParent /*=nullptr*/)
	: CDialogEx(IDD_GEOTUNE_DIALOG, pParent)
{
	m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
}

//	-----------------------------------------------------------
//	DoDataExchange
//	-----------------------------------------------------------

void CgeoTuneDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialogEx::DoDataExchange(pDX);
	DDX_Control(pDX, IDC_CAMERA_IMAGE, m_cameraImage);
	DDX_Control(pDX, IDC_EDIT1, m_log_edit);
}

//	-----------------------------------------------------------
//	MESSAGE_MAP
//	-----------------------------------------------------------

BEGIN_MESSAGE_MAP(CgeoTuneDlg, CDialogEx)
	ON_WM_PAINT()
	ON_WM_QUERYDRAGICON()
	ON_WM_TIMER()
	ON_BN_CLICKED(IDC_ENTER, &CgeoTuneDlg::OnBnClickedEnter)
	ON_BN_CLICKED(IDC_SET_SLAVE, &CgeoTuneDlg::OnBnClickedSetSlave)
	ON_BN_CLICKED(IDC_RADIO_GEO, &CgeoTuneDlg::OnBnClickedRadioGeo)
	ON_BN_CLICKED(IDC_RADIO_SLAVE, &CgeoTuneDlg::OnBnClickedRadioSlave)
	ON_BN_CLICKED(IDC_RADIO_RATE, &CgeoTuneDlg::OnBnClickedRadioRate)
	ON_BN_CLICKED(IDC_RADIO_STANDBY, &CgeoTuneDlg::OnBnClickedRadioStandby)
	ON_BN_CLICKED(IDC_SET_SLAVE2, &CgeoTuneDlg::OnBnClickedSetSlave2)
	ON_BN_CLICKED(IDC_LOAD_SLAVE1, &CgeoTuneDlg::OnBnClickedLoadSlave1)
	ON_BN_CLICKED(IDC_LOAD_SLAVE2, &CgeoTuneDlg::OnBnClickedLoadSlave2)
	ON_WM_DESTROY()
	ON_STN_CLICKED(IDC_CAMERA_IMAGE, &CgeoTuneDlg::OnStnClickedCameraImage)
	ON_BN_CLICKED(IDC_LENS_INIT, &CgeoTuneDlg::OnBnClickedLensInit)
	ON_BN_CLICKED(IDC_AUTO_TARGET, &CgeoTuneDlg::OnBnClickedAutoTarget)
	ON_BN_CLICKED(IDC_PREV_TARGET, &CgeoTuneDlg::OnBnClickedPrevTarget)
	ON_BN_CLICKED(IDC_NEXT_TARGET, &CgeoTuneDlg::OnBnClickedNextTarget)
	ON_BN_CLICKED(IDC_PREV_SET, &CgeoTuneDlg::OnBnClickedPrevSet)
	ON_BN_CLICKED(IDC_NEXT_SET, &CgeoTuneDlg::OnBnClickedNextSet)
	ON_BN_CLICKED(IDC_AUTONULL, &CgeoTuneDlg::OnBnClickedAutonull)
	ON_BN_CLICKED(IDC_RADIO_T1, &CgeoTuneDlg::OnBnClickedRadioT1)
	ON_BN_CLICKED(IDC_RADIO_T2, &CgeoTuneDlg::OnBnClickedRadioT2)
	ON_BN_CLICKED(IDC_RADIO_T3, &CgeoTuneDlg::OnBnClickedRadioT3)
	ON_BN_CLICKED(IDC_RADIO_T4, &CgeoTuneDlg::OnBnClickedRadioT4)
	ON_BN_CLICKED(IDC_RADIO_T5, &CgeoTuneDlg::OnBnClickedRadioT5)
	ON_BN_CLICKED(IDC_RADIO_T6, &CgeoTuneDlg::OnBnClickedRadioT6)
	ON_BN_CLICKED(IDC_RADIO_T7, &CgeoTuneDlg::OnBnClickedRadioT7)
	ON_BN_CLICKED(IDC_RADIO_T8, &CgeoTuneDlg::OnBnClickedRadioT8)
	ON_BN_CLICKED(IDC_RADIO_T9, &CgeoTuneDlg::OnBnClickedRadioT9)
	ON_BN_CLICKED(IDC_RADIO_T10, &CgeoTuneDlg::OnBnClickedRadioT10)
	ON_BN_CLICKED(IDC_RADIO_T11, &CgeoTuneDlg::OnBnClickedRadioT11)
	ON_BN_CLICKED(IDC_GRAPH_TOG, &CgeoTuneDlg::OnBnClickedGraphTog)
	ON_BN_CLICKED(IDC_PAUSE, &CgeoTuneDlg::OnBnClickedPause)
	ON_BN_CLICKED(IDC_CONNECT, &CgeoTuneDlg::OnBnClickedConnect)
	ON_BN_CLICKED(IDC_RADIO_STOW, &CgeoTuneDlg::OnBnClickedRadioStow)
	ON_BN_CLICKED(IDC_RADIO_OUTER, &CgeoTuneDlg::OnBnClickedRadioOuter)
	ON_BN_CLICKED(IDC_CAP_GRAPH, &CgeoTuneDlg::OnBnClickedCapGraph)
	ON_BN_CLICKED(IDC_CAMERA_TOG, &CgeoTuneDlg::OnBnClickedCameraTog)
	ON_BN_CLICKED(IDC_SET_HEADING, &CgeoTuneDlg::OnBnClickedSetHeading)
	ON_BN_CLICKED(IDC_SAVE_LOG, &CgeoTuneDlg::OnBnClickedSaveLog)
	ON_BN_CLICKED(IDC_HS_LOG, &CgeoTuneDlg::OnBnClickedHsLog)
	ON_BN_CLICKED(IDC_PORT, &CgeoTuneDlg::OnBnClickedPort)
END_MESSAGE_MAP()

//	-----------------------------------------------------------
//	OnInitDialog
//	-----------------------------------------------------------

void testDcm();

BOOL CgeoTuneDlg::OnInitDialog()
{
	CDialogEx::OnInitDialog();
	SetWindowText("geoTune");
	SetIcon(m_hIcon, TRUE);
	m_brush.CreateSolidBrush(RGB(200, 200, 200));

	testDcm();

#ifdef _VIMBA	
	VmbInt64_t nValue;
	VmbInt64_t nMin, nMax;

	if(VmbErrorSuccess == vimbaSys.Startup()){
		vimbaSys.OpenCameraByID("DEV_000F31035802", VmbAccessModeFull, pCamera);
		if(NULL != pCamera){
			if(VmbErrorSuccess == pCamera->GetFeatureByName("EFLensFocusCurrent", focusFeature)){
				focusFeature->GetValue(nValue);
				focusFeature->GetRange(nMin, nMax);
				focusFeature->GetValue(nValue);
			 if(VmbErrorSuccess == setupCamera()){
					cameraConnected = 1;
				}
				pCamera->GetFeatureByName("ExposureTimeAbs", exposureFeature);
				pCamera->GetFeatureByName("Gain", gainFeature);
				pCamera->GetFeatureByName("EFLensFStopCurrent", fstopFeature);

				pCamera->GetFeatureByName("EFLensInitialize", lensInitializeFeature);
				if(NULL != lensInitializeFeature){
					lensInitializeFeature->RunCommand();
				}
				focusFeature->SetValue(nValue);
			}

		}
	}
#endif

	leds[0].LoadBitmap(IDB_BITMAP1);
	leds[1].LoadBitmap(IDB_BITMAP2);
	leds[2].LoadBitmap(IDB_BITMAP4);
	leds[3].LoadBitmap(IDB_BITMAP3);

//	(CStatic *)GetDlgItem(IDC_LED1)->ModifyStyle(0xF, SS_BITMAP, SWP_NOSIZE);
	(CStatic *)GetDlgItem(IDC_LED2)->ModifyStyle(0xF, SS_BITMAP, SWP_NOSIZE);
	(CStatic *)GetDlgItem(IDC_LED3)->ModifyStyle(0xF, SS_BITMAP, SWP_NOSIZE);
	(CStatic *)GetDlgItem(IDC_LED4)->ModifyStyle(0xF, SS_BITMAP, SWP_NOSIZE);
	(CStatic *)GetDlgItem(IDC_LED5)->ModifyStyle(0xF, SS_BITMAP, SWP_NOSIZE);
	(CStatic *)GetDlgItem(IDC_LED6)->ModifyStyle(0xF, SS_BITMAP, SWP_NOSIZE);
	(CStatic *)GetDlgItem(IDC_LED7)->ModifyStyle(0xF, SS_BITMAP, SWP_NOSIZE);
	(CStatic *)GetDlgItem(IDC_LED8)->ModifyStyle(0xF, SS_BITMAP, SWP_NOSIZE);

	timer1 = SetTimer(1, 1000 / TIMER_FREQ, NULL);

	startGimbalThread();

	#if 0
		if(0 == openPort(COM_PORT, UTIL_BAUD)){
			setConnect(1);
			connected = 1;
			SetDlgItemText(IDC_PORT,getPortName());
		}
	#else
		OnBnClickedPort();
	#endif

	if(0 == gps::openPort("xxx", 460800)){
		gps::startGpsThread();
	}

#ifdef _VIMBA
 if(cameraConnected){
		startVimbaThread();
	}
#endif

 //	graphD.Create(	IDD_GRAPH_DIALOG,		this);

//	m_Image2x.Load("c:\\z.bmp");
	return TRUE;
}

//	-----------------------------------------------------------
//	fitRect
//	-----------------------------------------------------------

template <typename T>

CRect fitRect(T w, T h, const CRect &dst)
{
	double sw = static_cast<double>(dst.Width()) / w;
	double sh = static_cast<double>(dst.Height()) / h;
	double s = min(sw, sh);
	T new_w = static_cast<T>(w * s);
	T new_h = static_cast<T>(h * s);
	T off_w = (1 + dst.Width() - new_w) / 2;
	T off_h = (1 + dst.Height() - new_h) / 2;
	return CRect(off_w, off_h, off_w + new_w, off_h + new_h);
}

//	-----------------------------------------------------------
//	OnQueryDragIcon
//	-----------------------------------------------------------

HCURSOR CgeoTuneDlg::OnQueryDragIcon()
{
	return static_cast<HCURSOR>(m_hIcon);
}

//	-----------------------------------------------------------
//	commandGimbal
//	-----------------------------------------------------------

void CgeoTuneDlg::commandGimbal()
{
//	static int fpassGeo = 1;
//
//	if(controls.mode == MODE_GEO){
//		if(fpassGeo){
//			targetIndex = 5;
//			targetSet = 0;
//			selectTarget();
//			fpassGeo = 0;
//			return;
//		}
//	}

	if(novAtelCount != steering.gpsCount){
		steering.latitude2 = novAtelLat;
		steering.longitude2 = novAtelLon;
		steering.altitude2 = novAtelhMsl - 116*25.4f/1000.0f;
//		steering.altitude2 = novAtelhMsl;
		steering.gpsCount = novAtelCount;
		steering.gpsMode = novAtelMode;
		sendSteering(&steering);
		return;
	}
}

//	-----------------------------------------------------------
//	OnTimer
//	-----------------------------------------------------------

	int framesub = 0;
	extern int gyroFrame;
	extern int auto_auto_cap;
	extern int new_screen;

void CgeoTuneDlg::OnTimer(UINT_PTR nIDEvent)
{
	if(framesub++ >= 5){	// 15Hz to 5 Hz
		if(!settleCount){
			needFrame = 1;
			framesub = 0;
		}
	}
	if(autotarget){
		if(!settleCount){
			if(!needFrame){
				nextTarget();
			}
		}
	}
	commandGimbal();
	readout();
	if(gyroFrame){
		if(!gwshow){
			new_screen = 0;
		}
		if(auto_auto_cap){
			if(!gwshow){
				gwshow = 1;
				graphD.ShowWindow(gwshow?SW_SHOW:SW_HIDE);
			}
		}
		graphD.gView.setState();
		gyroFrame = 0;
	}

	CDialogEx::OnTimer(nIDEvent);
}

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

void CgeoTuneDlg::setLed(int id,int color)
{
	CStatic *m_picture = (CStatic *)GetDlgItem(id);
	if(NULL == m_picture)return;
	m_picture->SetBitmap(leds[color]);
}

//	-----------------------------------------------------------
//	threads
//	-----------------------------------------------------------

	uintptr_t gimbalThreadHandle = NULL;
	uintptr_t vimbaThreadHandle = NULL;

//	-----------------------------------------------------------
//	gimbalThread
//	-----------------------------------------------------------

void gimbalThread(void *)
{
	while (!quit){
		readGimbalM();
	}
	if(connected){
		setConnect(0);
		Sleep(100);
		closePort();
	}
	gimbalThreadHandle = NULL;
	_endthread();
}

//	-----------------------------------------------------------
//	startGimbalThread
//	-----------------------------------------------------------

void startGimbalThread()
{
	if(gimbalThreadHandle)return;
//	CWnd *pActiveWindow = CWnd::GetActiveWindow();
	gimbalThreadHandle = _beginthread(gimbalThread, 0, NULL);
}

#ifdef _VIMBA

//	-----------------------------------------------------------
//	vimbaThread
//	-----------------------------------------------------------

void vimbaThread(void *)
{
	while (!quit){
		if(m_camera){
			grabFrame();
			readCameraSettings();
		}else{
			Sleep(1000);
		}
	}
	vimbaThreadHandle = NULL;
	_endthread();
}

//	-----------------------------------------------------------
//	startVimbaThread
//	-----------------------------------------------------------

void startVimbaThread()
{
	if(vimbaThreadHandle)return;
//	CWnd *pActiveWindow = CWnd::GetActiveWindow();
	gimbalThreadHandle = _beginthread(vimbaThread, 0, NULL);
}

#endif

//	-----------------------------------------------------------
//	OnBnClickedEnter
//	-----------------------------------------------------------

void CgeoTuneDlg::OnBnClickedEnter()
{
	scanSettings();
	refreshpars = 1;
}

//	-------------------------------------------------------------------
//	OnBnClickedRadioGeo
//	-------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedRadioGeo()
{
	setMode(MODE_GEO);
	(GetDlgItem(IDC_ENTER))->SetFocus();
}

//	-------------------------------------------------------------------
//	OnBnClickedRadioSlave
//	-------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedRadioSlave()
{
	setMode(MODE_SLAVE);
	(GetDlgItem(IDC_ENTER))->SetFocus();
}

//	-------------------------------------------------------------------
//	OnBnClickedRadioRate
//	-------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedRadioRate()
{
	setMode(MODE_RATE);
	(GetDlgItem(IDC_ENTER))->SetFocus();
}

//	-------------------------------------------------------------------
//	OnBnClickedRadioStow
//	-------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedRadioStow()
{
	setMode(MODE_STOW);
	(GetDlgItem(IDC_ENTER))->SetFocus();
}

//	-------------------------------------------------------------------
//	OnBnClickedRadioOuter
//	-------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedRadioOuter()
{
	setMode(MODE_OUTER);
	(GetDlgItem(IDC_ENTER))->SetFocus();
}

//	-------------------------------------------------------------------
//	OnBnClickedRadioStandby
//	-------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedRadioStandby()
{
	setStandby(1);
	(GetDlgItem(IDC_ENTER))->SetFocus();
}

//	-------------------------------------------------------------------
//	OnBnClickedSetSlave
//	-------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedSetSlave()
{
	slavePos[0].focus = focusInt;
	slavePos[0].y = state.axis[0].pos;
	slavePos[0].p = state.axis[1].pos;
	slavePos[0].r = 0;
	refreshpars = 1;
}

//	-------------------------------------------------------------------
//	OnBnClickedSetSlave2
//	-------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedSetSlave2()
{
	slavePos[1].focus = focusInt;
	slavePos[1].y = state.axis[0].pos;
	slavePos[1].p = state.axis[1].pos;
	slavePos[1].r = 0;
	refreshpars = 1;
}

//	-------------------------------------------------------------------
//	setFocus
//	-------------------------------------------------------------------

void setFocus(int f)
{
#ifdef _VIMBA
	if(!cameraConnected)return;
	if(NULL == focusFeature)return;
	VmbInt64_t nValue = f;
	focTime = clock();
	focusFeature->SetValue(nValue);
	vimbaFocus = f;
#endif
}

//	-------------------------------------------------------------------
//	OnBnClickedLoadSlave1
//	-------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedLoadSlave1()
{
	setSlave(slavePos[0].y, slavePos[0].p, 0);
	setFocus(slavePos[0].focus);
	needFrame = 1;
}

//	-------------------------------------------------------------------
//	OnBnClickedLoadSlave2
//	-------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedLoadSlave2()
{
	setSlave(slavePos[1].y, slavePos[1].p, 0);
	setFocus(slavePos[1].focus); 
	needFrame = 1;
}

//	-------------------------------------------------------------------
//	setExposure
//	-------------------------------------------------------------------

void setExposure(double e)
{
#ifdef _VIMBA
	if(!cameraConnected)return;
	if(NULL == exposureFeature)return;
	exposureFeature->SetValue(e*1000);
#endif
}

//	-------------------------------------------------------------------
//	setGain
//	-------------------------------------------------------------------

void setGain(double g)
{
#ifdef _VIMBA
	if(!cameraConnected)return;
	if(NULL == gainFeature)return;
	gainFeature->SetValue(g);
#endif
}

//	-------------------------------------------------------------------
//	setFStop
//	-------------------------------------------------------------------

void setFStop(double s)
{
#ifdef _VIMBA
	if(!cameraConnected)return;
	if(NULL == fstopFeature)return;
	fstopFeature->SetValue(s);
#endif
}

//	-------------------------------------------------------------------
//	getFocus
//	-------------------------------------------------------------------

int getFocus()
{
#ifdef _VIMBA
	if(!cameraConnected)return 0;
	if(NULL == focusFeature)return 0;
	int64_t f;
	focusFeature->GetValue(f);
	return (int)f;
#else
	return 0;
#endif
}

//	-------------------------------------------------------------------
//	getFocusRange
//	-------------------------------------------------------------------

void getFocusRange()
{
#ifdef _VIMBA
	if(!cameraConnected)return;
	if(NULL == focusFeature)return;
	VmbInt64_t nMin, nMax;
	focusFeature->GetRange(nMin, nMax);
	focusMax = (int)nMax;
	focusMin = (int)nMin;
#endif
}

//	-------------------------------------------------------------------
//	getExposure
//	-------------------------------------------------------------------

double getExposure()
{
#ifdef _VIMBA
	if(!cameraConnected)return 0;
	if(NULL == exposureFeature)return 0;
	double e = 0;
	exposureFeature->GetValue(e);
	return e/1000;
#else
	return 0;
#endif
}

//	-------------------------------------------------------------------
//	getGain
//	-------------------------------------------------------------------

double getGain()
{
#ifdef _VIMBA
	if(!cameraConnected)return 0;
	if(NULL == gainFeature)return 0;
	double g = 0;
	gainFeature->GetValue(g);
	return g;
#else
	return 0;
#endif
}

//	-------------------------------------------------------------------
//	getFStop
//	-------------------------------------------------------------------

double getFStop()
{
#ifdef _VIMBA
	if(!cameraConnected)return 0;
	if(NULL == fstopFeature)return 0;
	double g = 0;
	fstopFeature->GetValue(g);
	return g;
#else
	return 0;
#endif
}

//	-------------------------------------------------------------------
//	OnBnClickedLensInit
//	-------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedLensInit()
{
#ifdef _VIMBA
	if(!cameraConnected)return;
	if(NULL == lensInitializeFeature)return;
	lensInitializeFeature->RunCommand();
	focusFeature->SetValue(focusInt);
	getFocusRange();
	refreshpars = 1;
#endif
}

//	-----------------------------------------------------------
//	OnDestroy
//	-----------------------------------------------------------

void CgeoTuneDlg::OnDestroy()
{
	CDialogEx::OnDestroy();

	quit = 1;

	if(timer1){
		KillTimer(timer1);
		timer1 = 0;
	}

#ifdef _VIMBA
	if(cameraConnected){
		if(pCamera){
			pCamera->Close();
		}
	}
	vimbaSys.Shutdown();
#endif

	Sleep(100);
	closePort();
	gps::closePort();
}

//	-----------------------------------------------------------
//	SetFeatureIntValue
//	-----------------------------------------------------------

#ifdef _VIMBA
inline VmbErrorType SetFeatureIntValue(const char *featureName, VmbInt64_t value)
{
 if(SP_ISNULL(pCamera))
	{
		return VmbErrorBadParameter;
	}
	AVT::VmbAPI::FeaturePtr pFeature;
	VmbErrorType result;
	result = SP_ACCESS(pCamera)->GetFeatureByName(featureName, pFeature);
 if(VmbErrorSuccess == result)
	{
		result = SP_ACCESS(pFeature)->SetValue(value);
	}
	return result;
}

//	-----------------------------------------------------------
//	GetFeatureIntValue
//	-----------------------------------------------------------

inline VmbErrorType GetFeatureIntValue(const char *s, int & rvalue)
{
	VmbInt64_t value;

 if(SP_ISNULL(pCamera)){
		return VmbErrorBadParameter;
	}
	AVT::VmbAPI::FeaturePtr pFeature;
	VmbErrorType result;
	result = SP_ACCESS(pCamera)->GetFeatureByName(s, pFeature);
 if(VmbErrorSuccess == result){
		result = SP_ACCESS(pFeature)->GetValue(value);
	}
 if(VmbErrorSuccess == result){
		rvalue = (int)value;
	}
	return result;
}
	int m_nWidth;
	int m_nHeight;
	int m_nPixelFormat;

//	-----------------------------------------------------------
//	setupCamera
//	-----------------------------------------------------------

VmbErrorType setupCamera()
{
	VmbErrorType res;
	AVT::VmbAPI::FeaturePtr pCommandFeature;

	// Set the GeV packet size to the highest possible value
	// (In this example we do not test whether this cam actually is a GigE cam)
	if (VmbErrorSuccess == pCamera->GetFeatureByName("GVSPAdjustPacketSize", pCommandFeature)){
		if (VmbErrorSuccess == pCommandFeature->RunCommand()){
			bool bIsCommandDone = false;
			do {
				if (VmbErrorSuccess != pCommandFeature->IsCommandDone(bIsCommandDone)){
					break;
				}
			} while (false == bIsCommandDone);
		}
	}

	// Save the current width
	res = GetFeatureIntValue("Width", m_nWidth);
	if (VmbErrorSuccess == res)
	{
		// Save current height
		res = GetFeatureIntValue("Height", m_nHeight);
		if (VmbErrorSuccess == res){
			// Set pixel format. For the sake of simplicity we only support Mono and BGR in this example.
			// Try to set BGR
			res = SetFeatureIntValue("PixelFormat", VmbPixelFormatBgr8);
			if (VmbErrorSuccess != res){
				// Fall back to Mono
				res = SetFeatureIntValue("PixelFormat", VmbPixelFormatMono8);
			}
			// Read back the currently selected pixel format
			res = GetFeatureIntValue("PixelFormat", m_nPixelFormat);
		}
	}
	if (VmbErrorSuccess != res)
	{
		// If anything fails after opening the camera we close it
		SP_ACCESS(pCamera)->Close();
	}

	return res;
}

//	----------------------------------------------------------------------------
//	grabFrame
//	----------------------------------------------------------------------------

#define NUM_COLORS 3
#define BIT_DEPTH 8
#define _double_buffer 0

void grabFrame()
{
	if(!cameraConnected)return;
	if(!((needFrame)||(saveFrame)))return;
	needFrame = 0;

	AVT::VmbAPI::FramePtr pFrame;
	VmbUint32_t nHeight;
	VmbUint32_t nWidth;
	VmbFrameStatusType framestatus;
	VmbErrorType err;
	VmbError_t Result;
	VmbUchar_t *pBuffer;
	VmbUchar_t *pColorBuffer = NULL;
	VmbUint32_t nSize;
	VmbPixelFormatType ePixelFormat;

	static int inx = 0;
	static HRESULT res = 0;
	pCamera->AcquireSingleImage(pFrame, 1000); if(SP_ISNULL(pFrame))return;
	pFrame->GetReceiveStatus(framestatus); if(VmbFrameStatusComplete != framestatus)return;
	//	pFrame->GetHeight(nWidth);
	//	pFrame->GetWidth(nHeight);
	pFrame->GetHeight(nHeight);
	pFrame->GetWidth(nWidth);

	err = pFrame->GetImage(pBuffer);		 if(VmbErrorSuccess != err)return;
	err = pFrame->GetImageSize(nSize);		 if(VmbErrorSuccess != err)return;
	err = pFrame->GetPixelFormat(ePixelFormat); if(VmbErrorSuccess != err)return;

	 if(NULL == m_Image){
			m_Image.Create(nWidth, -(int)nHeight, NUM_COLORS * BIT_DEPTH);
			m_Imagx.Create(nWidth, -(int)nHeight, NUM_COLORS * BIT_DEPTH);
		}
	 if(NULL == m_Image)return;
		int nStride1 = m_Image.GetPitch();
		int nBitsPerPixel = m_Image.GetBPP();
		int nStride2 = (nWidth*nBitsPerPixel) / 8;
	 if(nStride2 != nStride1)return;

	VmbImage SourceImage;
	VmbImage DestinationImage;

	SourceImage.Size = sizeof(SourceImage);
	DestinationImage.Size = sizeof(DestinationImage);

	SourceImage.Data = pBuffer;

#if _double_buffer
	DestinationImage.Data = m_Imagx.GetBits();
#else
	DestinationImage.Data = m_Image.GetBits();
#endif

	Result = VmbSetImageInfoFromPixelFormat(ePixelFormat, nWidth, nHeight, &SourceImage);

	static const std::string DisplayFormat("BGR24");
	Result = VmbSetImageInfoFromString(DisplayFormat.c_str(), DisplayFormat.size(), nWidth, nHeight, &DestinationImage);
	 if(VmbErrorSuccess != Result)return;

	Result = VmbImageTransform(&SourceImage, &DestinationImage, NULL, 0);
	 if(VmbErrorSuccess != Result){
			//Log(_TEXT("Error transforming image."), static_cast<VmbErrorType>(Result));
	}else{
#if _double_buffer
		m_Imagx.BitBlt(m_Image.GetDC(), 0, 0);
#endif
		int dt = clock() - focTime;
		newImage = 1;
		imageOk = 1;
		if(saveFrame){
			res = m_Image.Save(fname(dt,"images\\test",inx++,"jpg"),ImageFormatJPEG);
			saveFrame = 0;
		}
	}
}
#endif

//	-----------------------------------------------------------
//	upd
//	-----------------------------------------------------------

void upd(double *dest, double src, int *flag)
{
	char s[20];
	double val;
	int res = 1;
	sprintf_s(s, sizeof(s), "%1.7f", *dest);
 if(1 == sscanf_s(s, "%lf", &val)){
	 if(src == val)res = 0;
	}
 if(res){
		*flag = 1;
		*dest = src;
	}
}

//	-----------------------------------------------------------
//	upd
//	-----------------------------------------------------------

void upd(float *dest, float src, int *flag)
{
	char s[20];
	float val;
	int res = 1;
	sprintf_s(s, sizeof(s), "%1.4f", *dest);
 if(1 == sscanf_s(s, "%f", &val)){
	 if(src == val)res = 0;
	}
 if(res){
		*flag = 1;
		*dest = src;
	}
}
//	-----------------------------------------------------------
//	upd
//	-----------------------------------------------------------

void upd(__int16 *dest, __int16 src, int *flag)
{
 if(src != *dest){
		*flag = 1;
		*dest = src;
	}
}

//	-----------------------------------------------------------
//	upd
//	-----------------------------------------------------------

void upd(int *dest, int src, int *flag)
{
 if(src != *dest){
		*flag = 1;
		*dest = src;
	}
}
//	-----------------------------------------------------------
//	setText
//	-----------------------------------------------------------

void CgeoTuneDlg::setText(int id,char *s)
{
	char s0[200];
	GetDlgItemText(id,s0,sizeof(s0));
	if(strcmp(s,s0)){
		SetDlgItemText(id, (LPCTSTR)s);
	}
}

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

void CgeoTuneDlg::readout()
{	

	graphD.readout();
	if(newlog){
		m_log_edit.SetWindowTextA(logText2);
//		int minpos;
//		int maxpos;
//		m_log_edit.GetScrollRange(SB_VERT ,&minpos,&maxpos);
//		m_log_edit.SetScrollPos(SB_VERT,maxpos);
		int last = logText2.GetLength()-1;
		if(last>0)m_log_edit.SetSel(last,last);

		newlog = 0;
	}

	char s[200];
	sprintf_s(s, sizeof(s), "%i", frames); SetDlgItemText(IDC_FRAMES, (LPCTSTR)s);
//	setLed(IDC_LED1, (cameraConnected) ? (LED_GREEN) : (LED_RED));
	setLed(IDC_LED2, (state.navbox.NovatelHz > 2) ? (LED_GREEN) : (LED_RED));

	sprintf_s(s, sizeof(s), "%i", state.gyro.misc0); SetDlgItemText(IDC_MISC0, (LPCTSTR)s);
	sprintf_s(s, sizeof(s), "%i", state.gyro.misc1); SetDlgItemText(IDC_MISC1, (LPCTSTR)s);
	sprintf_s(s, sizeof(s), "%i", state.gyro.misc2); SetDlgItemText(IDC_MISC2, (LPCTSTR)s);
	sprintf_s(s, sizeof(s), "%i", state.gyro.misc3); SetDlgItemText(IDC_MISC3, (LPCTSTR)s);

	if(gimbalConnected){
		setLed(IDC_LED3, (vn200Connected) ? (LED_GREEN) : (LED_RED));
		setLed(IDC_LED4, (gyroConnected) ? (LED_GREEN) : (LED_RED));
		setLed(IDC_LED5, (gimbalConnected) ? (LED_GREEN) : (LED_RED));
		setLed(IDC_LED6, (settleCount) ? (LED_YELLOW) : (LED_GREEN));
		setLed(IDC_LED8, (controls.autonull) ? (LED_YELLOW) : (LED_GREEN));
	}else{
		setLed(IDC_LED3, LED_GREY);	// vn200Connected
		setLed(IDC_LED4, LED_GREY);	// gyroConnected
		setLed(IDC_LED5, LED_RED);	// gimbalConnected
		setLed(IDC_LED6, LED_GREY);	// controls.standby
		setLed(IDC_LED8, LED_GREY);	// controls.autonull
	}

	int ii = 4;
	if(vn200Connected){
		ii=state.nav.InsStatus;
		ii &= 0x03;
	}

	switch(ii){
		case 0: setLed(IDC_LED7, LED_RED	);SetDlgItemText(IDC_VN200_STATE,"VN200 Not tracking"		); break;
		case 1: setLed(IDC_LED7, LED_YELLOW	);SetDlgItemText(IDC_VN200_STATE,"VN200 Aligning (2deg)"	); break;
		case 2: setLed(IDC_LED7, LED_GREEN	);SetDlgItemText(IDC_VN200_STATE,"VN200 Tracking"			); break;
		case 3: setLed(IDC_LED7, LED_RED	);SetDlgItemText(IDC_VN200_STATE,"VN200 Loss of GPS"		); break;
		default: setLed(IDC_LED7, LED_GREY	);SetDlgItemText(IDC_VN200_STATE,"VN200 Not Connected"		); break;
	};
	((CButton *)(GetDlgItem(IDC_RADIO_T1 )))->SetCheck(targetIndex== 0);
	((CButton *)(GetDlgItem(IDC_RADIO_T2 )))->SetCheck(targetIndex== 1);
	((CButton *)(GetDlgItem(IDC_RADIO_T3 )))->SetCheck(targetIndex== 2);
	((CButton *)(GetDlgItem(IDC_RADIO_T4 )))->SetCheck(targetIndex== 3);
	((CButton *)(GetDlgItem(IDC_RADIO_T5 )))->SetCheck(targetIndex== 4);
	((CButton *)(GetDlgItem(IDC_RADIO_T6 )))->SetCheck(targetIndex== 5);
	((CButton *)(GetDlgItem(IDC_RADIO_T7 )))->SetCheck(targetIndex== 6);
	((CButton *)(GetDlgItem(IDC_RADIO_T8 )))->SetCheck(targetIndex== 7);
	((CButton *)(GetDlgItem(IDC_RADIO_T9 )))->SetCheck(targetIndex== 8);
	((CButton *)(GetDlgItem(IDC_RADIO_T10)))->SetCheck(targetIndex== 9);
	((CButton *)(GetDlgItem(IDC_RADIO_T11)))->SetCheck(targetIndex==10);

	setText(IDC_TARGET_NAME_1,  targetString(0));
	setText(IDC_TARGET_NAME_2,  targetString(1));
	setText(IDC_TARGET_NAME_3,  targetString(2));
	setText(IDC_TARGET_NAME_4,  targetString(3));
	setText(IDC_TARGET_NAME_5,  targetString(4));
	setText(IDC_TARGET_NAME_6,  targetString(5));
	setText(IDC_TARGET_NAME_7,  targetString(6));
	setText(IDC_TARGET_NAME_8,  targetString(7));
	setText(IDC_TARGET_NAME_9,  targetString(8));
	setText(IDC_TARGET_NAME_10, targetString(9));
	setText(IDC_TARGET_NAME_11, targetString(10));

	ranges();
	setText(IDC_TARGET_RANGE_1,  rangeString(0));
	setText(IDC_TARGET_RANGE_2,  rangeString(1));
	setText(IDC_TARGET_RANGE_3,  rangeString(2));
	setText(IDC_TARGET_RANGE_4,  rangeString(3));
	setText(IDC_TARGET_RANGE_5,  rangeString(4));
	setText(IDC_TARGET_RANGE_6,  rangeString(5));
	setText(IDC_TARGET_RANGE_7,  rangeString(6));
	setText(IDC_TARGET_RANGE_8,  rangeString(7));
	setText(IDC_TARGET_RANGE_9,  rangeString(8));
	setText(IDC_TARGET_RANGE_10, rangeString(9));
	setText(IDC_TARGET_RANGE_11, rangeString(10));

 if(controls.standby){
		((CButton *)(GetDlgItem(IDC_RADIO_GEO)))->SetCheck(0);
		((CButton *)(GetDlgItem(IDC_RADIO_RATE)))->SetCheck(0);
		((CButton *)(GetDlgItem(IDC_RADIO_SLAVE)))->SetCheck(0);
		((CButton *)(GetDlgItem(IDC_RADIO_STOW)))->SetCheck(0);
		((CButton *)(GetDlgItem(IDC_RADIO_OUTER)))->SetCheck(0);
		((CButton *)(GetDlgItem(IDC_RADIO_STANDBY)))->SetCheck(1);
	}else{
		((CButton *)(GetDlgItem(IDC_RADIO_GEO)))->SetCheck(controls.mode == MODE_GEO);
		((CButton *)(GetDlgItem(IDC_RADIO_RATE)))->SetCheck(controls.mode == MODE_RATE);
		((CButton *)(GetDlgItem(IDC_RADIO_SLAVE)))->SetCheck(controls.mode == MODE_SLAVE);
		((CButton *)(GetDlgItem(IDC_RADIO_STOW)))->SetCheck(controls.mode == MODE_STOW);
		((CButton *)(GetDlgItem(IDC_RADIO_OUTER)))->SetCheck(controls.mode == MODE_OUTER);
		((CButton *)(GetDlgItem(IDC_RADIO_STANDBY)))->SetCheck(0);
	}

	sprintf_s(s, sizeof(s), " %i", (int)focusInt);						setText(IDC_FOCUS, s);
	sprintf_s(s, sizeof(s), " %i", (int)focusMax);						setText(IDC_FOCUS_MAX, s);
	sprintf_s(s, sizeof(s), " %i", (int)focusMin);						setText(IDC_FOCUS_MIN, s);
	sprintf_s(s, sizeof(s), " %2.2f", state.axis[0].pos);				setText(IDC_YAW, s);
	sprintf_s(s, sizeof(s), " %2.2f", state.axis[1].pos);				setText(IDC_PITCH, s);

	setText(IDC_FOCUS_M, focus2MeterS(focusInt));
	setText(IDC_FOCUS_M1,focus2MeterS(slavePos[0].focus));
	setText(IDC_FOCUS_M2,focus2MeterS(slavePos[1].focus));

	sprintf_s(s, sizeof(s), " %1.7lf ", state.nav.PosLla[0]);			setText(IDC_VEC_LAT,s);
	sprintf_s(s, sizeof(s), " %1.7lf ", state.nav.PosLla[1]);			setText(IDC_VEC_LON,s);
	sprintf_s(s, sizeof(s), " %1.1lf ", state.nav.PosLla[2]);			setText(IDC_VEC_ALT,s);
	sprintf_s(s, sizeof(s), " %1.7lf ", state.geo.cameraL[0]);			setText(IDC_UB_LAT,s);
	sprintf_s(s, sizeof(s), " %1.7lf ", state.geo.cameraL[1]);			setText(IDC_UB_LON,s);
	sprintf_s(s, sizeof(s), " %1.1f ",  state.geo.cameraL[2]);			setText(IDC_UB_ALT,s);
	sprintf_s(s, sizeof(s), " %1.1f ",  novAtelhMsl);					setText(IDC_UB_ALT2,s);

	sprintf_s(s, sizeof(s), " %1.2f ", state.geo.losError[0]);			setText(IDC_LOS_ERR_Y,s);
	sprintf_s(s, sizeof(s), " %1.2f ", state.geo.losError[1]);			setText(IDC_LOS_ERR_P,s);
	sprintf_s(s, sizeof(s), " %1.2f ", state.geo.losError[2]);			setText(IDC_LOS_ERR_R,s);

	 if(refreshpars){

		sprintf_s(s, sizeof(s), "%1.6f", targetLat);					setText(IDC_EDIT_TARGET_LAT, s);
		sprintf_s(s, sizeof(s), "%1.6f", targetLon);					setText(IDC_EDIT_TARGET_LON, s);
		sprintf_s(s, sizeof(s), "%1.1f", targetAlt);					setText(IDC_EDIT_TARGET_ALT, s);
		sprintf_s(s, sizeof(s), "%1.2f", controls.axis[0].desiredPos);	setText(IDC_DESIRED_YAW, s);
		sprintf_s(s, sizeof(s), "%1.2f", controls.axis[1].desiredPos);	setText(IDC_DESIRED_PITCH, s);
		sprintf_s(s, sizeof(s), "%1.2f", slavePos[0].y);				setText(IDC_EDIT_Y, s);
		sprintf_s(s, sizeof(s), "%1.2f", slavePos[0].p);				setText(IDC_EDIT_P, s);
		sprintf_s(s, sizeof(s), "%i", (int)slavePos[0].focus);			setText(IDC_EDIT_FOCUS, s);
		sprintf_s(s, sizeof(s), "%1.2f", slavePos[1].y);				setText(IDC_EDIT_Y3, s);
		sprintf_s(s, sizeof(s), "%1.2f", slavePos[1].p);				setText(IDC_EDIT_P3, s);
		sprintf_s(s, sizeof(s), "%i", (int)slavePos[1].focus);			setText(IDC_EDIT_FOCUS2, s);
		sprintf_s(s, sizeof(s), "%i", getFocus());						setText(IDC_FOCUS, s);
		sprintf_s(s, sizeof(s), "%i", (int)focusInt);					setText(IDC_EDIT_FOCUS3, s);
																		setText(IDC_FOCUS_M3,focus2MeterS(focusInt));
		sprintf_s(s, sizeof(s), "%1.0f",exposureTimeAbs);				setText(IDC_EDIT_EXPOSURE, s);
		sprintf_s(s, sizeof(s), "%1.1f",cameraGain);					setText(IDC_EDIT_GAIN, s);
		sprintf_s(s, sizeof(s), "%1.2f",fstop);							setText(IDC_EDIT_FSTOP, s);
	}
	refreshpars = 0;

 if(newImage){
		newImage = 0;
		imageOk = 1;
		if(1){
			RECT rectx;
			GetWindowRect(&rectx);
			ScreenToClient(&rectx);
			InvalidateRect(&rectx, false);
		}else{
			m_cameraImage.InvalidateRect(&m_cameraImage.rect, false);
		}
	}
}

//	-------------------------------------------------------------------
//	scanSettings
//	-------------------------------------------------------------------

void CgeoTuneDlg::scanSettings(void)
{
	char s[81];
	int ival;
	float fval;
	double dval;
	double dval1;
	double dval2;
	double dval3;

	int res = 0;
	GetDlgItemText(IDC_EDIT_TARGET_LAT, 	s, sizeof(s)); if(1 == sscanf_s(s, "%lf", &dval1)) upd(&state.geo.targetL[0],			dval1, &res);
	GetDlgItemText(IDC_EDIT_TARGET_LON, 	s, sizeof(s)); if(1 == sscanf_s(s, "%lf", &dval2)) upd(&state.geo.targetL[1],			dval2, &res);
	GetDlgItemText(IDC_EDIT_TARGET_ALT, 	s, sizeof(s)); if(1 == sscanf_s(s, "%lf", &dval3)) upd(&state.geo.targetL[2],			dval3, &res);
	if(res){
		selectTarget(dval1,dval2,dval3);
	}

	res = 0;
	GetDlgItemText(IDC_EDIT_Y, 				s, sizeof(s)); if(1 == sscanf_s(s, "%f", &fval)) upd(&slavePos[0].y,					fval, &res);
	GetDlgItemText(IDC_EDIT_P,				s, sizeof(s)); if(1 == sscanf_s(s, "%f", &fval)) upd(&slavePos[0].p,					fval, &res);
	GetDlgItemText(IDC_EDIT_FOCUS,			s, sizeof(s)); if(1 == sscanf_s(s, "%i", &ival)) upd(&slavePos[0].focus,				ival, &res);
	GetDlgItemText(IDC_EDIT_Y3,				s, sizeof(s)); if(1 == sscanf_s(s, "%f", &fval)) upd(&slavePos[1].y,					fval, &res);
	GetDlgItemText(IDC_EDIT_P3,				s, sizeof(s)); if(1 == sscanf_s(s, "%f", &fval)) upd(&slavePos[1].p,					fval, &res);
	GetDlgItemText(IDC_EDIT_FOCUS2,			s, sizeof(s)); if(1 == sscanf_s(s, "%i", &ival)) upd(&slavePos[1].focus,				ival, &res);
	if(res){
	}

	res = 0;
	GetDlgItemText(IDC_DESIRED_YAW, 		s, sizeof(s)); if(1 == sscanf_s(s, "%f", &fval)) upd(&controls.axis[0].desiredPos,		fval, &res);
	GetDlgItemText(IDC_DESIRED_PITCH,		s, sizeof(s)); if(1 == sscanf_s(s, "%f", &fval)) upd(&controls.axis[1].desiredPos,		fval, &res);
	if(res){
		sendControls(&controls);
	}

	res = 0;
	int ifocus = getFocus();
	GetDlgItemText(IDC_EDIT_FOCUS3, s, sizeof(s)); if(1 == sscanf_s(s, "%i", &ival)) upd(&ifocus, ival, &res);
	if(res){
		setFocus(ival);
	}

	res = 0;
	double iexposure = getExposure();
	GetDlgItemText(IDC_EDIT_EXPOSURE, s, sizeof(s)); if(1 == sscanf_s(s, "%lf", &dval)) upd(&iexposure, dval, &res);
	if(res){
		setExposure(dval);
	}

	res = 0;
	double gain = getGain();
	GetDlgItemText(IDC_EDIT_GAIN, s, sizeof(s)); if(1 == sscanf_s(s, "%lf", &dval)) upd(&gain, dval, &res);
	if(res){
		setGain(dval);
	}

	res = 0;
	double fstop = getFStop();
	GetDlgItemText(IDC_EDIT_FSTOP, s, sizeof(s)); if(1 == sscanf_s(s, "%lf", &dval)) upd(&fstop, dval, &res);
	if(res){
		setFStop(dval);
	}

	refreshpars = 1;
}

//	-------------------------------------------------------------------
//	OnStnClickedCameraImage
//	-------------------------------------------------------------------

void CgeoTuneDlg::OnStnClickedCameraImage()
{
	m_fitWidth = (m_fitWidth)?(0):(1);
	m_ClearBackground = true;
}
//	-----------------------------------------------------------
//	render
//	-----------------------------------------------------------

void CgeoTuneDlg::render()
{
#if 0
//	CPen greyPen;

 if(!dcOK)return;
 if(NULL == m_Image)return;

//	CPaintDC dc(&m_cameraImage);
//	dc.SetGraphicsMode(GM_ADVANCED);
//	CRect rect;
//	m_cameraImage.GetClientRect(&rect);
// if(m_ClearBackground)
//	{
//		m_ClearBackground = false;
//		CBrush clearBrush(GetSysColor(COLOR_BTNFACE));
//		dc.FillRect(rect, &clearBrush);
//	}

	float hOut = (float)(rect.bottom - rect.top);
	float wOut = (float)(rect.right - rect.left);
	float hIn = (float)(m_Image.GetWidth());
	float wIn = (float)(m_Image.GetHeight());

	float sY = hOut / hIn;
	float sX = wOut / wIn;

	float s;		// scaling
	float oX;		// X offset
	float oY;		// Y offset

 if((sY < sX) ^ ((bool)m_fitWidth)){
		s = sY;						// fill Y, center X
		oX = wOut - (wOut - s * wIn) / 2;
		oY = 0;
	}else{
		s = sX;						// fill X, center Y
		oY = (hOut - s * hIn) / 2;
		oX = wOut;
	}

	XFORM xForm;

	xForm.eM11 = 0;
	xForm.eM21 = -s;
	xForm.eM12 = s;
	xForm.eM22 = 0;
	xForm.eDx = oX;
	xForm.eDy = oY;

//	dc.SetGraphicsMode(GM_ADVANCED);
//	dc.SetWorldTransform(&xForm);
	dc.MoveTo(rect.left,rect.top);
	dc.LineTo(rect.right,rect.bottom);
	dc.MoveTo(0,0);
	dc.LineTo(npixx,npixy);
//	dbitmap.bitb
//	dbm_cameraImage.GetDC();
//	m_Image.BitBlt(dc.m_hDC, 0, 0);
#endif
}

#if 0
void OnPaint()
{
 if(IsIconic())
	{
		CPaintDC dc(this); // device context for painting

		SendMessage(WM_ICONERASEBKGND, reinterpret_cast<WPARAM>(dc.GetSafeHdc()), 0);

		// Center icon in client rectangle
		int cxIcon = GetSystemMetrics(SM_CXICON);
		int cyIcon = GetSystemMetrics(SM_CYICON);
		CRect rect;
		GetClientRect(&rect);
		int x = (rect.Width() - cxIcon + 1) / 2;
		int y = (rect.Height() - cyIcon + 1) / 2;

		// Draw the icon
		dc.DrawIcon(x, y, m_hIcon);
	}else{
		CDialogEx::OnPaint();

	 if(NULL != m_Image)
		{
			CPaintDC dc(&m_cameraImage);
			dc.SetGraphicsMode(GM_ADVANCED);
			CRect rect;
			m_cameraImage.GetClientRect(&rect);
		 if(m_ClearBackground)
			{
				m_ClearBackground = false;
				CBrush clearBrush(GetSysColor(COLOR_BTNFACE));
				dc.FillRect(rect, &clearBrush);
			}

			float hOut = (float)(rect.bottom - rect.top);
			float wOut = (float)(rect.right - rect.left);
			float hIn = (float)(m_Image.GetWidth());
			float wIn = (float)(m_Image.GetHeight());

			float sY = hOut / hIn;
			float sX = wOut / wIn;

			float s;		// scaling
			float oX;		// X offset
			float oY;		// Y offset

		 if((sY < sX) ^ ((bool)m_fitWidth)){
				s = sY;						// fill Y, center X
				oX = wOut - (wOut - s * wIn) / 2;
				oY = 0;
			}else{
				s = sX;						// fill X, center Y
				oY = (hOut - s * hIn) / 2;
				oX = wOut;
			}

			XFORM xForm;

			xForm.eM11 = 0;
			xForm.eM21 = -s;
			xForm.eM12 = s;
			xForm.eM22 = 0;
			xForm.eDx = oX;
			xForm.eDy = oY;

			dc.SetGraphicsMode(GM_ADVANCED);
			dc.SetWorldTransform(&xForm);
			m_Image.BitBlt(dc.m_hDC, 0, 0);

		}
	}
}
#endif

#if 1
void CgeoTuneDlg::OnPaint()	// golden
{
 if(IsIconic())
	{
		CPaintDC dc(this); // device context for painting

		SendMessage(WM_ICONERASEBKGND, reinterpret_cast<WPARAM>(dc.GetSafeHdc()), 0);

		// Center icon in client rectangle
		int cxIcon = GetSystemMetrics(SM_CXICON);
		int cyIcon = GetSystemMetrics(SM_CYICON);
		CRect rect;
		GetClientRect(&rect);
		int x = (rect.Width() - cxIcon + 1) / 2;
		int y = (rect.Height() - cyIcon + 1) / 2;

		// Draw the icon
		dc.DrawIcon(x, y, m_hIcon);
	}else{
		CDialogEx::OnPaint();

	 if(NULL != m_Image)
		{
			CPaintDC dc(&m_cameraImage);
			dc.SetGraphicsMode(GM_ADVANCED);
			CRect rect;
			m_cameraImage.GetClientRect(&rect);
		 if(m_ClearBackground)
			{
				m_ClearBackground = false;
				CBrush clearBrush(GetSysColor(COLOR_BTNFACE));
				dc.FillRect(rect, &clearBrush);
			}

			float hOut = (float)(rect.bottom - rect.top);
			float wOut = (float)(rect.right - rect.left);
			float hIn = (float)(m_Image.GetWidth());
			float wIn = (float)(m_Image.GetHeight());

			float sY = hOut / hIn;
			float sX = wOut / wIn;

			float s;		// scaling
			float oX;		// X offset
			float oY;		// Y offset

		 if((sY < sX) ^ ((bool)m_fitWidth)){
				s = sY;						// fill Y, center X
				oX = wOut - (wOut - s * wIn) / 2;
				oY = 0;
			}else{
				s = sX;						// fill X, center Y
				oY = (hOut - s * hIn) / 2;
				oX = wOut;
			}

			XFORM xForm;

			xForm.eM11 = 0;
			xForm.eM21 = -s;
			xForm.eM12 = s;
			xForm.eM22 = 0;
			xForm.eDx = oX;
			xForm.eDy = oY;

			dc.SetGraphicsMode(GM_ADVANCED);
			dc.SetWorldTransform(&xForm);
			m_Image.BitBlt(dc.m_hDC, 0, 0);

		}
	}
}
#else
//	-----------------------------------------------------------
//	OnPaint
//	-----------------------------------------------------------

void CgeoTuneDlg::OnPaint()
{
	CPaintDC hwdc(this); // device context for painting
#if 0	
	return;

	painting = 1;

 if(dcOK == 0){

		//		GetClientRect(&rect);
		//		m_cameraImage.GetClientRect(&rect);
		//		ScreenToClient(&rect);
		npixx = rect.Width();
		npixy = rect.Height();
		dc.CreateCompatibleDC(&hwdc);
		//		dc.SetGraphicsMode(GM_ADVANCED);
		dbitmap.CreateCompatibleBitmap(&hwdc, rect.right, rect.bottom);
		CBrush bg;
		//		bg.CreateSolidBrush(GetSysColor(COLOR_BTNFACE));
		bg.CreateSolidBrush(0x0000ff);
		dc.SelectObject(dbitmap);
		CRect rec(rect);
		dc.FillRect(&rec, &bg);
		dcOK = 1;
		//inits();
	}
	render();
	CRect rectx = rect;
	ScreenToClient(&rectx);
	hwdc.BitBlt(0, 0, rect.right, rect.bottom, &dc, rectx.left, 0, SRCCOPY);

	painting = 0;
#endif
}
#endif

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

//#define PI (3.14159265358979323846)

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

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

//	----------------------------------------------------------------------------
//	ecefToGeodetic
//	----------------------------------------------------------------------------

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 triangulateTarget(double(*position1)[3], float(*los1D)[3],
	double(*position2)[3], float(*los2D)[3],
	double(*positiont)[3])
{
	double tanYaw1, tanYaw2;
	double ecef1[3], ecef2[3], diff[3];
	double point1[3], point2[3], point3[3], point4[3], pointa[3], pointb[3];
	double Re2tV[3][3];
	double target[3], targetecef[3];
	int i;
	double los1[3] = { toRad*(*los1D)[YAW],toRad*(*los1D)[PIT],toRad*(*los1D)[ROL] };
	double los2[3] = { toRad*(*los2D)[YAW],toRad*(*los2D)[PIT],toRad*(*los2D)[ROL] };

	tanYaw1 = tan(los1[YAW]);
	tanYaw2 = tan(los2[YAW]);
 if(fabs(tanYaw1 - tanYaw2) < (1.0*toRad)){
		return false;
	}

	geodeticToEcef(position1, &ecef1);
	geodeticToEcef(position2, &ecef2);

	earthToRe2t(position1, &Re2tV);
	for (i = 0; i < 3; i++)
		diff[i] = ecef2[i] - ecef1[i];
	point1[X] = point1[Y] = point1[Z] = 0.0;
	mult3by3times3by1(&Re2tV, &diff, &point3);

	point2[X] = cos(los1[YAW])*cos(los1[PIT]);
	point2[Y] = sin(los1[YAW])*cos(los1[PIT]);
	point2[Z] = -sin(los1[PIT]);

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

	LineLineIntersect(&point1, &point2, &point3, &point4, &pointa, &pointb);

	for (i = 0; i < 3; i++)
		target[i] = (pointa[i] + pointb[i]) / 2.0;

	multT3by3times3by1(&Re2tV, &target, &targetecef);

	for (i = 0; i < 3; i++)
		targetecef[i] += ecef1[i];
	ecefToGeodetic(&targetecef, positiont);
	//	ecefToGeodetic(&ecef2,positiont);

	return true;
}

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

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

#if 0

//	----------------------------------------------------------------------------
//	on_pushButton_5_clicked Sample L1
//	----------------------------------------------------------------------------

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 Sample L2
//	----------------------------------------------------------------------------

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();
	targetLat = t[0];
	targetLon = t[1];
	targetAlt = 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++);

}

#endif

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

void ranges(){

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

	geodeticToEcef(&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 = max(maxTargetRange, targets[targetSet][itarg].range);
			minTargetRange = min(minTargetRange, targets[targetSet][itarg].range);
		}
	}
#if 0
 if(autotarget){
		for (int itarg = 0; itarg < NTARG; itarg++){
		 if(targets[targetSet][itarg].range == minTargetRange){
				targetIndex = itarg;
				targetLat = targets[targetSet][targetIndex].lat;
				targetLon = targets[targetSet][targetIndex].lon;
				targetAlt = targets[targetSet][targetIndex].alt;
				refreshpars = 1;
				steering.latitude = targetLat;
				steering.longitude = targetLon;
				steering.altitude = (float)targetAlt;
				sendSteering(&steering);
				break;
			}
		}
	}
#endif
	fpass = 0;
}

//	----------------------------------------------------------------------------
//	rangeString
//	----------------------------------------------------------------------------

char *rangeString(unsigned int index)
{
	static char s[80] = "";
 if(index >= NTARG)return s;
 if(0==strcmp(targets[targetSet][targetIndex].desc, "Free")) return s;
	sprintf_s(s,sizeof(s),"%8.0f ", targets[targetSet][index].range);
	return s;
}

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

char *targetString(unsigned int index)
{
	static char s[80] = "";
 if(index >= NTARG)return s;
	sprintf_s(s,sizeof(s)," %s", targets[targetSet][index].desc);
	return s;
}


//	----------------------------------------------------------------------------
//	Focus
//	----------------------------------------------------------------------------
#if 0
	const float i_inf = 2010;
	const float f_inf = 3000;
	const float i_near = 1905;
	const float f_near = 23;
#else
	const float i_inf = 2020;
	const float f_inf = 3000;
	const float i_near = 1900;
	const float f_near = 20;
#endif

	const float foc_B = i_inf;						// Offset (B) of the focus from range equation (F = M * (1/range) + B)
	const float foc_M = (i_near - i_inf)*(f_near);	//  Slope (M) of the focus from range equation (F = M * (1/range) + B)

//	----------------------------------------------------------------------------
//	focus2MeterS
//	----------------------------------------------------------------------------

char *focus2MeterS(int ifoc)
{
	static char s[100];
	float foc = (float)ifoc;
	if(foc < foc_B){
		float range = foc_M / (foc - foc_B);
		sprintf_s(s, sizeof(s), " %1.0f", range);
		return s;
	}else{
		return " inf";
	}
}

//	----------------------------------------------------------------------------
//	setFocusM
//	----------------------------------------------------------------------------

void setFocusM(float range)
{
	float foc = foc_B + foc_M/range;
	setFocus((int)foc);
}

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

void CgeoTuneDlg::selectTarget()
{
//	autotarget = 0;
	targetLat = targets[targetSet][targetIndex].lat;
	targetLon = targets[targetSet][targetIndex].lon;
	targetAlt = targets[targetSet][targetIndex].alt;
	steering.latitude = targetLat;
	steering.longitude = targetLon;
	steering.altitude = (float)targetAlt;
	setFocusM(targets[targetSet][targetIndex].range);
	sendSteering(&steering);
	settleCount = 4;
	refreshpars = 1;
}

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

void CgeoTuneDlg::selectTarget(double lat,double lon, double alt)
{
	autotarget = 0;
	targetLat = lat;
	targetLon = lon;
	targetAlt = alt;
	refreshpars = 1;
	steering.latitude = targetLat;
	steering.longitude = targetLon;
	steering.altitude = (float)targetAlt;
	refreshpars = 1;
//	setFocusM(targets[targetSet][targetIndex].range);
	sendSteering(&steering);
	settleCount = 4;
}

//	----------------------------------------------------------------------------
//	OnBnClickedAutoTarget
//	----------------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedAutoTarget()
{
	autotarget = autotarget ? 0 : 1;
	(GetDlgItem(IDC_ENTER))->SetFocus();
}

//	----------------------------------------------------------------------------
//	prevTarget
//	----------------------------------------------------------------------------

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

//	----------------------------------------------------------------------------
//	OnBnClickedPrevTarget
//	----------------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedPrevTarget()
{
	prevTarget();
	(GetDlgItem(IDC_ENTER))->SetFocus();
}

//	----------------------------------------------------------------------------
//	OnBnClickedNextTarget
//	----------------------------------------------------------------------------

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

//	----------------------------------------------------------------------------
//	OnBnClickedNextTarget
//	----------------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedNextTarget()
{
	nextTarget();
	(GetDlgItem(IDC_ENTER))->SetFocus();
}

//	----------------------------------------------------------------------------
//	OnBnClickedPrevSet
//	----------------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedPrevSet()
{
 if(targetSet > 0){
		targetIndices[targetSet] = targetIndex;
		targetSet--;
		targetIndex = targetIndices[targetSet];
	}
	selectTarget();
	(GetDlgItem(IDC_ENTER))->SetFocus();
}

//	----------------------------------------------------------------------------
//	OnBnClickedNextSet
//	----------------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedNextSet()
{
 if(targetSet < (NTARS - 1)){
		targetIndices[targetSet] = targetIndex;
		targetSet++;
		targetIndex = targetIndices[targetSet];
	}
	selectTarget();
	(GetDlgItem(IDC_ENTER))->SetFocus();

}

//	-------------------------------------------------------------------
//	fname - filename
//	-------------------------------------------------------------------

#include <sys/timeb.h>

char *fname(int dt,char *type,int inx, char *ext)
{
	static char filename[256];
	struct tm newtime;
	time_t aclock;
	timeb tb;
	ftime(&tb);

	time( &aclock );					 // Get time in seconds
	localtime_s( &newtime, &aclock );	 // Convert time to struct

	sprintf_s( filename, sizeof(filename), "%s-%04i-%02i%02i%02i-%02i%02i%02i.%03i-%04i-%04i.%s",
		type,							// type
		dt,							// type
		newtime.tm_year-100,			// Year (current year minus 1900)
		newtime.tm_mon+1,				// Month (0 to 11; January = 0) 
		newtime.tm_mday,				// Day of month (1 to 31) 
		newtime.tm_hour,				// Hours since midnight (0 to 23) 
		newtime.tm_min,					// Minutes after hour (0 to 59) 
		newtime.tm_sec,					// Seconds after minute (0 to 59)
		tb.millitm,
		vimbaFocus,
		inx,							// inx
		ext								// File Extension
		);
	return filename;
}

//	int nCount = tb.millitm + (tb.time & 0xfffff) * 1000;

//	-------------------------------------------------------------------
//	OnBnClickedAutonull
//	-------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedAutonull()
{
	setAutonull(controls.autonull?0:1);
}

//	-------------------------------------------------------------------
//	OnBnClickedRadioTx
//	-------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedRadioT1(){		targetIndex =  0; selectTarget();	}
void CgeoTuneDlg::OnBnClickedRadioT2(){		targetIndex =  1; selectTarget();	}
void CgeoTuneDlg::OnBnClickedRadioT3(){		targetIndex =  2; selectTarget();	}
void CgeoTuneDlg::OnBnClickedRadioT4(){		targetIndex =  3; selectTarget();	}
void CgeoTuneDlg::OnBnClickedRadioT5(){		targetIndex =  4; selectTarget();	}
void CgeoTuneDlg::OnBnClickedRadioT6(){		targetIndex =  5; selectTarget();	}
void CgeoTuneDlg::OnBnClickedRadioT7(){		targetIndex =  6; selectTarget();	}
void CgeoTuneDlg::OnBnClickedRadioT8(){		targetIndex =  7; selectTarget();	}
void CgeoTuneDlg::OnBnClickedRadioT9(){		targetIndex =  8; selectTarget();	}
void CgeoTuneDlg::OnBnClickedRadioT10(){	targetIndex =  9; selectTarget();	}
void CgeoTuneDlg::OnBnClickedRadioT11(){	targetIndex = 10; selectTarget();	}

//	-------------------------------------------------------------------
//	readCameraSettings
//	-------------------------------------------------------------------

void readCameraSettings()
{
	static int index = 0;

	if(!cameraConnected)return;

	switch(index){
		case 0: focusInt = getFocus();				index++; break;
		case 1: getFocusRange();					index++; break;
		case 2: exposureTimeAbs = getExposure();	index++; break;
		case 3: cameraGain = getGain();				index++; break;
		case 4: fstop = getFStop();					index++; break;
		default:
			index=0;
			break;
	}

}

//	-------------------------------------------------------------------
//	OnBnClickedGraphTog
//	-------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedGraphTog()
{
	gwshow = gwshow?0:1;
	graphD.ShowWindow(gwshow?SW_SHOW:SW_HIDE);
}


//	-------------------------------------------------------------------
//	OnBnClickedPause
//	-------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedPause()
{
	m_pause = (m_pause)?(0):(1);
}

//	-------------------------------------------------------------------
//	OnBnClickedCapGraph
//	-------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedCapGraph()
{
	graphD.screenCap();
}

//	-------------------------------------------------------------------
//	OnBnClickedCameraTog
//	-------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedCameraTog()
{
	m_camera = (m_camera)?(0):(1);
}


//	-------------------------------------------------------------------
//	OnBnClickedSetHeading
//	-------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedSetHeading()
{
	setHeading();
}

//	----------------------------------------------------------------------------
//	logEvent
//	----------------------------------------------------------------------------

void CgeoTuneDlg::logEvent(char *s1)
{
//	CString s = s1;
//	OutputDebugString(s);
//	s.AppendChar(0x0d);
//	s.AppendChar(0x0a);
//	logText.Insert(0,s);
//	m_log_edit.SetWindowTextA(logText);
}

//	-------------------------------------------------------------------
//	timestring - YYMMDD-HHMMSS
//	-------------------------------------------------------------------

char *timestring(){

	static char s[256];
	struct tm newtime;
	time_t aclock;

	time( &aclock );					 // Get time in seconds
	localtime_s( &newtime, &aclock );	 // Convert time to struct

	sprintf_s( s, sizeof(s), "%02i%02i%02i-%02i%02i%02i",
			newtime.tm_year-100,			// Year (current year minus 1900)
			newtime.tm_mon+1,				// Month (0 to 11; January = 0) 
			newtime.tm_mday,				// Day of month (1 to 31) 
			newtime.tm_hour,				// Hours since midnight (0 to 23) 
			newtime.tm_min,					// Minutes after hour (0 to 59) 
			newtime.tm_sec					// Seconds after minute (0 to 59)
			);
	return s;
}

//	-------------------------------------------------------------------
//	fname - filename
//	-------------------------------------------------------------------

char *fname(char *type, char *comment, char *ext){

	static char filename[256];
	static char comment2[256];
	if(strlen(comment)){

		for(unsigned int i=0;i<strlen(comment);i++){
			comment2[i]=(
				(comment[i]=='\\')||
				(comment[i]=='/')||
				(comment[i]==':')||
				(comment[i]=='*')||
				(comment[i]=='?')||
				(comment[i]=='<')||
				(comment[i]=='>')||
				(comment[i]=='|'))?('_'):(comment[i]);
			comment2[i+1]=0;
			if(i>=250)break;
		}
		if(strlen(ext)){
			sprintf_s( filename, sizeof(filename), "%s-%s-%s.%s",
				type,							// Log
				timestring(),					// YYMMDD-HHMMSS
				comment2,						// Comment
				ext								// File Extension
				);
		} else {
			sprintf_s( filename, sizeof(filename), "%s-%s-%s",
				type,							// Log
				timestring(),					// YYMMDD-HHMMSS
				comment2						// Comment
				);
		}
	} else {

		if(strlen(ext)){
			sprintf_s( filename, sizeof(filename), "%s-%s.%s",
				type,							// Log
				timestring(),					// YYMMDD-HHMMSS
				ext								// File Extension
				);
		} else {
			sprintf_s( filename, sizeof(filename), "%s-%s",
				type,							// Log
				timestring()					// YYMMDD-HHMMSS
				);
		}
	}
	return filename;
}

//	-------------------------------------------------------------------
//	OnBnClickedSaveLog
//	-------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedSaveLog()
{
	if(0==logText.GetLength())return;

	FILE *sfile = NULL;
	char comment[256];
	graphD.GetDlgItemText(IDC_EDIT_COMMENT2,comment,sizeof(comment));
	fopen_s(&sfile, fname("Log",comment,"txt"), "w" );
	if(sfile != NULL){
		fprintf(sfile,"%s",logText.GetString());
		fclose(sfile);
	}
	logText.SetString("");
	logText2.SetString("");
	newlog = 1;
	gimLogCount = 0;
}

//	-------------------------------------------------------------------
//	OnBnClickedHsLog
//	-------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedHsLog()
{
	static int hslog = 0;
	hslog = hslog?0:1;
	setHSLog(hslog);
}

//	-------------------------------------------------------------------
//	goodPort
//	-------------------------------------------------------------------

int goodPort(int index)
{
	TCHAR lpTargetPath[1000];
	CString ComName;
	ComName.Format(_T("COM%d"), index+1);
	int res = QueryDosDevice(ComName, (LPSTR)lpTargetPath, sizeof(lpTargetPath));
	return res;
}

//	-------------------------------------------------------------------
//	OnBnClickedPort
//	-------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedPort()
{
	if(connected){
		setConnect(0);
		Sleep(100);
		closePort();
		connected = 0;
	}

	const int numports = 30;

	GetDlgItem(IDC_PORT)->EnableWindow(FALSE);
	for(int i=0;i<numports;i++){
		portIndex++;
		if(portIndex>=numports)portIndex=0;
		if(goodPort(portIndex)){
			int res = openPort(portIndex,UTIL_BAUD);
			SetDlgItemText(IDC_PORT,getPortName());
			if(0==res){
				connected = 1;
				setConnect(1);
				break;
			}
			Sleep(200);
		}
	}
	SetDlgItemText(IDC_CONNECT,connected?"Disconnect":"Connect");
	GetDlgItem(IDC_PORT)->EnableWindow(TRUE);
}


//	-------------------------------------------------------------------
//	OnBnClickedConnect
//	-------------------------------------------------------------------

void CgeoTuneDlg::OnBnClickedConnect()
{
	if(connected){
		setConnect(0);
		Sleep(100);
		closePort();
		connected = 0;
	}else{
		if(0 == openPort(portIndex, UTIL_BAUD)){
			connected = 1;
			setConnect(1);
		}
	}
	SetDlgItemText(IDC_PORT,getPortName());
	SetDlgItemText(IDC_CONNECT,connected?"Disconnect":"Connect");
}


void yprEulerToRotX(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;
}

void testDcm(){
	
	static double ypr[3] = {-90,-90,0};
	static double c[3][3];
	double inc = .0001;

	yprEulerToRotX(&ypr,&c);

	static char s[40];
	sprintf_s(s,sizeof(s),"$VNWRG,26,%1.0f,%1.0f,%1.0f,%1.0f,%1.0f,%1.0f,%1.0f,%1.0f,%1.0f*XXXX",
		c[0][0]+inc,c[0][1]+inc,c[0][2]+inc,
		c[1][0]+inc,c[1][1]+inc,c[1][2]+inc,
		c[2][0]+inc,c[2][1]+inc,c[2][2]+inc
	);
	OutputDebugString(s);
//	$VNWRG,26,0,0,1,-1,0,0,0,-1,0*XXXX	// 0,-90,90	// dual sensor
	ypr[0]=0;

}

/*
$VNWRG,26,1,0,0,0,1,0,0,0,1*XXXX
$VNWRG,26,1,0,0,0,1,0,0,0,1*XXXX
*/