
// ex-1Dlg.cpp : implementation file
//

#include "stdafx.h"
#include "ex-1.h"
#include "ex-1Dlg.h"
#include "afxdialogex.h"



// CAboutDlg dialog used for App About


// ===================Slamtec example code start===================
#include <rpos/robot_platforms/slamware_core_platform.h>

// basic namespace for all operation
using namespace rpos::robot_platforms;
// namespace for exception and other core functions
using namespace rpos::core;

// namespace for map operation
using namespace rpos::features;
using namespace rpos::features::location_provider;
using namespace rpos::system::detail;

SlamwareCorePlatform robot;
rpos::actions::MoveAction moveAction;

// ===================Slamtec example code end  ===================

#ifdef _DEBUG
#define new DEBUG_NEW
#endif

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

// Dialog Data
	enum { IDD = IDD_ABOUTBOX };

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

// Implementation
protected:
	DECLARE_MESSAGE_MAP()
};

CAboutDlg::CAboutDlg() : CDialogEx(CAboutDlg::IDD)
{
}

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

BEGIN_MESSAGE_MAP(CAboutDlg, CDialogEx)
END_MESSAGE_MAP()


// Cex1Dlg dialog




Cex1Dlg::Cex1Dlg(CWnd* pParent /*=NULL*/)
	: CDialogEx(Cex1Dlg::IDD, pParent)
{
	m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
	m_info_yaw = 0.0;
	m_info_y = 0.0;
	m_info_x = 0.0;
	m_info_sdk_version = _T("");
	m_info_fw_version = _T("");
	m_info_battery = 0;
	m_info_action_status = _T("");
	m_info_fw_version = _T("");
	m_info_sdk_version = _T("");
	m_dest_x = _T("");
	m_dest_y = _T("");
}

void Cex1Dlg::DoDataExchange(CDataExchange* pDX)
{
	CDialogEx::DoDataExchange(pDX);
	DDX_Text(pDX, IDC_INFO_YAW, m_info_yaw);
	DDX_Text(pDX, IDC_INFO_Y, m_info_y);
	DDX_Text(pDX, IDC_INFO_X, m_info_x);
	DDX_Text(pDX, IDC_INFO_SDK_VERSION, m_info_sdk_version);
	DDX_Text(pDX, IDC_INFO_FW_VERSION, m_info_fw_version);
	DDX_Text(pDX, IDC_INFO_BATTERY, m_info_battery);
	DDX_Text(pDX, IDC_INFO_ACTIONSTATUS, m_info_action_status);
	DDX_Text(pDX, IDC_INFO_FW_VERSION, m_info_fw_version);
	DDX_Text(pDX, IDC_INFO_SDK_VERSION, m_info_sdk_version);
	DDX_Text(pDX, IDC_DEST_X, m_dest_x);
	DDX_Text(pDX, IDC_DEST_Y, m_dest_y);
	DDX_Control(pDX, IDC_PIC_MAP, m_pic_map);
}

BEGIN_MESSAGE_MAP(Cex1Dlg, CDialogEx)
	ON_WM_SYSCOMMAND()
	ON_WM_PAINT()
	ON_WM_QUERYDRAGICON()
	ON_BN_CLICKED(IDC_BTN_FORWARD, &Cex1Dlg::OnBnClickedBtnForward)
	ON_BN_CLICKED(IDC_BTN_LEFT, &Cex1Dlg::OnBnClickedBtnLeft)
	ON_BN_CLICKED(IDC_BTN_BACK, &Cex1Dlg::OnBnClickedBtnBack)
	ON_BN_CLICKED(IDC_BTN_RIGHT, &Cex1Dlg::OnBnClickedBtnRight)
	ON_BN_CLICKED(IDCANCEL, &Cex1Dlg::OnBnClickedCancel)
	ON_WM_TIMER()
	ON_BN_CLICKED(IDC_BTN_MOVE, &Cex1Dlg::OnBnClickedBtnMove)
	ON_BN_CLICKED(IDC_BTN_STOP, &Cex1Dlg::OnBnClickedBtnStop)
END_MESSAGE_MAP()


// Cex1Dlg message handlers

