// BaseClientDlg.cpp : Implementation file
//

#include "stdafx.h"
#include "BaseClient.h"
#include "BaseClientDlg.h"
#include "ListenSocket.h"
#include "RequestSocket.h"
#include "ImageTP.h"
#include <Cpcinfo.h>

#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

#define TEST_PORT 54321
#define PHOTO_FILE "photo.jpg"

/////////////////////////////////////////////////////////////////////////////
// CBaseClientDlg dialog

CBaseClientDlg::CBaseClientDlg(CWnd* pParent /*=NULL*/)
	: CDialog(CBaseClientDlg::IDD, pParent)
{
	//{{AFX_DATA_INIT(CBaseClientDlg)
	m_iRadio1 = -1;
	m_sPlayStatus = _T("");
	m_iHeadAngleH = 0;
	m_iHeadAngleV = 0;
	//}}AFX_DATA_INIT
	m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
	m_iRadio1 = -1;
	m_sPlayStatus = _T("");
	m_pVAibo = NULL;
	m_nAiboID = -1;
    m_nPlayID = -1;
	m_iTargetHeadAngleH = 0;
	m_iTargetHeadAngleV = 0;
	m_bConnected = FALSE;
	m_bControl = FALSE;
	m_myRequest = FALSE;
	m_bUpdateHeadAngle = FALSE;
	m_bIsBusy = FALSE;
// for Socket
	m_pListen = NULL;
	m_uPort = TEST_PORT;
	m_bSocketEnable = FALSE;
// ImageTP
	m_pImageTP = NULL;
// NetServiceStatus
	m_bNetServiceSensorEnable = FALSE;
	m_bNetServiceImageEnable = FALSE;
// Sensor Transfer Status
	m_bSensorTransferStart = FALSE;
	m_bSetSkipRate = FALSE;
}

void CBaseClientDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CBaseClientDlg)
	DDX_Control(pDX, IDC_IPADDRESS1, m_ipAddress);
	DDX_Radio(pDX, IDC_RADIO1, m_iRadio1);
	DDX_Text(pDX, IDC_EDIT1, m_sPlayStatus);
	DDX_Text(pDX, IDC_HEAD_H, m_iHeadAngleH);
	DDX_Text(pDX, IDC_HEAD_V, m_iHeadAngleV);
	//}}AFX_DATA_MAP
}

