// AuvMonDlg.cpp : implementation file
//

#include "stdafx.h"
#include "AuvMon.h"
#include "AuvMonDlg.h"
#include "smac.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#endif

#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)
#define USBL_BUFFER_SIZE 256

// CAboutDlg dialog used for App About

class CAboutDlg : public CDialog
{
public:
	CAboutDlg();

// Dialog Data
	enum { IDD = IDD_ABOUTBOX };

	protected:
	virtual void DoDataExchange(CDataExchange* pDX);    // DDX/DDV support

// Implementation
protected:
	DECLARE_MESSAGE_MAP()
};

CAboutDlg::CAboutDlg() : CDialog(CAboutDlg::IDD)
{
}

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

BEGIN_MESSAGE_MAP(CAboutDlg, CDialog)
END_MESSAGE_MAP()


// CAuvMonDlg dialog



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

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

BEGIN_MESSAGE_MAP(CAuvMonDlg, CDialog)
	ON_WM_TIMER()
	ON_WM_SYSCOMMAND()
	ON_WM_PAINT()
	ON_WM_QUERYDRAGICON()
	//}}AFX_MSG_MAP
	ON_LBN_SELCHANGE(IDC_RECLIST, OnLbnSelchangeList1)
	ON_BN_CLICKED(IDC_USBLCON, OnBnClickedUsblcon)
	ON_BN_CLICKED(IDC_AUVCON, OnBnClickedAuvcon)
	ON_BN_CLICKED(IDC_BUTTON1, OnBnClickedButton1)
	ON_BN_CLICKED(IDOK, OnBnClickedOk)
	ON_EN_CHANGE(IDC_XMITPERIOD, OnEnChangeXmitperiod)
	ON_BN_CLICKED(IDC_REQSTAT, OnBnClickedReqstat)
	ON_BN_CLICKED(IDC_ABORTMSSN, OnBnClickedAbortmssn)
	ON_BN_CLICKED(IDC_STARTSRVY, OnBnClickedStartsrvy)
	ON_NOTIFY(LVN_ITEMCHANGED, IDC_STATLIST, OnLvnItemchangedStatlist)
END_MESSAGE_MAP()


// CAuvMonDlg message handlers

