// pc-dasDlg.cpp : implementation file
//
#include "stdafx.h"
#include <windows.h>
#include <stdio.h>
#include <sys/timeb.h>
#include <string.h>
#include "pc-das.h"
#include "pc-dasDlg.h"
#include "UTM.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#endif

// CpcdasDlg dialog

#define RED_BUTTON RGB(255,65,65)
#define GRN_BUTTON RGB(20,255,20)
#define YLW_BUTTON RGB(244,244,0)
#define BLK_TEXT   RGB(0,0,0)

extern double radToDeg(double rad);
extern double degToRad(double deg);

CpcdasDlg::CpcdasDlg(CWnd* pParent /*=NULL*/)
	: CDialog(CpcdasDlg::IDD, pParent)
{
	m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
}

void CpcdasDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
}

BEGIN_MESSAGE_MAP(CpcdasDlg, CDialog)
	ON_WM_TIMER()
	ON_WM_PAINT()
	ON_WM_QUERYDRAGICON()

	//}}AFX_MSG_MAP
	ON_BN_CLICKED(IDC_parosciPortSet, OnBnClickedparosciPortSet)
	ON_BN_CLICKED(IDC_kftPortSet, OnBnClickedkftPortSet)
	ON_BN_CLICKED(IDC_wfPortSet, OnBnClickedwfPortSet)
	ON_BN_CLICKED(IDC_resonAddrSet, OnBnClickedresonAddrSet)
	ON_BN_CLICKED(IDC_rockAddrSet, OnBnClickedRockAddrSet)
	ON_BN_CLICKED(IDC_3DaddrSet, OnBnClicked3DAddrSet)
	ON_BN_CLICKED(IDOK, OnBnClickedOk)

	ON_BN_CLICKED(IDC_gianAddrSet, OnBnClickedGianAddrSet)
	ON_BN_CLICKED(IDC_alignRestart, &CpcdasDlg::OnBnClickedalignrestart)
	ON_BN_CLICKED(IDC_CHECK11, &CpcdasDlg::OnBnClickedShutdown)
END_MESSAGE_MAP()


// CpcdasDlg message handlers

BOOL CpcdasDlg::OnInitDialog()
{
	CDialog::OnInitDialog();
    CComboBox *portList;

	// Set the icon for this dialog.  The framework does this automatically
	//  when the application's main window is not a dialog
	SetIcon(m_hIcon, TRUE);			// Set big icon
	SetIcon(m_hIcon, FALSE);		// Set small icon

	// TODO: Add extra initialization here
	WSADATA wsaData;
    int iResult = WSAStartup(MAKEWORD(2, 2), &wsaData);

	_timerId = SetTimer(1, 0xA, NULL); //25 msec timer period for sampling serial ports
	_gpsList = (CListCtrl *)GetDlgItem(IDC_gpsList);
	_gpsList->InsertColumn(0, "GPS Data", LVCFMT_LEFT, 400);
	_parosciList = (CListCtrl *)GetDlgItem(IDC_parosciList);
	_parosciList->InsertColumn(0, "Parosci Data", LVCFMT_LEFT, 400);

	_winfrogList = (CListCtrl *)GetDlgItem(IDC_wfList);
	_winfrogList->InsertColumn(0, "Winfrog Data", LVCFMT_LEFT, 400);

	portList = (CComboBox *)GetDlgItem(IDC_kftPort);
	portList->AddString("1");
	portList->AddString("2");
	portList->AddString("3");
	portList->AddString("4");
	portList->AddString("5");
	portList->AddString("6");
	portList->AddString("7");
	portList->AddString("8");
	portList->SetCurSel(3);

	portList = (CComboBox *)GetDlgItem(IDC_parosciPort);
	portList->AddString("1");
	portList->AddString("2");
	portList->AddString("3");
	portList->AddString("4");
	portList->SetCurSel(2);

	portList = (CComboBox *)GetDlgItem(IDC_winfrogPort);
	portList->AddString("2101");
	portList->AddString("2102");
	portList->AddString("2103");
	/*portList->AddString("4");
	portList->AddString("5");
	portList->AddString("6");
	portList->AddString("7");
	portList->AddString("8");
	*/portList->SetCurSel(2101);

	_kearfottBtn.SubclassDlgItem(IDC_kftPortSet, this);
	_kearfottBtn.SetColor(BLK_TEXT, RED_BUTTON);
	_kearfottConnected = FALSE;
	_kearfott = NULL;

	_parosciBtn.SubclassDlgItem(IDC_parosciPortSet, this);
	_parosciBtn.SetColor(BLK_TEXT, RED_BUTTON);
	_parosciConnected = FALSE;

	_winfrogBtn.SubclassDlgItem(IDC_wfPortSet, this);
	_winfrogBtn.SetColor(BLK_TEXT, RED_BUTTON);
	_winfrogConnected = FALSE;
	_winfrogSocket = NULL;
	_wfNMEASocket = NULL;

	_resonAddrBtn.SubclassDlgItem(IDC_resonAddrSet, this);
	_resonAddrBtn.SetColor(BLK_TEXT, RED_BUTTON);
	_resonConnected = FALSE;
	SetDlgItemText(IDC_resonIPAddrs, (LPCTSTR)("134.89.32.108"));
    _reson7K = NULL;

	_rockAddrBtn.SubclassDlgItem(IDC_rockAddrSet, this);
	_rockAddrBtn.SetColor(BLK_TEXT, RED_BUTTON);
	_rockConnected = FALSE;
	SetDlgItemText(IDC_rockIPAddrs, (LPCTSTR)("134.89.21.7"));
    _rockSocket = NULL;

	_3DAddrBtn.SubclassDlgItem(IDC_3DaddrSet, this);
	_3DAddrBtn.SetColor(BLK_TEXT, RED_BUTTON);
	_3DConnected = FALSE;
	SetDlgItemText(IDC_3DipAddress, (LPCTSTR)("192.168.168.130"));
	_3DSocket = NULL; _3DSocketHead2 = NULL;

    _gianAddrBtn.SubclassDlgItem(IDC_gianAddrSet, this);
	_gianAddrBtn.SetColor(BLK_TEXT, RED_BUTTON);
	_gianConnected = FALSE;
	SetDlgItemText(IDC_GianipAddress, (LPCSTR)("134.89.20.75"));
	_gianSocket = NULL;
	_gianDvlSocket = NULL;
	_sonardyneSocket = NULL;
	_sledSocket = _sledDvlSocket =  NULL;

	_ctdSocket = NULL;
	_ctdConnected = FALSE;

    _pressureValid = FALSE; _pressureDepth = 0.;
	_usblValid = FALSE;
	_salinityValid = FALSE;
    _zeroRock = TRUE;
	_alignRestart = FALSE;
	_shutdownKft = FALSE;

	_gpsAidEnableCBox.SubclassDlgItem(IDC_gpsAidEnable, this);
	_dopAidEnableCBox.SubclassDlgItem(IDC_dopAidEnable, this);
	_dopPingEnableCBox.SubclassDlgItem(IDC_dopPingEnable, this);
	_latLonCBox.SubclassDlgItem(IDC_lat_lon_valid, this);

	return TRUE;  // return TRUE  unless you set the focus to a control
}

