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

// 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_GPSCON, OnBnClickedGpscon)
	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)
END_MESSAGE_MAP()


// CAuvMonDlg message handlers

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

	// 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, 1000, 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;

	_gpsBtn.SubclassDlgItem(IDC_GPSCON, this);
	_gpsBtn.SetColor(BLK_TEXT, RED_BUTTON);
	_gpsConnected = TRUE;//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, "1");
	SetDlgItemText(IDC_GPSCOM, "7");
	SetDlgItemText(IDC_USBLCOM, "8");

	//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()
{
	// If there's data waiting from WinFrog, then read and parse for transmit
	//if (_usblPort.ReadDataWaiting() > 100)
	if (1)
	{
	  char usbldata[USBL_BUFFER_SIZE];
	  unsigned long usbl_size;

	  //usbl_size = _usblPort.ReadData((void*)usbldata, USBL_BUFFER_SIZE);
	  // For testing, create dummy string according to WinFrog spec
	  //
	  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.;
	  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");
	  wf = strdup(usbldata);
	  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);

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

	  // 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?
	  long now;
	  time(&now);

	  if (_xmit && (now - _lastXmitTime) >= _period)
	  {
	    SendNavPacket(now);
	    _lastXmitTime = now;
	  }

	  return 0;
	}

	// This should rarely, if ever, occur
	else if (_usblPort.ReadDataWaiting() > 0)
	{
	  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)
{
	// For testing, create dummy string according to WinFrog spec
	//
	char wfBuf[256];
	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.;
	sprintf(wfBuf, "$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");
	wf = strdup(wfBuf);
	sscanf(wf, "%6s,%2d%2d%f,%2d%f,%c,%3d%f,%c,%f,%f,%f,,%f,%f,%f",
		msgType,&hh,&mm,&ss,&latdd,&latmm,&n,&longdd,&longmm,&w,&dum,&dum,&dum,&dum,&dum,&slant);
	free((void*)wf);

	double flat = latdd + (latmm/60.);
	if (n == 'S') flat = 0. - flat;
	double flong = longdd + (longmm/60.);
	if (w == 'E') flong = 0. - flong;

	// 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", hh, mm, ss, flat, flong, slant);
	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)
			  {
				  // some dummy values for testing
#if 0
				  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";

	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::OnBnClickedGpscon()
{
	// TODO: Add your control notification handler code here
	// Attempt to establish comms with GPS receiver
	//
	CString s;
	GetDlgItemText(IDC_GPSCOM, s);
	if (_gpsPort.Open(atoi(s.GetString()), 9600))
	{
	  // Port opened, set-up GPS to send data
	  _gpsConnected = TRUE;
	  _gpsBtn.SetColor(BLK_TEXT, GRN_BUTTON);
	}
}

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


// 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 (_gpsConnected && _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];
		long 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);

	_auvPort.SendData(packet, packet_size);
}

void CAuvMonDlg::OnBnClickedAbortmssn()
{
	// TODO: Add your control notification handler code here
	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::CmdType::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
	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);
	}
}