BOOL CAuvMonDlg::OnInitDialog()
{
	CDialog::OnInitDialog();

	WSADATA wsaData;
	int iResult = WSAStartup(MAKEWORD(2, 2), &wsaData);

	// Add "About..." menu item to system menu.

	// IDM_ABOUTBOX must be in the system command range.
	ASSERT((IDM_ABOUTBOX & 0xFFF0) == IDM_ABOUTBOX);
	ASSERT(IDM_ABOUTBOX < 0xF000);

	CMenu* pSysMenu = GetSystemMenu(FALSE);
	if (pSysMenu != NULL)
	{
		CString strAboutMenu;
		strAboutMenu.LoadString(IDS_ABOUTBOX);
		if (!strAboutMenu.IsEmpty())
		{
			pSysMenu->AppendMenu(MF_SEPARATOR);
			pSysMenu->AppendMenu(MF_STRING, IDM_ABOUTBOX, strAboutMenu);
		}
	}

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

	_timerId = SetTimer(1, 500, NULL);
	SetDlgItemText(IDC_XMITPERIOD, "5");
	OnEnChangeXmitperiod();

	statList = (CListCtrl*)GetDlgItem(IDC_STATLIST);
	statList->InsertColumn(0, "Status Message", LVCFMT_LEFT, 400);

	recList = (CListCtrl*)GetDlgItem(IDC_RECLIST);
	recList->InsertColumn(0, "Transmitted Nav Records", LVCFMT_LEFT, 400);

	WriteStat("AuvMon Started...ready");

	_lastXmitTime = 0;

	_usblBtn.SubclassDlgItem(IDC_USBLCON, this);
	_usblBtn.SetColor(BLK_TEXT, RED_BUTTON);
	_usblConnected = FALSE;


	_auvBtn.SubclassDlgItem(IDC_AUVCON, this);
	_auvBtn.SetColor(BLK_TEXT, RED_BUTTON);
	_auvConnected = FALSE;

	_xmitBtn.SubclassDlgItem(IDC_BUTTON1, this);
	_xmitBtn.SetColor(BLK_TEXT, RED_BUTTON);
	_xmit = FALSE;

	SetDlgItemText(IDC_AUVCOM, "5");
	SetDlgItemText(IDC_GPSCOM, "7");
	SetDlgItemText(IDC_USBLCOM, "1");

	//SetDlgItemText(IDC_XMITREC, "N/A");
	//SetDlgItemText(IDC_XMITACK, "N/A");
	SetDlgItemText(IDC_LAT,   "---");
	SetDlgItemText(IDC_LONG,  "---");
	SetDlgItemText(IDC_HEAD,  "---");
	SetDlgItemText(IDC_DEPTH, "---");
	SetDlgItemText(IDC_ALT,   "---");
	SetDlgItemText(IDC_VOLT,  "---");
	SetDlgItemText(IDC_BEHAVE, "N/A");

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

void CAuvMonDlg::OnSysCommand(UINT nID, LPARAM lParam)
{
	if ((nID & 0xFFF0) == IDM_ABOUTBOX)
	{
		CAboutDlg dlgAbout;
		dlgAbout.DoModal();
	}
	else
	{
		CDialog::OnSysCommand(nID, lParam);
	}
}

void CAuvMonDlg::Update()
{
	char buf[32];

	sprintf(buf, "%.6f", _auvLat);
	SetDlgItemText(IDC_LAT,   buf);
	sprintf(buf, "%.6f", _auvLong);
	SetDlgItemText(IDC_LONG,  buf);
	sprintf(buf, "%.6f", _auvHead);
	SetDlgItemText(IDC_HEAD,  buf);
	sprintf(buf, "%.2f", _auvDepth);
	SetDlgItemText(IDC_DEPTH, buf);
	sprintf(buf, "%.2f", _auvAlt);
	SetDlgItemText(IDC_ALT,   buf);
	sprintf(buf, "%.2f", _auvVolt);
	SetDlgItemText(IDC_VOLT,  buf);
	sprintf(buf, "%.2f", _auvHumid);
	SetDlgItemText(IDC_HUMIDITY,  buf);
	SetDlgItemText(IDC_BEHAVE, _auvBehavior);
	SetDlgItemText(IDC_ABORT, _auvAbort);

	OnPaint();
}

void CAuvMonDlg::OnTimer(UINT nIDEvent)
{
	// Check for a message from the AUV
	if (_auvConnected && GetAuvData())
	{
		Update();
	}

	// Check for data from WinFrog
	if (_usblConnected)
	{
		GetUsblData();
	}

}

int CAuvMonDlg::GetUsblData()
{
	static int oldlatdd = 0, oldlongdd = 0;
	static float oldlatmm = 0.0, oldlongmm = 0.0;
	static int nbytes = 0;
	// If there's data waiting from WinFrog, then read and parse for transmit

	if (_usblPort.ReadDataWaiting() > 0)
//	if (_usblPort.ReadDataWaiting() > 20)
	{
	  char usbldata[USBL_BUFFER_SIZE];
	  unsigned long usbl_size;
	  char *ptr;
	  int bytesRead;

	  memset((void *)usbldata, 0, USBL_BUFFER_SIZE);
	  ptr = usbldata;

	  do {
		  bytesRead = _usblPort.ReadDataBlocking((void *)ptr, 1);
		  if (!bytesRead) return 1;
		  if ((ptr - usbldata + 1) > (USBL_BUFFER_SIZE - 1)) return 1;
	  } while (*(ptr++) != 0x0a);

	  //usbl_size = _usblPort.ReadData(((char *)usbldata)+nbytes, USBL_BUFFER_SIZE-1);

	  char msgType[10];
	  char n = ' ', w = ' ';
	  int hh = 0, mm = 0, latdd = 0, longdd = 0;
	  float ss = 0, latmm = 0, longmm = 0, slant = 0, dum = 0.;
	  int oldData;

	  // For testing, create dummy string according to WinFrog spec
	  //
	  // sprintf(usbldata, "$PUSBA,111213.145,3345.7500,N,12123.7500,E,1.1,2.2,3.3,,4.4,5.55,7.8,a.a,ddmm.mmm,a,dddmm.mmmm,a*hh\xd\xa");

	  int ntokens = sscanf(usbldata, "%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);
//      int ntokens = sscanf(wf, "%6s,%2d%2d%f,%2d%f,%c,%3d%f,%c",
//		msgType, &_usblhh, &_usblmm, &_usblss, &latdd,&latmm,&n,&longdd, &longmm, &w);

	  if (ntokens != 16) {
		WriteStat("received too few tokens in USBL string");
	  }

	  if (ntokens == 16) {
	    _usblLat = latdd + (latmm/60.f);
		if (n == 'S') _usblLat = 0.f - _usblLat;
		_usblLong = longdd + (longmm/60.f);
		if (w == 'W') _usblLong = 0.f - _usblLong;

		if ((latdd==oldlatdd) && (latmm==oldlatmm) && (longdd==oldlongdd) && (longmm==oldlongmm)) {
			oldData = 1;
		} else {
			oldData = 0;
		}
		  

		// Placing the call to SendNavPacket here ensures that only fresh
		// USBL data is sent to the AUV.
		// Are we due to xmit a Nav packet?
		time_t now;
		time(&now);

		if (_xmit && ((now - _lastXmitTime) >= _period))
		{
			if (!oldData) {
				SendNavPacket(now);
				_lastXmitTime = now;
				oldlatdd = latdd; oldlatmm = latmm; oldlongdd = longdd; oldlongmm = longmm;
			} else {
				//WriteStat("USBL data is stale");
			}
		}
	  }
	  return 0;
	}


	// This should rarely, if ever, occur
	else if (_usblPort.ReadDataWaiting() > 0)
	{
		char usbldata[USBL_BUFFER_SIZE];
		unsigned long usbl_size;

		usbl_size = _usblPort.ReadData((void*)usbldata, USBL_BUFFER_SIZE);

		WriteStat("Data on usbl port of insufficient length");
	}

	return 1;
}

// Assemble and transmit Nav packet to AUV via acoustic modem link
void CAuvMonDlg::SendNavPacket(long ts)
{
	// Assemble packet for AUV
	SMAC_RECORD_HEADER sh;
	SMAC_SAN_RECORD    san;
	char packet[100];
	unsigned long packet_size;

	san.wfLat = _usblLat;
	san.wfLong = _usblLong;
	san.wfSlant = _usblSlant;
	san.wfTime = _usblhh*3600.f + _usblmm*60.f + _usblss;

	smac::encodePacket(&sh, &san, packet, &packet_size);

	// Send packet to AUV
	_auvPort.SendData(packet, packet_size);
	_auvPort.SendData("Done", 5);

	// Write this record info to record list
	char msg[256];
	sprintf(msg, "%02d:%02d:%.3f   LT:%.5f   LG:%.5f   SL:%.2f", _usblhh, _usblmm, 
		_usblss, _usblLat, _usblLong, _usblSlant);
	WriteRecList(msg);
}

// Read data from AUV via acoustic modem link
int CAuvMonDlg::GetAuvData()
{
	char packet[100];
	unsigned long packet_size;
	// Read status packet from modem
	if (_auvPort.ReadDataWaiting() >= sizeof(SMAC_RECORD_HEADER) + sizeof(SMAC_STAT_RECORD)
		&& ((packet_size) = _auvPort.ReadData((void*)packet, 100)) > 0)
	{
	  SMAC_RECORD_HEADER hdr;
	  SMAC_STAT_RECORD stat;
	  int decodeOK = 0;

	  //smac::decodeHeader(packet, packet_size, &hdr);
	  hdr.record_type = smac::RecordType::STAT;
	  switch(hdr.record_type) {
		  case smac::RecordType::STAT:
			  decodeOK = smac::decodeStatRecord(packet, packet_size, &stat);
			  if (0 == decodeOK)
			  {
#if 0
				  // some dummy values for testing
				  stat.auvAbort = 0;
				  stat.auvAlt = 50.2f;
				  stat.auvBehavior = 1; //Abort
				  stat.auvDepth = 200.f;
				  stat.auvHeading = 120.f;
				  stat.auvHumid = 20.f;
				  stat.auvLat = 39.1234f;
				  stat.auvLong = -120.f;
				  stat.auvVolt = 54.321f;
#endif
				  UpdateAuvData(stat);
				  return 1;
			  }
			  else if (1 == decodeOK)
			      WriteStat("Checksum failed in Auv Status data packet");

			  else if (2 == decodeOK)
			      WriteStat("Checksum failed in Auv Status header");

			  break;

		  default:
		      WriteStat("Unknown message received from AUV");

			  break;
	  }

	}
	return 0; // No data
}

void CAuvMonDlg::UpdateAuvData(SMAC_STAT_RECORD stat)
{
	_auvLat = stat.auvLat;
	_auvLong = stat.auvLong;
	_auvHead = stat.auvHeading;
	_auvDepth = stat.auvDepth;
	_auvAlt = stat.auvAlt;
	_auvVolt = stat.auvVolt;
	_auvHumid = stat.auvHumid;

	if (stat.auvBehavior < smac::BhavrType::INVALID_BHAVR)
	  _auvBehavior.Format("%s", smac::_behaviorNames[stat.auvBehavior]);
	else
	  _auvBehavior.Format("%s", "UNKNOWN");

	if (stat.auvAbort)
	  _auvAbort = "Yes";
	else
	  _auvAbort = "No";

	char gllString[100];
	double lat_deg, lat_min, lon_deg, lon_min;
	lat_min = 60.0*modf(_auvLat, &lat_deg); lon_min = 60*modf(_auvLong, &lon_deg);
	time_t secs;

	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);
	_nmeaSocket->sending(gllString, strlen(gllString));
	WriteStat(gllString);
	return;
}

// 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 CAuvMonDlg::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 CAuvMonDlg::OnQueryDragIcon()
{
	return static_cast<HCURSOR>(m_hIcon);
}

void CAuvMonDlg::OnLbnSelchangeList1()
{
	// TODO: Add your control notification handler code here
}

void CAuvMonDlg::OnEnChangeEdit7()
{
	// TODO:  If this is a RICHEDIT control, the control will not
	// send this notification unless you override the CDialog::OnInitDialog()
	// function and call CRichEditCtrl().SetEventMask()
	// with the ENM_CHANGE flag ORed into the mask.

	// TODO:  Add your control notification handler code here
}


void CAuvMonDlg::OnBnClickedUsblcon()
{
	// TODO: Add your control notification handler code here
	// Attempt to establish comms with USBL
	//
	CString s;
	GetDlgItemText(IDC_USBLCOM, s);
//	if (_usblPort.Open(atoi(s.GetString()), 9600))
	if (_usblPort.Open(atoi(s.GetString()), 9600) || 1)
	{
	  // Port opened, set-up USBL to send data
	  WriteStat("WinFrog responding...Connected");
	  _usblConnected = TRUE;
	  _usblBtn.SetColor(BLK_TEXT, GRN_BUTTON);
	}
}

void CAuvMonDlg::OnBnClickedAuvcon()
{
	// TODO: Add your control notification handler code here
	// Attempt to establish comms with AUV via acoustic modem
	//
	CString s;
	GetDlgItemText(IDC_AUVCOM, s);
	if (_auvPort.Open(atoi(s.GetString()), 9600))
	{
	  // Port opened, set-up USBL to send data
	  WriteStat("AUV responding...Connected");
	  _auvConnected = TRUE;
	  _auvBtn.SetColor(BLK_TEXT, GRN_BUTTON);
	}

	_nmeaSocket = new SimpleSocket();
	_nmeaSocket->createConnectedStreamClient(2050, "134.89.22.33");
	if (!_nmeaSocket->ready()) {
		WriteStat("socket to winfrog not connected");
	}
	else {
		WriteStat("socket to winfrog connected");
	}
	//set up a network connection to winfrog running on rcnav

}


// Start transmitting Nav packets
void CAuvMonDlg::OnBnClickedButton1()
{
	// TODO: Add your control notification handler code here
	if (!_xmit)
	{
	  char p[10];
	  GetDlgItemText(IDC_XMITPERIOD, p, 10);
	  _period = (atoi(p));
	  
	  if (_usblConnected && _auvConnected)
	  {
	    WriteStat("Start transmitting Nav packets");
	    _xmit = TRUE;
	    _xmitBtn.SetColor(BLK_TEXT, GRN_BUTTON);
	    SetDlgItemText(IDC_BUTTON1, "Stop Packet Xmit");
	    return;
	  }
	  else
	  {
	    WriteStat("WinFrog-USBL|AUV not connected");
		return;
	  }
	}

	else
	{
	  WriteStat("Stop transmitting Nav packets");
	  _lastXmitTime = 0;
	  _xmit = FALSE;
	  _xmitBtn.SetColor(BLK_TEXT, RED_BUTTON);
	  SetDlgItemText(IDC_BUTTON1, "Start Packet Xmit");
	  return;
	}
}

// Write the string to the status list after pre-pending the date/time
void CAuvMonDlg::WriteStat(const char *msg)
{
	if (statList)
	{
		char line[256];
		time_t now;
		struct tm *tms;

		// Get current date/time
		time(&now);
		tms = localtime(&now);
		// Format string
		sprintf(line, "[%03d %02d:%02d:%02d] %s", tms->tm_yday, tms->tm_hour, tms->tm_min,
		tms->tm_sec, msg);
		// Insert the string at the top of the list
		statList->InsertItem(0, line);
	}
}

// Write the string to the record list
void CAuvMonDlg::WriteRecList(const char *msg)
{
	if (recList)
	{
		recList->InsertItem(0, msg);
	}
}

void CAuvMonDlg::OnBnClickedOk()
{
	// TODO: Add your control notification handler code here
	KillTimer(_timerId);
	OnOK();
}

void CAuvMonDlg::OnEnChangeXmitperiod()
{
	// TODO:  If this is a RICHEDIT control, the control will not
	// send this notification unless you override the CDialog::OnInitDialog()
	// function and call CRichEditCtrl().SetEventMask()
	// with the ENM_CHANGE flag ORed into the mask.

	// TODO:  Add your control notification handler code here
}

void CAuvMonDlg::OnBnClickedReqstat()
{
	// TODO: Add your control notification handler code here
	// Create and send a status request command over the modem to the AUV
	//
	if (!_auvConnected)
	{
	  WriteStat("Not connected to AUV");
	  return;
	}

    WriteStat("Requesting status from AUV");

	SMAC_RECORD_HEADER sh;
	SMAC_CMD_RECORD sc;
	char packet[100];
	unsigned long packet_size;

	sh.parameter = smac::REQ_STAT;
	smac::encodePacket(&sh, &sc, packet, &packet_size);
//	char cmdstr[30];
//	sprintf(cmdstr, "+++\xd\xa\x0"); _auvPort.SendData(cmdstr, strlen(cmdstr)); Sleep(1000);
//	sprintf(cmdstr, "ats14=%d\xd\xa", 2); _auvPort.SendData(cmdstr, strlen(cmdstr)); Sleep(500);
//	sprintf(cmdstr, "ato\xd\xa\x0"); _auvPort.SendData(cmdstr, strlen(cmdstr)); Sleep(500);
	_auvPort.SendData(packet, packet_size);
}

void CAuvMonDlg::OnBnClickedAbortmssn()
{
	// TODO: Add your control notification handler code here
	if (!_auvConnected)
	{
	  WriteStat("Not connected to AUV");
	  return;
	}

	int ans = MessageBox("Abort mission and return to surface?",
		"Abort Mission", MB_YESNO);

	if (ans == IDYES)
	{
	  WriteStat("Sending \"Abort\" message to AUV");
	  if (_xmit) OnBnClickedButton1(); // Turn off nav packet transmit

	  SMAC_RECORD_HEADER hdr;
	  SMAC_CMD_RECORD sc;
  	  char packet[100];
	  unsigned long packet_size;

	  hdr.parameter = smac::ABORT_MISSION;
	  smac::encodePacket(&hdr, &sc, packet, &packet_size);
	  _auvPort.SendData(packet, packet_size);
	}
}

void CAuvMonDlg::OnBnClickedStartsrvy()
{
	// TODO: Add your control notification handler code here
	if (!_auvConnected)
	{
	  WriteStat("Not connected to AUV");
	  return;
	}

	int ans = MessageBox("Start mapping survey now?",
		"Start Survey", MB_YESNO);

	if (ans == IDYES)
	{
	  WriteStat("Sending \"Start Survey\" message to AUV");
	  if (_xmit) OnBnClickedButton1(); // Turn off nav packet transmit

	  SMAC_RECORD_HEADER hdr;
	  SMAC_CMD_RECORD sc;
  	  char packet[100];
	  unsigned long packet_size;

	  hdr.parameter = smac::CmdType::START_SURVEY;
	  smac::encodePacket(&hdr, &sc, packet, &packet_size);
	  _auvPort.SendData(packet, packet_size);
	}
}

void CAuvMonDlg::OnLvnItemchangedStatlist(NMHDR *pNMHDR, LRESULT *pResult)
{
	LPNMLISTVIEW pNMLV = reinterpret_cast<LPNMLISTVIEW>(pNMHDR);
	// TODO: Add your control notification handler code here
	*pResult = 0;
}