BEGIN_MESSAGE_MAP(CBaseClientDlg, CDialog)
	//{{AFX_MSG_MAP(CBaseClientDlg)
	ON_WM_PAINT()
	ON_WM_QUERYDRAGICON()
	ON_BN_CLICKED(IDC_BTN_AUTONOMY, OnBtnAutonomy)
	ON_BN_CLICKED(IDC_BTN_CONNECT, OnBtnConnect)
	ON_BN_CLICKED(IDC_BTN_PLAY, OnBtnPlay)
	ON_BN_CLICKED(IDC_BTN_REMOTE, OnBtnRemote)
	ON_WM_DESTROY()
	ON_BN_CLICKED(IDC_BTN_SOCKET, OnBtnSocket)
	ON_BN_CLICKED(IDC_BTN_IMAGETP, OnBtnImagetp)
	ON_BN_CLICKED(IDC_BTN_TAKEPICTURE, OnBtnTakepicture)
	ON_BN_CLICKED(IDC_BTN_HEADUP, OnBtnHeadup)
	ON_BN_CLICKED(IDC_BTN_HEADLEFT, OnBtnHeadleft)
	ON_BN_CLICKED(IDC_BTN_HEADRIGHT, OnBtnHeadright)
	ON_BN_CLICKED(IDC_BTN_HEADDOWN, OnBtnHeaddown)
	ON_BN_CLICKED(IDC_BTN_HEADHOME, OnBtnHeadhome)
	ON_BN_CLICKED(IDC_BTN_FORWARD, OnBtnForward)
	ON_BN_CLICKED(IDC_BTN_LEFTTURN, OnBtnLeftturn)
	ON_BN_CLICKED(IDC_BTN_WALKSTOP, OnBtnWalkstop)
	ON_BN_CLICKED(IDC_BTN_RIGHTTURN, OnBtnRightturn)
	ON_BN_CLICKED(IDC_BTN_BACK, OnBtnBack)
	//}}AFX_MSG_MAP
	// VAIBO Sever Messages
	ON_REGISTERED_MESSAGE(WM_VAIBO_CONNECT, OnVAIBOConnect)
	ON_REGISTERED_MESSAGE(WM_VAIBO_CLIENT_UPDATE, OnVAIBOConnectUpdate)
	ON_REGISTERED_MESSAGE(WM_VAIBO_DISCONNECT, OnVAIBODisconnect)
	ON_REGISTERED_MESSAGE(WM_VAIBO_SEMANTICS, OnVAIBOSemantics)
	ON_REGISTERED_MESSAGE(WM_VAIBO_REMOTE_NOTIFY, OnVAIBORemoteNotify)
	ON_REGISTERED_MESSAGE(WM_IMAGETP_UPDATE, OnITPImageUpdate)
	ON_REGISTERED_MESSAGE(WM_VAIBO_SENSOR_DATA, OnVAIBOSensorData)
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CBaseClientDlg message handlers

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

	SetIcon(m_hIcon, TRUE);			// Set big icon
	SetIcon(m_hIcon, FALSE);		// Set small icon

	// TODO: Add extra initialization here
	if ( m_pVAibo == NULL ) {
		m_pVAibo = new CVAIBO( this->m_hWnd );
	}
	ASSERT(m_pVAibo);

	INT	nField0, nField1, nField2, nField3;
	LPCTSTR	p = theApp.m_sAiboIP.GetBuffer(128);
	::sscanf( p, "%d.%d.%d.%d", &nField0, &nField1, &nField2, &nField3 );
	theApp.m_sAiboIP.ReleaseBuffer();
	m_ipAddress.SetAddress( nField0, nField1, nField2, nField3 );

	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 CBaseClientDlg::OnPaint()
{
	if (IsIconic())
	{
		CPaintDC dc(this); // device context for painting

		SendMessage(WM_ICONERASEBKGND, (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();

		if (m_pImageTP && m_pImageTP->m_bNewPhoto) {
		    BITMAPINFO* bmi = (BITMAPINFO*)(m_pImageTP->m_pImageBufN.GetData());
			int bitmapWidth = bmi->bmiHeader.biWidth;
			int bitmapHeight = bmi->bmiHeader.biHeight;
			unsigned char *image = m_pImageTP->m_pImageBufN.GetData() + sizeof(BITMAPINFO);
			CPaintDC dc(&staticPicture());

			CRect rect;
			staticPicture().GetClientRect(&rect);
			::SetStretchBltMode(dc.m_hDC, HALFTONE);
			int nReturn = ::StretchDIBits(dc.m_hDC,
				rect.left,rect.top,rect.Width(),rect.Height(),
				0,0,
				bitmapWidth, bitmapHeight,
				image, bmi, DIB_RGB_COLORS,SRCCOPY);
		}
	}
}

HCURSOR CBaseClientDlg::OnQueryDragIcon()
{
	return (HCURSOR) m_hIcon;
}

//	ON_REGISTERED_MESSAGE(WM_VAIBO_CLIENT_UPDATE, OnVAIBOConnectUpdate)
//	Another application has already connected to AIBO
LRESULT CBaseClientDlg::OnVAIBOConnectUpdate( WPARAM wParam, LPARAM lParam )
{
	LRESULT result;

    if ((unsigned) m_nAiboID != HIWORD(wParam)) {
		// This is not my AIBO that I requested to connect to
		return S_FALSE;
	}
	if (!m_myRequest) {
		// This is not my request, another application changes the connection status.
		return S_FALSE;
	}
	m_myRequest = FALSE;
	if (LOWORD(wParam) == 1) {
		result = OnVAIBOConnect(wParam, lParam );
	} else {
		result = OnVAIBODisconnect(wParam, lParam );
	}
	return result;
}
//	ON_REGISTERED_MESSAGE(WM_VAIBO_CONNECT, OnVAIBOConnect)
LRESULT CBaseClientDlg::OnVAIBOConnect( WPARAM wParam, LPARAM lParam )
{
	if ((unsigned) m_nAiboID != HIWORD(wParam)) {
		// This is not my AIBO that I requested to connect to
		return S_FALSE;
	}
	buttonConnect().SetWindowText( "Disconnect" );
	m_bConnected = TRUE;
	m_bControl = FALSE;
	buttonRemote().EnableWindow(TRUE);
	buttonAutonomy().EnableWindow(FALSE);
	buttonPlay().EnableWindow(FALSE);
	// Check another application getting remote control
	if ( m_pVAibo->IsRemote() ) {
		OnVAIBORemoteNotify(wParam, 1);
	} 
	return S_OK;
}

//	ON_REGISTERED_MESSAGE(WM_VAIBO_DISCONNECT, OnVAIBODisconnect)
LRESULT CBaseClientDlg::OnVAIBODisconnect( WPARAM wParam, LPARAM lParam )
{
	if ((unsigned) m_nAiboID != HIWORD(wParam)) {
		// This is not my AIBO that I was connected to
		return S_FALSE;
	}
	buttonConnect().SetWindowText( "Connect" );
	m_bConnected = FALSE;
	m_bControl = FALSE;
	m_nAiboID = -1;
	m_bNetServiceSensorEnable = FALSE;
	m_bSensorTransferStart = FALSE;
	m_bNetServiceImageEnable = FALSE;
	buttonRemote().EnableWindow(FALSE);
	buttonAutonomy().EnableWindow(FALSE);
	buttonPlay().EnableWindow(FALSE);
	buttonImageTP().EnableWindow(FALSE);
	buttonTakePicture().EnableWindow(FALSE);
	return S_OK;
}
// ON_REGISTERED_MESSAGE(WM_VAIBO_SEMANTICS, OnVAIBOSemantics)
LRESULT CBaseClientDlg::OnVAIBOSemantics( WPARAM wParam, LPARAM lParam )
{
	int	subinfo[4];
	int result;

	if ((unsigned) m_nAiboID != HIWORD(wParam)) {
		// This semantics data didn't come from my AIBO
		return S_FALSE;
	}

	switch (LOWORD(wParam)) {
	case SEM_RESULT : 
		m_pVAibo->GetSemInfo( subinfo );
		if (m_nPlayID == subinfo[0]) {
			// This is my requested command.
			UpdateData(TRUE);
			switch (lParam) {
			case RESULT_COMPLETE :
				m_sPlayStatus.Format("No.%d motion completed", subinfo[0]);
				break;
			case RESULT_INCOMPLETE :
				m_sPlayStatus.Format("No.%d motion incomplete", subinfo[0]);
				break;
			}
			UpdateData(FALSE);
			if (m_bIsBusy && m_bSocketEnable) {
				if (m_pListen)
					m_pListen->m_pRequest->Send("OK\n",3);
				// ignore error
				m_bSocketEnable = FALSE;
			}
			m_bIsBusy = FALSE; // Request Command has finished
		}
		break;
	case SEM_START :
		switch (lParam) {
		case START_IMAGETP :
			if (m_pImageTP) {
				result = m_pVAibo->SendCmd( APPCMD_IMAGE_PLAY ); 
			    if (result != VAIBO_NOERROR) {
				    CString strMsg;
					strMsg.Format(IDS_SENDCMD_ERROR, "APPCMD_IMAGE_PLAY", result);
					AfxMessageBox( strMsg, MB_OK|MB_ICONSTOP );
					return S_FALSE;
			    }
				m_pImageTP->m_bActiveImagePlay = TRUE;
			}
			break;
		case START_SENSORTP :
			result = m_pVAibo->SensorTransferStart();
			if (result != VAIBO_NOERROR) {
				CString strMsg;
				strMsg.Format(IDS_SENSORTRANSFER_START_ERROR, result);
				AfxMessageBox( strMsg, MB_OK|MB_ICONSTOP );
				return S_FALSE;
			}
			m_bNetServiceSensorEnable = TRUE;
			m_bSensorTransferStart = TRUE;
			break;
		}
		break;
	}
	return S_OK;
}

//	ON_REGISTERED_MESSAGE(WM_VAIBO_REMOTE_NOTIFY, OnVAIBORemoteNotify)
LRESULT CBaseClientDlg::OnVAIBORemoteNotify( WPARAM wParam, LPARAM lParam )
{
	CString strMsg;
	int result, state;

	if ((unsigned) m_nAiboID != HIWORD(wParam)) {
		// This is not my AIBO that I requested to connect to
		return S_FALSE;
	}
	if (lParam) {
		// External control
		buttonRemote().EnableWindow(FALSE);
		buttonAutonomy().EnableWindow(TRUE);
		buttonPlay().EnableWindow(TRUE);
		m_bControl = TRUE;
		buttonImageTP().EnableWindow(TRUE);

		state = m_pVAibo->GetStateSensorTP();
		switch (state) {
		case SENSORTP_STATE_CLOSE : 
			if (!m_pVAibo->RequestNetService(SENSOR_ID)) {
				strMsg.Format(IDS_REQNETSERVICE_ERROR, "SENSOR");
				AfxMessageBox( strMsg, MB_OK|MB_ICONSTOP );
				return S_FALSE;
			} 
			m_bNetServiceSensorEnable = TRUE;
			m_bSensorTransferStart = FALSE;
			m_bSetSkipRate = FALSE;
			break;
			// SensorTransferStart() will be invoked in OnVAIBOSemantics()
		case SENSORTP_STATE_SERVICE : 
		case SENSORTP_STATE_PAUSE :
			// Someone has started RequestNetService(SENSOR_ID)
			result = m_pVAibo->SensorTransferStart();
			if (result != VAIBO_NOERROR) {
				strMsg.Format(IDS_SENSORTRANSFER_START_ERROR, result);
				AfxMessageBox( strMsg, MB_OK|MB_ICONSTOP );
				return S_FALSE;
			}
			m_bNetServiceSensorEnable = TRUE;
			m_bSensorTransferStart = TRUE;
			m_bSetSkipRate = FALSE;
			break;
		case SENSORTP_STATE_PLAY  :
			// Someone has started SensorTransferStart()
			m_pVAibo->SensorSetSkipRate(20);
			m_bNetServiceSensorEnable = TRUE;
			m_bSensorTransferStart = TRUE;
			m_bSetSkipRate = TRUE;
			break;
		}
		// Check ImageTP has already started.
		state = m_pVAibo->GetStateImageTP();
		if (m_pImageTP != NULL && state == IMAGETP_STATE_PLAY) {
			buttonTakePicture().EnableWindow(TRUE);
		}
	} else {
		// Internal control
		buttonRemote().EnableWindow(TRUE);
		buttonAutonomy().EnableWindow(FALSE);
		buttonPlay().EnableWindow(FALSE);
		m_bControl = FALSE;
		// Camera image is still coming even if AIBO is in the internal control mode.
		// Sensor data  is still coming even if AIBO is in the internal control mode.
		buttonTakePicture().EnableWindow(FALSE);
		buttonImageTP().EnableWindow(FALSE);
	}
	return S_OK;
}

// ON_REGISTERED_MESSAGE(WM_VAIBO_SENSOR_DATA, OnVAIBOSensorData)
LRESULT CBaseClientDlg::OnVAIBOSensorData( WPARAM wParam, LPARAM lParam )
{
	if ( m_pVAibo != NULL
		&& m_bConnected
		&& (m_pVAibo->GetStateSensorTP() != SENSORTP_STATE_CLOSE)
		&& m_bSensorTransferStart
	    && (unsigned) m_nAiboID == HIWORD(wParam))
		{
			UpDateSensorValue((int)LOWORD(wParam), (SensorRec*)lParam);
			if (!m_bSetSkipRate) {
				m_pVAibo->SensorSetSkipRate(20);
				m_bSetSkipRate = TRUE;
			}
		}
	return S_OK;
}

void CBaseClientDlg::OnBtnConnect()
{
 	if ( !m_bConnected ) {
 		UpdateData();

 		BYTE		nField0, nField1, nField2, nField3;
 		m_ipAddress.GetAddress(nField0, nField1, nField2, nField3 );
 		theApp.m_sAiboIP.Format("%d.%d.%d.%d", nField0, nField1, nField2, nField3 );
		buttonConnect().SetWindowText( "Connecting..." );
		m_myRequest = TRUE;
		int nAiboID;
 		nAiboID = m_pVAibo->Connect((LPCTSTR)theApp.m_sAiboIP);
 		if (nAiboID < 0) {
 			CString msgerr;
 			msgerr.Format(IDS_AIBOCONNECT_ERROR, nAiboID);
 			AfxMessageBox(msgerr);
		}else{
			m_nAiboID = nAiboID;
		}
 	} else {
		buttonConnect().SetWindowText( "Disconnecting..." );
		if ( m_pImageTP != NULL) {
			m_pImageTP->Terminate();
			delete m_pImageTP;
			m_pImageTP = NULL;
		}
		m_myRequest = TRUE;
 		int result = m_pVAibo->Disconnect();
		if (result != VAIBO_NOERROR) {
	 		CString msgerr;
 			msgerr.Format(IDS_AIBODISCONNECT_ERROR, result);
 			AfxMessageBox(msgerr);
		}
	}
}

void CBaseClientDlg::OnBtnPlay()
{
	int result = VAIBO_NOERROR;

	UpdateData();
	if ( m_bControl ) {
		switch (m_iRadio1) {
		// CommandList.txt contains a list of motions that AIBO can perform.
		case 0 : result = m_pVAibo->PlayMotion((0x11 << 24) | (0x00 << 16) | (0x00 << 8) | 0x05,
					 FALSE, FALSE, FALSE); // LMS=0x11 0x00 0x00 Level 5 Hello motion
				break;
		case 1 : result = m_pVAibo->PlayMotion((0x11 << 24) | (0x00 << 16) | (0x03 << 8) | 0x00,
					 FALSE, FALSE, FALSE); // LMS=0x11 0x00 0x03 Bye motion
				break;
		case 2 : result = m_pVAibo->PlayMotion((0x15 << 24) | (0x03 << 16) | (0x00 << 8) | 0x00,
					 FALSE, FALSE, FALSE); // LMS=0x15 0x03 0x00 Dance motion
		}
		if (result < 0 ) {
			CString strMsg;
			strMsg.Format(IDS_PLAYCONTENTS_ERROR, result);
			AfxMessageBox( strMsg, MB_OK|MB_ICONSTOP );
		}
		m_nPlayID = result;
		m_sPlayStatus.Format("No.%d motion is playing", m_nPlayID);
		m_bIsBusy = TRUE;
		UpdateData(FALSE);
	}
}

void CBaseClientDlg::OnBtnRemote()
{
	CString strMsg;
	if ( m_pVAibo != NULL && !m_bControl) {
		// Check another application getting remote control
		if ( m_pVAibo->IsRemote() ) {
			OnVAIBORemoteNotify(((unsigned)m_nAiboID)<<16, 1);
		} else 
		// Start controlling from PC
		if ( !(m_pVAibo->ExternalControl(FALSE)) ) {
			strMsg.Format(IDS_EXTERNALCONTROL_ERROR);
			AfxMessageBox( strMsg, MB_OK|MB_ICONSTOP );
		}
	}
}

void CBaseClientDlg::OnBtnAutonomy()
{
	CString strMsg;
	if ( m_pVAibo != NULL && m_bControl) {
		// Check another application releasing remote control
		if ( !(m_pVAibo->IsRemote()) ) {
			OnVAIBORemoteNotify(((unsigned)m_nAiboID)<<16, 0);
		} else 
		// Stop controlling from PC
		if ( !(m_pVAibo->InternalControl(FALSE)) ) {
			strMsg.Format(IDS_INTERNALCONTROL_ERROR);
			AfxMessageBox( strMsg, MB_OK|MB_ICONSTOP );
		}
	}
}

void CBaseClientDlg::OnDestroy()
{
	CDialog::OnDestroy();
	int result;

	if ( m_pImageTP != NULL) {
		m_pImageTP->Terminate();
		delete m_pImageTP;
		m_pImageTP = NULL;
	}

	if ( m_pVAibo != NULL ) {
		if ( m_bConnected ) {
			 result = m_pVAibo->Disconnect();
			if (result != VAIBO_NOERROR) {
	 			CString msgerr;
 				msgerr.Format(IDS_AIBODISCONNECT_ERROR, result);
 				AfxMessageBox(msgerr);
			}
		}
		delete m_pVAibo;
		m_pVAibo = NULL;
	}
	// Terminate Listen Socket
	StopListening();
}

void CBaseClientDlg::OnBtnSocket()
{
	HICON hNew;
	if ( m_pListen != NULL ) {
		StopListening();
		hNew = ::LoadIcon(AfxGetResourceHandle(), MAKEINTRESOURCE(IDI_REDBALL));
		ASSERT(hNew != NULL);
		::DestroyIcon(SocketStatus().SetIcon(hNew));
		buttonSocket().SetWindowText( "Socket Server ON" );
	} else {
		StartListening();
		hNew = ::LoadIcon(AfxGetResourceHandle(), MAKEINTRESOURCE(IDI_BLUEBALL));
		ASSERT(hNew != NULL);
		::DestroyIcon(SocketStatus().SetIcon(hNew));
		buttonSocket().SetWindowText( "Socket Server OFF" );
	}
	UpdateWindow();                 // draw everything now
}

void CBaseClientDlg::StopListening()
{
	if ( m_pListen != NULL )
	{
		m_pListen->Close();
		delete m_pListen;
		m_pListen = NULL;
	}
}

BOOL CBaseClientDlg::StartListening()
{
	BOOL bOk = FALSE;
	StopListening();
	m_pListen = new CListenSocket(this);
	if ( m_pListen )
	{
		if ( m_pListen->Create( m_uPort, SOCK_STREAM, FD_ACCEPT ) )
			bOk = m_pListen->Listen();

		if ( !bOk )
		{
			CString strMsg;
			int nErr = m_pListen->GetLastError();
			if ( nErr == WSAEADDRINUSE )
				strMsg.Format( IDS_LISTEN_INUSE, m_uPort );
			else
				strMsg.Format( IDS_LISTEN_ERROR, m_uPort );

			AfxMessageBox( strMsg, MB_OK|MB_ICONSTOP );
			m_pListen->Close();
			delete m_pListen;
			m_pListen = NULL;
		}
	}
	else
		AfxMessageBox( IDS_CANT_LISTEN, MB_OK|MB_ICONSTOP );

	return bOk;
}

void CBaseClientDlg::OnBtnImagetp()
{
	if (m_bControl) {
		if (!m_pImageTP) {
			m_pImageTP = new CImageTP(m_pVAibo, this);
		}
		if (!m_pImageTP->Initialize()) {
			delete m_pImageTP;
			m_pImageTP = NULL;
			buttonTakePicture().EnableWindow(FALSE);
			return;
		}
		buttonTakePicture().EnableWindow(TRUE);
	}
}

void CBaseClientDlg::OnBtnTakepicture()
{
	if (m_pImageTP) {
		BOOL bOk = m_pImageTP->TakePicture(PHOTO_FILE);
		if (!bOk) m_nPlayID = -1;
		m_bIsBusy = FALSE;
	}
}

void CBaseClientDlg::TakePicture(LPCSTR pPhotoFile)
{
	if (m_pImageTP) {
		BOOL bOk = m_pImageTP->TakePicture(pPhotoFile);
		if (!bOk) m_nPlayID = -1;
		m_bIsBusy = FALSE;
	}
}

LRESULT CBaseClientDlg::OnITPImageUpdate( WPARAM wParam, LPARAM recvLen )
{
	if ((unsigned) m_nAiboID != HIWORD(wParam)) {
		// This semantics data didn't come from my AIBO
		return S_FALSE;
	}
	if (m_pImageTP) {
		if (!m_pImageTP->m_bDoneSetParam) {
			int result;
			result = m_pVAibo->SendCmd( APPCMD_IMAGE_SETPARAM, TRUE, CMDSID_PROFILE, IMAGE_PROFILE_HIGH );
		    if (result != VAIBO_NOERROR) {
				CString strMsg;
			    strMsg.Format(IDS_SENDCMD_ERROR, "APPCMD_IMAGE_SETPARAM(IMAGE_PROFILE_HIGH)", result);
				AfxMessageBox( strMsg, MB_OK|MB_ICONSTOP );
				return S_FALSE;
			}
			m_pImageTP->m_bDoneSetParam = TRUE;
		}
		if (!m_pImageTP->HandleImageData((UINT)recvLen))
			return S_FALSE;
	}
	return S_OK;
}


void CBaseClientDlg::DoWalking(int walkstyle, int distance, int angle)
{
	if ( m_pVAibo != NULL ) {
		if ( m_bControl ) {
			int result;
			result = m_pVAibo->DoWalking(walkstyle, angle, distance, FALSE);
			if (result < 0 ) {
				CString strMsg;
				strMsg.Format(IDS_DOWALKING_ERROR, result);
				AfxMessageBox( strMsg, MB_OK|MB_ICONSTOP );
			}
			m_nPlayID = result;
			m_sPlayStatus.Format("No.%d motion is playing", m_nPlayID);
			m_bIsBusy = TRUE;
			UpdateData(FALSE);
		}
	}
}

void CBaseClientDlg::MoveHead(int angleH,int angleV)
{
	if ( m_pVAibo != NULL ) {
		if ( m_bControl ) {
			int result;
			m_iTargetHeadAngleH += angleH;
			m_iTargetHeadAngleV += angleV;
			if (m_iTargetHeadAngleH > 90) m_iTargetHeadAngleH = 90;
			if (m_iTargetHeadAngleH < -90) m_iTargetHeadAngleH = -90;
			if (m_iTargetHeadAngleV > 90) m_iTargetHeadAngleV = 90;
			if (m_iTargetHeadAngleV < -90) m_iTargetHeadAngleV = -90;

			result = m_pVAibo->ChangeHeadAngle(m_iTargetHeadAngleH, m_iTargetHeadAngleV, 3, FALSE);
			if (result < 0 ) {
				CString strMsg;
				strMsg.Format(IDS_CHANGEHEADANGLE_ERROR, result);
				AfxMessageBox( strMsg, MB_OK|MB_ICONSTOP );
			}
			m_nPlayID = result;
			m_sPlayStatus.Format("No.%d motion is playing", m_nPlayID);
			m_bIsBusy = TRUE;
			m_bUpdateHeadAngle = TRUE;
			UpdateData(FALSE);
		}
	}
}

void CBaseClientDlg::MoveHeadCenter()
{
	if ( m_pVAibo != NULL ) {
		if ( m_bControl ) {
			int result;
			m_iTargetHeadAngleH = 0;
			m_iTargetHeadAngleV = 0;
			result = m_pVAibo->ChangeHeadAngle(0, 0, 3, FALSE);
			if (result < 0 ) {
				CString strMsg;
				strMsg.Format(IDS_CHANGEHEADANGLE_ERROR, result);
				AfxMessageBox( strMsg, MB_OK|MB_ICONSTOP );
			}
			m_nPlayID = result;
			m_sPlayStatus.Format("No.%d motion is playing", m_nPlayID);
			m_bIsBusy = TRUE;
			m_bUpdateHeadAngle = TRUE;
		}
	}
	m_bUpdateHeadAngle = TRUE;
	UpdateData(FALSE);
}

void CBaseClientDlg::UpDateSensorValue(int numOfData, SensorRec *sensorRecAry)
{
	if (!m_bUpdateHeadAngle || m_bIsBusy)
		return;
	m_bUpdateHeadAngle = FALSE;

	int iHeadTilt(0), iHeadTilt2(0), iHeadPan(0);
	// Loop for each sensor data

	for (int i = 0; i < numOfData; i++) {
		// Sensor value
		long lValue = (long)(sensorRecAry[i].value);
		// Sensor ID
		switch (sensorRecAry[i].sensorID) {
		case HeadTilt:
			iHeadTilt  = (int)(lValue/3141592.0*180.0);  //in degrees
			break;
		case HeadPan:
			iHeadPan   = (int)(lValue/3141592.0*180.0);  //in degrees
			break;
		case HeadTilt2:
			iHeadTilt2 = (int)(lValue/3141592.0*180.0);	 //in degrees
			break;  
		}
	}
	m_iHeadAngleH = iHeadTilt + iHeadTilt2;
	m_iHeadAngleV = iHeadPan;
	UpdateData(FALSE);
}

void CBaseClientDlg::OnBtnHeadup()
{
	MoveHead(0, 10);
}

void CBaseClientDlg::OnBtnHeadleft()
{
	MoveHead(10, 0);
}

void CBaseClientDlg::OnBtnHeadright()
{
	MoveHead(-10, 0);
}

void CBaseClientDlg::OnBtnHeaddown()
{
	MoveHead(0, -10);
}

void CBaseClientDlg::OnBtnHeadhome()
{
	MoveHeadCenter();
}

void CBaseClientDlg::OnBtnForward()
{
	DoWalking(DW_FORWARD_FAST, 30, 0);
}

void CBaseClientDlg::OnBtnLeftturn()
{
	DoWalking(DW_TURN_LEFT, 0, 45);
}

void CBaseClientDlg::OnBtnWalkstop()
{
	DoWalking(DW_STOP, 0, 0);
}

void CBaseClientDlg::OnBtnRightturn()
{
	DoWalking(DW_TURN_RIGHT, 0, 45);
}

void CBaseClientDlg::OnBtnBack()
{
	DoWalking(DW_BACK, 30, 0);
}