BOOL Cex1Dlg::OnInitDialog()
{
	CDialogEx::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)
	{
		BOOL bNameValid;
		CString strAboutMenu;
		bNameValid = strAboutMenu.LoadString(IDS_ABOUTBOX);
		ASSERT(bNameValid);
		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

	// initialize the action status as IDLE
	m_info_action_status = "IDLE";

	// initialize all the status information display
	m_info_x = 0.0;
	m_info_y = 0.0;
	m_info_yaw = 0.0;
	m_info_battery = robot.getBatteryPercentage();	
	m_info_fw_version = robot.getSDPVersion().c_str();
	m_info_sdk_version = robot.getSDKVersion().c_str();

	// update default into dispaly
	UpdateData(FALSE);

	// initialize map display
	drawMap();

	// set timer#1 with interval 100 milisecond for status update
	SetTimer(1, 100, NULL);
	// set timer@4 with interval 33 milisecond for map update
	SetTimer(4, 33, NULL);

	this->SetDefID(IDC_BTN_MOVE);
	return TRUE;  // return TRUE  unless you set the focus to a control
}

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

// 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 Cex1Dlg::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();
	}
}

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

void Cex1Dlg::OnBnClickedBtnForward()
{
	// TODO: Add your control notification handler code here
	try{
		rpos::core::Direction direction(rpos::core::ACTION_DIRECTION::FORWARD);
		moveAction = robot.moveBy(direction);
	}catch(const ConnectionFailException &e) 
	{
		MessageBox(_T("连接异常，程序关闭"), _T("错误"), MB_OK);
		CDialogEx::OnCancel();
    } catch(const RequestTimeOutException &e) 
	{
		MessageBox(_T("请求超时，请重试"), _T("错误"), MB_OK);
    }catch(const std::exception &e)
	{
		MessageBox(_T("发生异常，程序关闭"), _T("错误"), MB_OK);
	}
}

void Cex1Dlg::OnBnClickedBtnBack()
{
	// TODO: Add your control notification handler code here
	try{
		rpos::core::Direction direction(rpos::core::ACTION_DIRECTION::BACKWARD);
		moveAction = robot.moveBy(direction);
	}catch(const ConnectionFailException &e) 
	{
		MessageBox(_T("连接异常，程序关闭"), _T("错误"), MB_OK);
		CDialogEx::OnCancel();
    } catch(const RequestTimeOutException &e) 
	{
		MessageBox(_T("请求超时，请重试"), _T("错误"), MB_OK);
    }catch(const std::exception &e)
	{
		MessageBox(_T("发生异常，程序关闭"), _T("错误"), MB_OK);
	}
}

void Cex1Dlg::OnBnClickedBtnLeft()
{
	// TODO: Add your control notification handler code here
	try{
		rpos::core::Direction direction(rpos::core::ACTION_DIRECTION::TURNLEFT);
		moveAction = robot.moveBy(direction);
	}catch(const ConnectionFailException &e) 
	{
		MessageBox(_T("连接异常，程序关闭"), _T("错误"), MB_OK);
		CDialogEx::OnCancel();
    } catch(const RequestTimeOutException &e) 
	{
		MessageBox(_T("请求超时，请重试"), _T("错误"), MB_OK);
    }catch(const std::exception &e)
	{
		MessageBox(_T("发生异常，程序关闭"), _T("错误"), MB_OK);
	}
}

void Cex1Dlg::OnBnClickedBtnRight()
{
	// TODO: Add your control notification handler code here
	try{
		rpos::core::Direction direction(rpos::core::ACTION_DIRECTION::TURNRIGHT);
		moveAction = robot.moveBy(direction);
	}catch(const ConnectionFailException &e) 
	{
		MessageBox(_T("连接异常，程序关闭"), _T("错误"), MB_OK);
		CDialogEx::OnCancel();
    } catch(const RequestTimeOutException &e) 
	{
		MessageBox(_T("请求超时，请重试"), _T("错误"), MB_OK);
    }catch(const std::exception &e)
	{
		MessageBox(_T("发生异常，程序关闭"), _T("错误"), MB_OK);
        CDialogEx::OnCancel();
	}
}

void Cex1Dlg::OnBnClickedCancel()
{
	// TODO: Add your control notification handler code here
	robot.disconnect();
	CDialogEx::OnCancel();
}

void Cex1Dlg::OnTimer(UINT_PTR nIDEvent)
{
	if(nIDEvent == 1)
	{
		// TODO: Add your message handler code here and/or call default
		UpdateData(TRUE);
		try{
			// update current pose (position + heading angle)
			rpos::core::Pose pose = robot.getPose();
			m_info_x = pose.x();
			m_info_y = pose.y();
			m_info_yaw = pose.yaw();

			// update battery status
			m_info_battery = robot.getBatteryPercentage();

			// update action status
			if(moveAction.isEmpty())
				m_info_action_status = "Idle";
			else
			{
				switch(moveAction.getStatus())
				{
				case ActionStatusWaitingForStart:
					m_info_action_status = "Waiting For Start";
					break;

				case ActionStatusRunning:
					m_info_action_status = "Running";
					break;

				case ActionStatusFinished:
					m_info_action_status = "Finished";
					break;

				case ActionStatusPaused:
					m_info_action_status = "Paused";
					break;

				case ActionStatusStopped:
					m_info_action_status = "Stopped";
					break;

				case ActionStatusError:
					m_info_action_status = "Error";
					break;

				default:
					m_info_action_status = "Error";
					break;
				}
			}
		}catch(const ConnectionFailException &e) 
		{
			MessageBox(_T("连接异常，程序关闭"), _T("错误"), MB_OK);
			CDialogEx::OnCancel();
		}catch(const std::exception &e)
		{
			MessageBox(_T("发生异常，程序关闭"), _T("错误"), MB_OK);
			CDialogEx::OnCancel();
		}
		// update the display
		UpdateData(FALSE);
	}
	else
	{
		// update map display
		drawMap();
	}
	CDialogEx::OnTimer(nIDEvent);
}