// If you add a minimize button to your dialog, you will need the code below
//  to draw the icon.  For MFC applications using the document/view model,
//  this is automatically done for you by the framework.

void CpcdasDlg::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
	{
		CDialog::OnPaint();
	}
}

// The system calls this function to obtain the cursor to display while the user drags
//  the minimized window.
HCURSOR CpcdasDlg::OnQueryDragIcon()
{
	return static_cast<HCURSOR>(m_hIcon);
}

void CpcdasDlg::OnBnClickedkftPortSet()
{
	// TODO: Add your control notification handler code here
    CString s;
	GetDlgItemText(IDC_kftPort, s);
	
	if (_kearfottPort.Open(atoi(s.GetString()), 115200, ODDPARITY, 8, ONESTOPBIT))
	{
	  _kearfottBtn.SetColor(BLK_TEXT, GRN_BUTTON);
	}
	_kearfott = new Kearfott(&_kearfottPort);
	_kearfottConnected = TRUE;
}

void CpcdasDlg::OnBnClickedparosciPortSet()
{
	// TODO: Add your control notification handler code here
    CString s;
	GetDlgItemText(IDC_parosciPort, s);
	if (_parosciPort.Open(atoi(s.GetString()), 115200, NOPARITY, 8, ONESTOPBIT) ) {
		_parosciConnected = TRUE;
		_parosciBtn.SetColor(BLK_TEXT, GRN_BUTTON);
	}

	//set up parosci log file
	time_t ltime;
	struct tm *timeinfo;
	int length;
	char fileName[256];

	time(&ltime);
	timeinfo = gmtime(&ltime);
	if (timeinfo) {
		length = strftime(fileName, 256, "%Y%m%d_%H%M%S_parosci.txt\0", timeinfo);
		_parosciLog = CreateFile(fileName, GENERIC_WRITE, 0, NULL, OPEN_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
	}
	else {
		_parosciLog = NULL;
	}
/*
	_parosciSocket = new SimpleSocket();
	(void)_parosciSocket->createConnectedStreamClient(4001, "134.89.32.108");
    //ADDED EJM
	//createConnectedStreamClient(4001,"134.89.32.108");

	if (_parosciSocket->ready())
		{
		_parosciConnected = TRUE;
		_parosciBtn.SetColor(BLK_TEXT, GRN_BUTTON);
        //ADDED EJM
		_parosciSocket->setOptionsTimeout(SimpleSocket::SS_RECEIVE,0,1000);
		}
*/
   _ctdSocket = new SimpleSocket();
   //(void)_ctdSocket->createConnectedStreamClient(5040,"134.89.21.7"); //western flyer CTD
  // (void)_ctdSocket->createConnectedStreamClient(5014,"134.89.22.19"); //rachel carson
   (void)_ctdSocket->createConnectedStreamClient(5014, "134.89.20.75"); //euler
   if (_ctdSocket->ready()) {
		_ctdConnected = TRUE;	
   }
}

void CpcdasDlg::OnBnClickedwfPortSet()
{
	// TODO: Add your control notification handler code here
	CString s;
	GetDlgItemText(IDC_winfrogPort, s);


	_winfrogSocket = new SimpleSocket();
	(void)_winfrogSocket->createConnectedStreamClient(atoi(s.GetString()), "134.89.22.200"); //digi one SP in back of ventana nav rack
	if (_winfrogSocket->ready()) {
		_winfrogConnected = TRUE;
		_winfrogBtn.SetColor(BLK_TEXT, GRN_BUTTON);
	}
	/*
	if (_winfrogPort.Open(atoi(s.GetString()), 9600))
	{
	  _winfrogConnected = TRUE;
	  _winfrogBtn.SetColor(BLK_TEXT, GRN_BUTTON);
	}
	*/

	//set up a network connection to winfrog running on rcnav
	_wfNMEASocket = new SimpleSocket();
	_wfNMEASocket->createConnectedStreamClient(2052, "134.89.32.110");
}

void CpcdasDlg::updateKearfottGUI()
{
	char buf[40];
	char *modeStrings[11] = {
		"Idle",
		"Coarse Stat Align",
		"Fine Stat Align Incomplete",
		"Fine Stat Align Complete",
		"Coarse GPS Align",
		"Fine GPS Align Incomplete",
		"Fine GPS Align Complete",
		"Dop Align Incomplete",
		"Dop Align Complete",
		"Aided Navigation",
		"Go/No Go"
	};

	if (_kearfott->_scaledNavData.mode >=0x00 && _kearfott->_scaledNavData.mode <= 0x0a) {
		SetDlgItemText(IDC_kft_mode, modeStrings[_kearfott->_scaledNavData.mode]);
	} else {
		SetDlgItemText(IDC_kft_mode, "Unknown");
	}

	sprintf(buf, "0x%2X\0", _kearfott->_scaledNavData.monitor);
	SetDlgItemText(IDC_kft_monitor, buf);

	if (_kearfott->_scaledNavData.monitor & DEPTH_LOOP_OPEN) {
		SetDlgItemText(IDC_kft_depthLoopOpen, "True");
	} else {
		SetDlgItemText(IDC_kft_depthLoopOpen, "False");
	}

	if (_kearfott->_scaledNavData.monitor & GPS_DATA_PROCESSED) {
		SetDlgItemText(IDC_kft_gpsProcessFlag, "True");
	} else {
		SetDlgItemText(IDC_kft_gpsProcessFlag, "False");
	}

	if (_kearfott->_scaledNavData.monitor & GPS_DATA_REJECTED) {
		SetDlgItemText(IDC_kft_gpsRejectFlag, "True");
	} else {
		SetDlgItemText(IDC_kft_gpsRejectFlag, "False");
	}

	if (_kearfott->_scaledNavData.monitor & DOP_DATA_PROCESSED) {
		SetDlgItemText(IDC_kft_dvlProcessFlag, "True");
	} else {
		SetDlgItemText(IDC_kft_dvlProcessFlag, "False");
	}

	if (_kearfott->_scaledNavData.monitor & DOP_DATA_REJECTED) {
		SetDlgItemText(IDC_kft_dvlRejectFlag, "True");
	} else {
		SetDlgItemText(IDC_kft_dvlRejectFlag, "False");
	}

	if (_kearfott->_scaledNavData.monitor & ZUPT_PROCESSED) {
		SetDlgItemText(IDC_kft_zuptProcessFlag, "True");
	} else {
		SetDlgItemText(IDC_kft_zuptProcessFlag, "False");
	}

	if (_kearfott->_scaledNavData.monitor & 0x02) {
		SetDlgItemText(IDC_kft_alignNoPosFlag, "True");
	} else {
		SetDlgItemText(IDC_kft_alignNoPosFlag, "False");
	}

	if (_kearfott->_scaledNavData.monitor & VALID_DVL_HEIGHT) {
		SetDlgItemText(IDC_kft_validDVLHeightFlag, "True");
	} else {
		SetDlgItemText(IDC_kft_validDVLHeightFlag, "False");
	}

	SetDlgItemInt(IDC_kft_Counter, _kearfott->_scaledNavData.cycles);

	sprintf(buf,"%20f\0", _kearfott->_scaledNavData.utcTime);
	SetDlgItemText(IDC_kft_UTCSecs, buf);

	sprintf(buf, "%20f\0", radToDeg(_kearfott->_scaledNavData.prate));
	SetDlgItemText(IDC_kft_xrate, buf);

	sprintf(buf, "%20f\0", radToDeg(_kearfott->_scaledNavData.qrate));
	SetDlgItemText(IDC_kft_yrate, buf);

	sprintf(buf, "%20f\0", radToDeg(_kearfott->_scaledNavData.rrate));
	SetDlgItemText(IDC_kft_zrate, buf);

	sprintf(buf, "%20f\0", radToDeg(_kearfott->_scaledNavData.lat));
	SetDlgItemText(IDC_kft_lat, buf);

	sprintf(buf, "%20f\0", radToDeg(_kearfott->_scaledNavData.lon));
	SetDlgItemText(IDC_kft_lon, buf);

	sprintf(buf, "%20f\0", _kearfott->_scaledNavData.depth);
	SetDlgItemText(IDC_kft_depth, buf);

	sprintf(buf, "%20f\0", _kearfott->_scaledNavData.bheight);
	SetDlgItemText(IDC_kft_altitude, buf);

	sprintf(buf, "%20f\0", _kearfott->_scaledNavData.vbodyx*100.0);
	SetDlgItemText(IDC_kft_nxvel, buf);

	sprintf(buf, "%20f\0", _kearfott->_scaledNavData.vbodyy*100.0);
	SetDlgItemText(IDC_kft_nyvel, buf);

	sprintf(buf, "%20f\0", _kearfott->_scaledNavData.vbodyz*100.0);
	SetDlgItemText(IDC_kft_nzvel, buf);

	sprintf(buf, "%20f\0", radToDeg(_kearfott->_scaledNavData.roll));
	SetDlgItemText(IDC_kft_roll, buf);

	sprintf(buf, "%20f\0", radToDeg(_kearfott->_scaledNavData.pitch));
	SetDlgItemText(IDC_kft_pitch, buf);

	sprintf(buf, "%20f\0", radToDeg(_kearfott->_scaledNavData.heading));
	SetDlgItemText(IDC_kft_heading, buf);

	sprintf(buf, "%20f\0", _kearfott->_scaledNavData.accelx);
	SetDlgItemText(IDC_kft_xacc, buf);

	sprintf(buf, "%20f\0", _kearfott->_scaledNavData.accely);
	SetDlgItemText(IDC_kft_yacc, buf);

	sprintf(buf, "%20f\0", _kearfott->_scaledNavData.accelz);
	SetDlgItemText(IDC_kft_zacc, buf);

	
	if (_gpsList && _kearfott->_ggaValid){
		_gpsList->InsertItem(0, _kearfott->_ggaStr);
		_kearfott->_ggaValid = FALSE;
	}

	//set dvl values
	SetDlgItemInt(IDC_dvl_Counter, _kearfott->_dopplerPD4.ktnav);

	sprintf(buf, "%20f\0", (float)_kearfott->_dopplerPD4.xvelbtm);
	SetDlgItemText(IDC_dvl_xvel, buf);

	sprintf(buf, "%20f\0", (float)_kearfott->_dopplerPD4.yvelbtm);
	SetDlgItemText(IDC_dvl_yvel, buf);

	sprintf(buf, "%20f\0", (float)_kearfott->_dopplerPD4.zvelbtm);
	SetDlgItemText(IDC_dvl_zvel, buf);

	sprintf(buf, "%20f\0", (float)_kearfott->_dopplerPD4.evelbtm);
	SetDlgItemText(IDC_dvl_evel, buf);

	sprintf(buf, "%20f\0", (float)_kearfott->_dopplerPD4.bm1);
	SetDlgItemText(IDC_dvl_bm1, buf);

	sprintf(buf, "%20f\0", (float)_kearfott->_dopplerPD4.bm2);
	SetDlgItemText(IDC_dvl_bm2, buf);

	sprintf(buf, "%20f\0", (float)_kearfott->_dopplerPD4.bm3);
	SetDlgItemText(IDC_dvl_bm3, buf);

	sprintf(buf, "%20f\0", (float)_kearfott->_dopplerPD4.bm4);
	SetDlgItemText(IDC_dvl_bm4, buf);

	sprintf(buf, "%20f\0", (float)_kearfott->_dopplerPD4.toping_hr);
	SetDlgItemText(IDC_dvl_top_hr, buf);

	sprintf(buf, "%20f\0", (float)_kearfott->_dopplerPD4.toping_min);
	SetDlgItemText(IDC_dvl_top_min, buf);

	sprintf(buf, "%20f\0", _kearfott->_dopplerPD4.toping_sec+(float)(_kearfott->_dopplerPD4.toping_hun/100.0));
	SetDlgItemText(IDC_dvl_top_sec, buf);
}

void CpcdasDlg::updateReson7k()
{
	char recordData[512];  
	char *ptrToData = recordData;
	char *ptrs = ptrToData;

	if (_reson7K && _kearfott) {
		//send position data
		U32 __ival = 0; //WGS84
		memcpy((void*)ptrs, &__ival, sizeof(U32)); ptrs += sizeof(U32);  // datum identifier
		F32 __fval = 0.0;
		memcpy((void*)ptrs, &__fval, sizeof(F32)); ptrs += sizeof(F32); // latency
		memcpy((void*)ptrs, &(_kearfott->_scaledNavData.lat), sizeof(F64)); ptrs += sizeof(F64); //lat in radians
		memcpy((void*)ptrs, &(_kearfott->_scaledNavData.lon), sizeof(F64)); ptrs += sizeof(F64); //lon in radians
		F64 __dval = 0.0;
		memcpy((void*)ptrs, &__dval, sizeof(F64)); ptrs += sizeof(F64); //height; set to 0.0 since we use the depth record for this
		U8 __cval = 0;
		memcpy((void*)ptrs, &__cval, sizeof(U8)); ptrs += sizeof(U8); //position type flag, 0 = lat/lon
		memcpy((void*)ptrs, &__cval, sizeof(U8)); ptrs += sizeof(U8); //UTM zone, meaningless
		memcpy((void*)ptrs, &__cval, sizeof(U8)); ptrs += sizeof(U8); //quality flag, 0 = navigation data
		memcpy((void*)ptrs, &__cval, sizeof(U8)); ptrs += sizeof(U8); //positioning flag, set to 0 for now
		memcpy((void*)ptrs, &__cval, sizeof(U8)); ptrs += sizeof(U8); //positioning flag, set to 0 for now
		double sampleSeconds;
		time_t sec;
		time(&sec);
		if ((_kearfott->_scaledNavData.utcTime - sec % 86400)>43200) {
			sec -= 86400;
		}
		else if ((sec % 86400 - _kearfott->_scaledNavData.utcTime)>43200) {
			sec += 86400;
		}
		sampleSeconds = (double)(sec - sec % 86400) + _kearfott->_scaledNavData.utcTime;
		_reson7K->MB_RemoteCommand(Reson6046Command::Position, ptrToData, &sampleSeconds);
		ptrs = ptrToData; //reset pointer

		//send altitude if valid
		if (_kearfott->_scaledNavData.monitor & VALID_DVL_HEIGHT) {
			__fval = _kearfott->_scaledNavData.bheight;
			_reson7K->MB_RemoteCommand(Reson6046Command::Altitude, (char *)&__fval, &sampleSeconds);
		}

		//roll, pitch & heave
		__fval = _kearfott->_scaledNavData.roll;
		memcpy((void*)ptrs, &__fval, sizeof(F32)); ptrs += sizeof(F32); // roll
		__fval = _kearfott->_scaledNavData.pitch;
		memcpy((void*)ptrs, &__fval, sizeof(F32)); ptrs += sizeof(F32); // pitch
		__fval = 0.0; //no heave from kearfott (for now!)
		memcpy((void*)ptrs, &__fval, sizeof(F32)); ptrs += sizeof(F32); // heave
		_reson7K->MB_RemoteCommand(Reson6046Command::RollPitchHeave, ptrToData, &sampleSeconds);

		//headingupdateGian
		__fval = _kearfott->_scaledNavData.heading;
		_reson7K->MB_RemoteCommand(Reson6046Command::Heading, (char *)&__fval, &sampleSeconds);
	}
}

void CpcdasDlg::updateRock()
{
static double initNorthing = 0.0, initEasting = 0.0;

	if (_rockSocket) {
		double lat = _kearfott->_scaledNavData.lat*180.0/M_PI;
		double lon = _kearfott->_scaledNavData.lon*180.0/M_PI;
		double northing, easting;
		char zone[5];
		char buf[1024];

		UTM::LLtoUTM(lat, lon, northing, easting, zone);
		if (_zeroRock) {
			initNorthing = northing; initEasting = easting;
			_zeroRock = FALSE;
		}
		northing -= initNorthing; easting -= initEasting;
		sprintf(buf, "[%g, %g, %g, %g, %g, %g, %g, %g, %g, %g, %g, %g, %g]",
			northing, easting, _pressureDepth,
			_kearfott->_scaledNavData.vbodyx,
			_kearfott->_scaledNavData.vbodyy,
			_kearfott->_scaledNavData.vbodyz,
			_kearfott->_scaledNavData.roll,
			_kearfott->_scaledNavData.pitch,
			_kearfott->_scaledNavData.heading,
			_kearfott->_scaledNavData.prate,
			_kearfott->_scaledNavData.qrate,
			_kearfott->_scaledNavData.rrate,
			_kearfott->_scaledNavData.bheight);

		_rockSocket->broadcast(buf, (unsigned int)strlen(buf));
		//_winfrogList->InsertItem(0, buf);
		//printf(buf);
		//printf("\n\r");

	}

}

void CpcdasDlg::updateGian()
{
	if (_gianSocket && _kearfott) _gianSocket->broadcast(&(_kearfott->_scaledNavData), sizeof(_kearfott->_scaledNavData));
	if (_sledSocket && _kearfott) _sledSocket->broadcast(&(_kearfott->_scaledNavData), sizeof(_kearfott->_scaledNavData));
}

void CpcdasDlg::updateGianDvl()
{
	if (_gianDvlSocket && _kearfott) _gianDvlSocket->broadcast(&(_kearfott->_dopplerPD4), sizeof(_kearfott->_dopplerPD4));
	if (_sledDvlSocket && _kearfott) _sledDvlSocket->broadcast(&(_kearfott->_dopplerPD4), sizeof(_kearfott->_dopplerPD4));
}


//2 byte project code 0x3d03
//1 byte version 1
//3 doubles - temp (C), salinity (PSU), pressure (dbar)
void CpcdasDlg::update3D()
{
char buf[27];
char *ptr;

	if (_3DSocket) {
		buf[0] = 0x03; buf[1] = 0x3d; buf[2] = 0x01;
        ptr = buf+3;
		double value = _ctdTemp;
		memcpy(ptr, &value, sizeof(double)); ptr += sizeof(double);
		value = _ctdSalinity;
		memcpy(ptr, &value, sizeof(double)); ptr += sizeof(double);
		value = _pressureDepth;
		memcpy(ptr, &value, sizeof(double)); ptr += sizeof(double);
		_3DSocket->sending(buf, ptr-buf);
	}

	if (_3DSocketHead2) {
		buf[0] = 0x03; buf[1] = 0x3d; buf[2] = 0x01;
		ptr = buf + 3;
		double value = _ctdTemp;
		memcpy(ptr, &value, sizeof(double)); ptr += sizeof(double);
		value = _ctdSalinity;
		memcpy(ptr, &value, sizeof(double)); ptr += sizeof(double);
		value = _pressureDepth;
		memcpy(ptr, &value, sizeof(double)); ptr += sizeof(double);
		_3DSocketHead2->sending(buf, ptr - buf);
	}
}

void CpcdasDlg::updateWinfrog()
{
	char gllString[100];
	double auv_lat, auv_lon, lat_min, lon_min, lat_deg, lon_deg;
	time_t secs;

	auv_lat = _kearfott->_scaledNavData.lat*180.0 / M_PI;
	auv_lon = _kearfott->_scaledNavData.lon*180.0 / M_PI;
	lat_min = 60.0*modf(auv_lat, &lat_deg); lon_min = 60 * modf(auv_lon, &lon_deg);

	time(&secs);
	secs %= 86400;

	sprintf(gllString, "$GPGLL,%02d%08.5f,%c,%03d%08.5f,%c,%02d%02d%02d,A\r\n\0",
		(int)(abs(lat_deg)), abs(lat_min), (lat_deg>0) ? 'N' : 'S',
		(int)(abs(lon_deg)), abs(lon_min), (lon_deg > 0) ? 'E' : 'W',
		(int)(secs / 3600), (int)((secs % 3600) / 60), secs % 60);
	if (_wfNMEASocket && _wfNMEASocket->ready()) _wfNMEASocket->sending(gllString, strlen(gllString));
}

BOOL CpcdasDlg::getParosciData() {
	char buf[41];
    int nbytes = 0;
    double pressure;

	if (_parosciPort.ReadDataWaiting() ) {
		do {
			nbytes = _parosciPort.ReadData(buf, 1);
		} while (buf[0] != '*');

		do {
			nbytes += _parosciPort.ReadData(buf+nbytes, 40-nbytes);
		} while (nbytes != 40 && buf[nbytes-1] != 0x0a);
		//nbytes = _parosciPort.ReadData(buf+1, 13);
        if (nbytes == 40) return FALSE;
		if (strncmp("*0001", buf, 5) || buf[nbytes-1] != 0x0a ) {
			_pressureValid = FALSE;
			return FALSE;
		}
		if (sscanf(buf+6, "%lf", &pressure) != 1) {
			_pressureValid = FALSE;
			return FALSE;
		}

		_absolutePressureDbar = (double)10.0*pressure;
		//pressure = pressure/.6838530; //pressure in PSI
		//pressure = pressure * 0.068948; //pressure now in bar
		
		//convert to meters SW
		double dbar, C, gr;
		static double st2, init_pressure;
		static int first=1;
		if (first) // calc & store sin^2(latitude)
			{
			st2 = sin ( degToRad(37.0));
			st2 *= st2;
			init_pressure = .97;
			first = 0;
			}
		//convert to dbar
		dbar = _pressureDbar = 10.0*(pressure - init_pressure);
		C = (((-1.8200e-15*dbar + 2.279e-10)*dbar - 2.5212e-05)*dbar + 9.72659)
			* dbar;
		gr = 9.780318*((2.3600e-05*st2 + 5.2788e-3)*st2 + 1) + 1.092e-06*dbar;

		_pressureDepth = C/gr;
		_pressureValid = TRUE;
		return TRUE;
	}

	return FALSE;
}

double CpcdasDlg::calcSalinity(double conductivity, // S/m * 10 (see PSS 1978 notes)
			     double temp,         // degrees C
			     double pressure)     // decibars
{
  double cond_35_15_0 = 42.914;
  double A1 = 0.0000207;
  double A2 = -6.370e-10;
  double A3 = 3.989e-15;
  double B1 = 0.03426;
  double B2 = 0.0004464;
  double B3 = 0.4215;
  double B4 = -0.003107;
  double c0 = 0.6766097;
  double c1 = 0.0200564;
  double c2 = 0.0001104259;
  double c3 = -6.9698e-7;
  double c4 = 1.0031e-9;

  double a0 = 0.0080;
  double a1 = -0.1692;
  double a2 = 25.3851;
  double a3 = 14.0941;
  double a4 = -7.0261;
  double a5 = 2.7081;
  double b0 = 0.0005;
  double b1 = -0.0056;
  double b2 = -0.0066;
  double b3 = -0.0375;
  double b4 = 0.0636;
  double b5 = -0.0144;
  double k = 0.0162;

 
  double R = conductivity / cond_35_15_0;
  double Rp = 1 + (pressure*(A1 + A2*pressure + A3*pressure*pressure))
                         / (1 + B1*temp + B2*temp*temp + B3*R + B4*R*temp);
  double rT = c0 + c1*temp + c2*temp*temp + c3*pow(temp,3) + c4*pow(temp,4);
  double Rt = R / (Rp*rT);
  double Rtx = sqrt(Rt);

//  double salA = a0*pow(Rt,0.5) + a1*pow(Rt,0.5) + a2*pow(Rt,0.5) + a3*pow(Rt,0.5)
//                               + a4*pow(Rt,0.5) + a5*pow(Rt,0.5);
//  double salB = b0*pow(Rt,0.5) + b1*pow(Rt,0.5) + b2*pow(Rt,0.5) + b3*pow(Rt,0.5)
//                               + b4*pow(Rt,0.5) + b5*pow(Rt,0.5);

   double salA = a0 +
                 a1*Rtx + 
                 a2*Rtx*Rtx + 
                 a3*Rtx*Rtx*Rtx + 
                 a4*Rtx*Rtx*Rtx*Rtx +
                 a5*Rtx*Rtx*Rtx*Rtx*Rtx;
   double salB = b0 +
		 b1*Rtx +
		 b2*Rtx*Rtx +
	 	 b3*Rtx*Rtx*Rtx +
		 b4*Rtx*Rtx*Rtx*Rtx +
		 b5*Rtx*Rtx*Rtx*Rtx;
  double t_factor = (temp - 15)/(1 + k*(temp - 15));
  double sal = salA + t_factor*salB;

  return sal;
}

double CpcdasDlg::calcSoundVelocity(double S, double T, double P)
{
	P = 0.1*P;  //Conversion to bar
	double c00 = 1402.388;
	double c01 = 5.03711;
	double c02 = -5.80852e-2;
	double c03 = 3.3420e-4;
	double c04 = -1.47800e-6;
	double c05 = 3.1464e-9;

	double c10 = 0.153563;
	double c11 = 6.8982e-4;
	double c12 = -8.1788e-6;
	double c13 = 1.3621e-7;
	double c14 = -6.1185e-10;

	double c20 = 3.1260e-5;
	double c21 = -1.7107e-6;
	double c22 = 2.5974e-8;
	double c23 = -2.5335e-10;
	double c24 = 1.0405e-12;

	double c30 = -9.7729e-9;
	double c31 = 3.8504e-10;
	double c32 = -2.3643e-12;

	double P2 = P*P;
	double P3 = P2*P;
	double Cw = c00 + (c01 + (c02 + (c03 + (c04 + c05*T)*T)*T)*T)*T
		+ (c10 + (c11 + (c12 + (c13 + c14*T)*T)*T)*T)*P
		+ (c20 + (c21 + (c22 + (c23 + c24*T)*T)*T)*T)*P2
		+ (c30 + (c31 + c32*T)*T)*P3;

	double a00 = 1.389;
	double a01 = -1.262e-2;
	double a02 = 7.164e-5;
	double a03 = 2.006e-6;
	double a04 = -3.21e-8;

	double a10 = 9.4742e-5;
	double a11 = -1.2580e-5;
	double a12 = -6.4885e-8;
	double a13 = 1.0507e-8;
	double a14 = -2.0122e-10;

	double a20 = -3.9064e-7;
	double a21 = 9.1041e-9;
	double a22 = -1.6002e-10;
	double a23 = 7.988e-12;
		
	double a30 = 1.100e-10;
	double a31 = 6.649e-12;
	double a32 = -3.389e-13;
		
	double A = a00 + (a01 + (a02 + (a03 + a04*T)*T)*T)*T
		+ (a10 + (a11 + (a12 + (a13 + a14*T)*T)*T)*T)*P
		+ (a20 + (a21 + (a22 + a23*T)*T)*T)*P2
		+ (a30 + (a31 + a32*T)*T)*P3;

	double b00 = -1.922e-2;
	double b01 = -4.42e-5;
	double b10 = 7.3637e-5;
	double b11 = 1.7945e-7;

	double B = b00 + b01*T + (b10 + b11*T)*P;

	double d00 = 1.727e-3;
	double d10 = -7.9836e-6;

	double D = d00 + d10*P;

	return Cw + A*S + B*S*sqrt(S) + D*pow(S, 2);
}

BOOL CpcdasDlg::getCTDData() {
	char buf[200], *ptr=NULL;
    int nbytes = 0;
    int ntokens;
	float ctdPressPSI, ctd_depth;

	memset((void *)buf, 0, 200); nbytes = 0;
	do {
		if (_ctdSocket->receiveDatagram(buf + nbytes, 1) < 0) break;
		nbytes++;
	} while (buf[nbytes - 1] != 0x0a);
	//nbytes = _ctdSocket->receiveDatagram(buf, 199);
	if (nbytes <= 0) return FALSE;

	if (1) { //parsing code for CTD packets coming from Subastion
		ntokens = sscanf(buf, "%f,%f,%f,%lf,%lf\r\n", &_ctdTemp, &_ctdCond, &_ctdPress, &_ctdSalinity, &_soundVelocity);
		if (ntokens != 5) { return FALSE; }
		_ctdSalinity = calcSalinity(10.0*_ctdCond, _ctdTemp, _ctdPress);
		_soundVelocity = calcSoundVelocity(_ctdSalinity, _ctdTemp, _ctdPress);
	}
	else { //parsing code for CTD packet coming from Rickets & Ventana
		ntokens = sscanf(buf, "%f, %f, %f, %f, %f, %f, %f", &_ctdTemp, &_ctdCond, &_ctdPress, &_ctdV1, &_ctdV2, &_ctdV3, &_ctdV4);
		if (ntokens != 7) {
			return FALSE;
		}
		else {
			_ctdSalinity = calcSalinity(10.0*_ctdCond, _ctdTemp, _ctdPress);
			_soundVelocity = calcSoundVelocity(_ctdSalinity, _ctdTemp, _ctdPress);
			return TRUE;
		}
	}
}

BOOL CpcdasDlg::getWinfrogData()
{
	int nbytes = 0;
	int bytesRead;
	nbytes = _winfrogSocket->receiveDatagram(_usblData, 1);
	if (nbytes < 0) return FALSE;
	while (_usblData[0] != '$') {
		nbytes = _winfrogSocket->receiveDatagram(_usblData, 1);
		if (nbytes < 0) _usblData[0] = 0x00;
	}
	do {
		bytesRead = _winfrogSocket->receiveDatagram(_usblData + nbytes, 256 - nbytes);
		if (bytesRead > 0) nbytes += bytesRead;
	} while (nbytes != 256 && _usblData[nbytes - 1] != 0x0a);

	if (nbytes == 256) return FALSE;
	_usblData[nbytes] = 0x00;

	/*
	if (_winfrogSocket.ReadDataWaiting()) {
		int nbytes = 0;
		do {
			nbytes = _winfrogPort.ReadData(_usblData, 1);
		} while (_usblData[0] != '$');
		do {
			nbytes += _winfrogPort.ReadData(_usblData+nbytes, 256-nbytes);
		} while (nbytes != 256 && _usblData[nbytes-1] != 0x0a);

        if (nbytes == 256) return FALSE;
        _usblData[nbytes] = 0x00;
*/

	if (nbytes > 0) {
	char msgType[10];
		char n = ' ', w = ' ', *wf = 0;
		int hh = 0, mm = 0, latdd = 0, longdd = 0;
		float ss = 0, latmm = 0, longmm = 0, slant = 0, dum = 0.;
		int oldData;
		int _usblhh, _usblmm;
		float _usblss, _usblSlant;

		wf = strdup(_usblData);
		int ntokens = sscanf(wf, "%6s,%2d%2d%f,%2d%f,%c,%3d%f,%c,%f,%f,%f,%f,%f,%f",
							 msgType,&_usblhh,&_usblmm,&_usblss,
							 &latdd,&latmm,&n,&longdd,&longmm,&w,&dum,&dum,&dum,&dum,&dum,&_usblSlant);
		free((void*)wf);
		if (ntokens == 16) {
			_usblLat = latdd + (latmm/60.f);
			if (n == 'S') _usblLat = 0.f - _usblLat;
			_usblLon = longdd + (longmm/60.f);
			if (w == 'W') _usblLon = 0.f - _usblLon;
		    if ((_latLonCBox.GetState() & 0x003)) {
				_usblValid = TRUE;
			} else {
				_usblValid = FALSE;
				return FALSE;
			}
			return _usblValid;
		}
	return FALSE;
	}
return FALSE;
}

void CpcdasDlg::OnTimer(UINT nIDEvent)
{
	static int _cnt = 0, _wfUpdateCnt = 0;

	// Check for messages from the parosci and send to reson as necessary
	if (_parosciConnected) {
		if (getParosciData()) {
			if (_reson7K) {
				char resonBuffer[20];
				char *ptr = resonBuffer;
				U8 depthDesc = 0, corrFlag = 0;
				U16 reserved = 0;
				F32 __fval = (F32)_pressureDepth;
				memcpy((void*)ptr, &depthDesc, sizeof(U8)); ptr += sizeof(U8);
				memcpy((void*)ptr, &corrFlag, sizeof(U8)); ptr += sizeof(U8);
				memcpy((void*)ptr, &reserved, sizeof(U16)); ptr += sizeof(U16);
				memcpy((void*)ptr, &__fval, sizeof(F32)); // depth
				_reson7K->MB_RemoteCommand(Reson6046Command::Depth, resonBuffer, (double *)NULL);
			}
			if (_3DSocket) { update3D();}
			if (_parosciLog) {
				struct _timeb tmb;
				char buf[80];
				DWORD bytesOut;
				double sampleTime, absolutePressure;

				_ftime(&tmb);
				sampleTime = (double)tmb.time + (double)tmb.millitm / 1000.;
				sprintf(buf, "%.3lf %4.8lf\xd\xa\0", sampleTime, _absolutePressureDbar); //add back the static surface pressure offset
				WriteFile(_parosciLog, buf, strlen(buf), &bytesOut, NULL);
			}
		}
	}

	if (_ctdConnected) {
		if (getCTDData()) {
			char buf[80];
			_salinityValid = TRUE; _salinity = _ctdSalinity;
			if (_reson7K) _reson7K->MB_RemoteCommand(Reson6046Command::SetMBSoundVelocity, _soundVelocity);
			sprintf(buf, "  %4.3f\xd\xa\0", _soundVelocity);
			if (_sonardyneSocket) _sonardyneSocket->broadcast(buf, (unsigned int)strlen(buf));
		} else {
			_salinityValid = FALSE;
		}
	}

	// Check for messages from winfrog
	if (_winfrogConnected) {
		if (getWinfrogData()) {
			_winfrogList->InsertItem(0, _usblData);
		}
	}

	// Check for messages from the kearfott, do any external aiding, send to reson as needed
	if (_kearfottConnected) {
		int kearfottStat;
		do {
			kearfottStat = _kearfott->run();
			if (kearfottStat == 2) {
				updateReson7k();
				updateRock();
				updateGian();
			}
			if (kearfottStat == 3) {
				updateGianDvl();
			}
			if (_kearfott->_ggaValid) {
				_gpsList->InsertItem(0, _kearfott->_ggaStr);
				_kearfott->_ggaValid = FALSE;
			}

			if (kearfottStat == 2 && _wfUpdateCnt++ == 25) {
				_wfUpdateCnt = 0;
				updateWinfrog();
			}
		} while (kearfottStat != 0);
	}

	if (_cnt++ == 45) {
		int value = _gpsAidEnableCBox.GetState();
		if ((_gpsAidEnableCBox.GetState() & 0x003)) {
			_gpsAidEnable = TRUE;
		} else {
			_gpsAidEnable = FALSE;
		}
		if ((_dopAidEnableCBox.GetState() & 0x003)) {
			_dopAidEnable = TRUE;
		} else {
			_dopAidEnable = FALSE;
		}
		if ((_dopPingEnableCBox.GetState() & 0x003)) {
			_dopPingEnable = TRUE;
		} else {
			_dopPingEnable = FALSE;
		}
		_dopPingEnable = _dopAidEnable = TRUE;

		if (_kearfottConnected) {
			updateKearfottGUI();
			//external updates to Kearfott
			_kearfott->updateExternalSensors(
			_usblValid, _usblLat, _usblLon,
			_pressureValid, _pressureDepth,
			_salinityValid, _salinity,
			_gpsAidEnable, _dopAidEnable, _dopPingEnable, _alignRestart, _shutdownKft);
		}

		if (_parosciConnected && _pressureValid) {
			//update pressure in list box
			char buf1[20];
			sprintf(buf1,"%lf", _pressureDepth);
			_parosciList->InsertItem(0, buf1);
		}

        _usblValid = FALSE;
		_pressureValid = FALSE;
		_salinityValid = FALSE;
		
		_cnt = 0;
	}
}

void CpcdasDlg::OnBnClickedresonAddrSet()
{
	// TODO: Add your control notification handler code here
	CString s;
	GetDlgItemText(IDC_resonIPAddrs, s);
    _reson7K = new Reson7K((char *)(LPCTSTR)s);
	if (_reson7K->_mbDataSocket->ready())
		_resonAddrBtn.SetColor(BLK_TEXT, GRN_BUTTON);
}

void CpcdasDlg::OnBnClickedRockAddrSet()
{
	// TODO: Add your control notification handler code here
	CString s;
	GetDlgItemText(IDC_rockIPAddrs, s);

	if (!_rockSocket) _rockSocket = new SimpleSocket();
	(void)_rockSocket->createBroadcastDatagramSend(7000, (char *)(LPCTSTR)s);
	if (_rockSocket->ready()) {
		_rockAddrBtn.SetColor(BLK_TEXT, GRN_BUTTON);
		_zeroRock = TRUE;
	}
}

void CpcdasDlg::OnBnClicked3DAddrSet()
{
	CString s;
	GetDlgItemText(IDC_3DipAddress, s);
	if (_3DSocket) delete _3DSocket;
	_3DSocket = new SimpleSocket();
	(void)_3DSocket->createConnectedStreamClient(2009,(char *)(LPCTSTR)s);
	if (_3DSocket->ready()) _3DAddrBtn.SetColor(BLK_TEXT, GRN_BUTTON);
	_3DSocketHead2 = new SimpleSocket();
	(void)_3DSocketHead2->createConnectedStreamClient(2009, "192.168.168.131");
}

void CpcdasDlg::OnBnClickedGianAddrSet()
{
	CString s;

	//get IP address
	GetDlgItemText(IDC_GianipAddress, s);

	//create socket for navigation data
	if (_gianSocket) delete _gianSocket;
	_gianSocket = new SimpleSocket();
	(void)_gianSocket->createBroadcastDatagramSend(1975,(char *)(LPCTSTR)s);
	if (_gianSocket->ready()) _gianAddrBtn.SetColor(BLK_TEXT, GRN_BUTTON);

	//create socket for dvl data
	if (_gianDvlSocket) delete _gianDvlSocket;
	_gianDvlSocket = new SimpleSocket();
	(void)_gianDvlSocket->createBroadcastDatagramSend(1976, (char *)(LPCTSTR)s);

	//create socket for sending sonardyne data
	if (_sonardyneSocket) delete _sonardyneSocket;
	_sonardyneSocket = new SimpleSocket();
	(void)_sonardyneSocket->createBroadcastDatagramSend(6677, "192.168.179.51");

	//create a socket to send data to sled controller
	if (_sledSocket) delete _sledSocket;
	_sledSocket = new SimpleSocket();
	(void)_sledSocket->createBroadcastDatagramSend(1975, "134.89.23.52");

	if (_sledDvlSocket) delete _sledDvlSocket;
	_sledDvlSocket = new SimpleSocket();
	(void)_sledDvlSocket->createBroadcastDatagramSend(1976, "134.89.23.52");
	
}

void CpcdasDlg::OnBnClickedOk()
{
	// TODO: Add your control notification handler code here
	OnOK();
	delete _kearfott;
	ExitProcess(1);
}

void CpcdasDlg::OnIpnFieldchangedIpaddress2(NMHDR *pNMHDR, LRESULT *pResult)
{
	LPNMIPADDRESS pIPAddr = reinterpret_cast<LPNMIPADDRESS>(pNMHDR);
	// TODO: Add your control notification handler code here
	*pResult = 0;
}

void CpcdasDlg::OnBnClickedGianaddrset()
{
	// TODO: Add your control notification handler code here
}


void CpcdasDlg::OnBnClickedalignrestart()
{
	if (_alignRestart) {
		_alignRestart = FALSE;
	}
	else { _alignRestart = TRUE; }
	// TODO: Add your control notification handler code here
}

void CpcdasDlg::OnBnClickedShutdown()
{
	_shutdownKft = TRUE;
}