void Cex1Dlg::OnBnClickedBtnMove()
{
	// TODO: Add your control notification handler code here
	double _x = _tstof(m_dest_x);
	double _y = _tstof(m_dest_y);


	try{
		rpos::core::Location loc(_x, _y);
		moveAction = robot.moveTo(loc, false, true);
	}catch(const ConnectionFailException &e) 
	{
		MessageBox(_T("连接异常，程序关闭"), _T("错误"), MB_OK);
		CDialogEx::OnCancel();
    }catch(const std::exception &e)
	{
		MessageBox(_T("发生异常，程序关闭"), _T("错误"), MB_OK);
        CDialogEx::OnCancel();
	}
}


void Cex1Dlg::OnBnClickedBtnStop()
{
	// TODO: Add your control notification handler code here
	try {
		if(!moveAction.isEmpty())
			moveAction.cancel();
	}catch(const ConnectionFailException &e) 
	{
		MessageBox(_T("连接异常，程序关闭"), _T("错误"), MB_OK);
		CDialogEx::OnCancel();
    }catch(const std::exception &e)
	{
		MessageBox(_T("发生异常，程序关闭"), _T("错误"), MB_OK);
        CDialogEx::OnCancel();
	}
}


// draw map based on the data from Slamware
void Cex1Dlg::drawMap(void)
{
	BITMAPINFO _mapDesc ;
	DWORD* pData ;
	HBITMAP hBitmap ;

	try{
		// get map from slamware
		rpos::core::RectangleF knownArea = robot.getLocationProvider().getKnownArea(MapTypeBitmap8Bit, location_provider::EXPLORERMAP);
		location_provider::Map map = robot.getMap(MapTypeBitmap8Bit, knownArea, rpos::features::location_provider::EXPLORERMAP);
		int mapWidth = map.getMapDimension().x();
		int mapHeight = map.getMapDimension().y();

		// initialize the bitmap header structure
		memset(&_mapDesc, 0, sizeof(_mapDesc));							// clear BITMAPHEADER
		_mapDesc.bmiHeader.biSize = sizeof(_mapDesc);					// size of BITMAPHEADER
		_mapDesc.bmiHeader.biWidth = mapWidth;							// width by pixels
		_mapDesc.bmiHeader.biHeight = mapHeight;						// height by pixels
		_mapDesc.bmiHeader.biPlanes = 1;								// 1 plane
		_mapDesc.bmiHeader.biBitCount = 32;								// 24bit Color
		_mapDesc.bmiHeader.biCompression = BI_RGB;						// no compression
		_mapDesc.bmiHeader.biSizeImage = mapHeight * mapWidth;			// size by pixels

		// create DIB image to fill the map data
		hBitmap = CreateDIBSection (::GetDC(NULL), (BITMAPINFO *) &_mapDesc, DIB_RGB_COLORS, (void**)&pData, NULL, 0);
		// fill the map data
		for (int posY = 0; posY < mapHeight; ++posY)
		{
			for (int posX = 0; posX < mapWidth; ++posX)
			{            
				// get map pixel
				rpos::system::types::_u8 mapValue_8bit = map.getMapData()[posX + posY * mapWidth] + 127;
				// fill the bitmap data
				pData[posX + posY*mapWidth] = RGB(mapValue_8bit,mapValue_8bit,mapValue_8bit);
			}
		}
		// update bitmap map file to picture control component
		m_pic_map.SetBitmap(hBitmap);

	}catch(const ConnectionFailException &e) 
	{
		MessageBox(_T("连接异常，程序关闭"), _T("错误"), MB_OK);
		CDialogEx::OnCancel();
    }catch(const std::exception &e)
	{
		MessageBox(_T("发生异常，程序关闭"), _T("错误"), MB_OK);
        CDialogEx::OnCancel();
	}
}
