// 3DDlg.cpp : Implementation file
//

#include "stdafx.h"
#include "RemoteTest.h"
#include "3DDlg.h"
#include "CpcInfo.h"
#include <math.h>

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

#define		DATAPATH		"\\ModelData"		// ModelData file path

/////////////////////////////////////////////////////////////////////////////
// C3DDlg  dialog


C3DDlg::C3DDlg( int robotDesign, int posture, CWnd* pParent /*=NULL*/)
	: CDialog(C3DDlg::IDD, pParent)
{
	//{{AFX_DATA_INIT(C3DDlg)
	//}}AFX_DATA_INIT
	m_robotDesign = robotDesign;
	m_posture = posture;
	m_aibo3D = NULL;
	m_d3DPosX = 0.0f;
	m_d3DPosY = 0.0f;
	m_d3DPosZ = 0.0f;

	m_d3DPitch = 0.0f;
	m_d3DYaw = 0.0f;

	m_d3DHomePitch = 0.0f;
	m_d3DHomeYaw = 0.0f;

	m_ptMouse.x = 0;
	m_ptMouse.y = 0;
	m_b3DViewChange = FALSE;

	m_jointArray = NULL;
	m_sensorArray = NULL;
	m_sensorID = NULL;
	m_sensor = NULL;

	pModelIDIndex = NULL;

}

void C3DDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(C3DDlg)
	//}}AFX_DATA_MAP
}


BEGIN_MESSAGE_MAP(C3DDlg, CDialog)
	//{{AFX_MSG_MAP(C3DDlg)
	ON_WM_DESTROY()
	ON_BN_CLICKED(IDC_BTN_LOAD, OnBtnLoad)
	ON_BN_CLICKED(IDC_BTN_HOME, OnBtnHomePosition)
	ON_BN_CLICKED(IDC_RADIO_MOVE, OnRadioMove)
	ON_BN_CLICKED(IDC_RADIO_TURN, OnRadioTurn)
	ON_BN_CLICKED(IDC_RADIO_ZOOM, OnRadioZoom)
	ON_BN_CLICKED(IDC_BUTTON_FLOOR, OnBtnFloor)
	ON_BN_CLICKED(IDC_BUTTON_BACK, OnBtnBack)
	ON_BN_CLICKED(IDC_BUTTON_FLOOR2, OnBtnFloor2)
	ON_BN_CLICKED(IDC_BUTTON_BACK2, OnBtnBack2)

	ON_WM_LBUTTONDOWN()
	ON_WM_LBUTTONUP()
	ON_WM_MOUSEMOVE()
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// C3DDlg message handlers

void C3DDlg::OnOK()
{
	CDialog::OnOK();
}

void C3DDlg::OnDestroy()
{
	CDialog::OnDestroy();

	if ( m_aibo3D != NULL ) {
		delete m_aibo3D;
	}
	if ( m_sensor ) free(m_sensor);
	if ( m_jointArray ) free(m_jointArray);
	if ( m_sensorArray ) free(m_sensorArray);
	if ( m_sensorID ) free(m_sensorID);

	if ( pModelIDIndex ) delete pModelIDIndex;
}

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

	CheckRadioButton( IDC_RADIO_MOVE, IDC_RADIO_ZOOM, IDC_RADIO_MOVE );
	m_3DMode = VM3D_MOVE;

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

}

void C3DDlg::OnBtnLoad()
{
	CRect		rect;
	CString		m_dstPath;
	int			err;
	int			modelKind;

	CRemoteTestApp	*pApp = (CRemoteTestApp*)AfxGetApp();
	CCpcInfo	cpc(m_robotDesign);

	if ( m_jointArray ) free(m_jointArray);
	if ( m_sensorArray ) free(m_sensorArray);
	if ( m_sensorID ) free(m_sensorID);
	if ( m_sensor ) free(m_sensor);

	int num = cpc.GetJointArray(NULL);
	m_jointArray = (int*)malloc(sizeof(int)*num);
	cpc.GetJointArray(m_jointArray);

	m_sensorArrayNum = cpc.GetSensorArray(NULL);
	m_sensorArray = (int*)malloc(sizeof(int)*m_sensorArrayNum);
	cpc.GetSensorArray(m_sensorArray);
	m_sensor = (DWORD*)calloc(sizeof(DWORD),SENSOR_MAX/*m_sensorArrayNum*/);
	m_dstPath = pApp->m_appPath + DATAPATH;

	CWnd	*wnd = GetDlgItem(IDC_STATIC_3D);
	wnd->GetWindowRect(&rect);
	this->ScreenToClient(&rect);
	modelKind = DESIGN_ERS7;

	if ( m_robotDesign != DESIGN_ERS7 ) {
		AfxMessageBox( "Invalid RobotDesign" );
		return;
	}

	CAIBO3D		*aibo3D;
	aibo3D = new CAIBO3D;
	err = aibo3D->Create3D( modelKind, (char*)(LPCTSTR)m_dstPath, this->m_hWnd, rect, RGB(0,0,0), RGB(0,255,0) );
	if ( err != AIBO3D_NOERROR ) {
		delete aibo3D;
		AfxMessageBox( "Couldn't create 3D model" );
		return;
	}
	m_aibo3D = aibo3D;

	m_aibo3D->HomePosition();
	m_aibo3D->ClearTrack();
	m_aibo3D->GetCameraPos(&m_d3DHomePosX, &m_d3DHomePosY, &m_d3DHomePosZ);
	m_d3DHomePosZ = -0.56f;		// Make this the default zoom.
	m_d3DHomePosY = -0.15f;		// Make this the default Y coordinate.

	// The initial position is the home position.
	m_d3DPosX = m_d3DHomePosX;
	m_d3DPosY = m_d3DHomePosY;
	m_d3DPosZ = m_d3DHomePosZ;
	m_d3DPitch = m_d3DHomePitch;
	m_d3DYaw = m_d3DHomeYaw;

	m_aibo3D->RotateCamera(m_d3DPitch, m_d3DYaw);
	m_aibo3D->MoveCamera( 0.0f, -0.15f );
	m_aibo3D->ZoomCamera( -0.56f );
	m_aibo3D->Set3DPosture( m_posture );

	// Duplicate models.
	{
		// The first body
		m_aibo3D->SetCopyModelTransfer( 0, -0.3f, 0.0f, -0.3f );
		m_aibo3D->SetCopyModelRotation( 0, 0.0f,  45.0f, 0.0f );
		// The second body
		m_aibo3D->SetCopyModelTransfer( 1,  0.3f, 0.0f, -0.3f );
		m_aibo3D->SetCopyModelRotation( 1, 0.0f, -45.0f, 0.0f );
		// The third body
		m_aibo3D->SetCopyModelTransfer( 2, -0.3f, 0.0f,  0.3f );
		m_aibo3D->SetCopyModelRotation( 2, 0.0f, 135.0f, 0.0f );
		// The fourth body
		m_aibo3D->SetCopyModelTransfer( 3,  0.3f, 0.0f,  0.3f );
		m_aibo3D->SetCopyModelRotation( 3, 0.0f, -135.0f, 0.0f );
	}

	// Text drawing
	// 2D
	m_aibo3D->CreateDrawText2DSurface( 100, 50 );
	m_aibo3D->SetDrawText2DDrawArea( 10, 10, 100, 50 );
	m_aibo3D->SetDrawText2DColor( RGB( 0x00, 0x00, 0xff ) );
	m_aibo3D->SetDrawText2DBackTransparent( true );
	m_aibo3D->ShowDrawText2D( true );
	// 3D
	m_aibo3D->CreateDrawText3DSurface( 100, 50 );
	m_aibo3D->SetDrawText3DBasePoint( 0.0f, 0.0f, 0.0f );
	m_aibo3D->SetDrawText3DDrawArea( -0.05f, 0.45f, 0.4f, 0.2f );
	m_aibo3D->SetDrawText3DBackColor( RGB( 0xff, 0xff, 0xff ) );
	HFONT	hFont = CreateFont(
						16,								// height of font
				        0,								// average character width
					    0,								// angle of escapement
					    0,								// base-line orientation angle
						FW_NORMAL,						// font weight
						FALSE,							// italic attribute flag
						FALSE,							// underline attribute flag
						FALSE,							// strikeout attribute flag
						SHIFTJIS_CHARSET,				// character set identifier
						OUT_DEFAULT_PRECIS,				// output precision
						CLIP_DEFAULT_PRECIS,			// clipping precision
						DRAFT_QUALITY,					// output quality
						DEFAULT_PITCH | FF_DONTCARE,	// pitch and family
						"Courier New"				 // pointer to typeface name string
				  );
	m_aibo3D->SetDrawText3DFont( hFont );
	m_aibo3D->ShowDrawText3D( true );


	m_aibo3D->Show();

	buttonLoad().EnableWindow( false );
}

void C3DDlg::OnBtnHomePosition()
{
	m_d3DPosX = m_d3DHomePosX;
	m_d3DPosY = m_d3DHomePosY;
	m_d3DPosZ = m_d3DHomePosZ;
	m_d3DPitch = m_d3DHomePitch;
	m_d3DYaw = m_d3DHomeYaw;

	if( m_aibo3D ) {
		m_aibo3D->RotateCamera(m_d3DPitch, m_d3DYaw);
		m_aibo3D->MoveCamera(m_d3DPosX, m_d3DPosY);
		m_aibo3D->ZoomCamera(m_d3DPosZ);
		m_aibo3D->ClearTrack(TRUE);
	}
}

void C3DDlg::OnRadioMove()
{
	if ( m_3DMode != VM3D_MOVE ) {
		m_3DMode = VM3D_MOVE;
		CheckRadioButton( IDC_RADIO_MOVE, IDC_RADIO_ZOOM, IDC_RADIO_MOVE );
	}
}

void C3DDlg::OnRadioTurn()
{
	if ( m_3DMode != VM3D_TURN ) {
		m_3DMode = VM3D_TURN;
		CheckRadioButton( IDC_RADIO_MOVE, IDC_RADIO_ZOOM, IDC_RADIO_TURN );
	}
}

void C3DDlg::OnRadioZoom()
{
	if ( m_3DMode != VM3D_ZOOM ) {
		m_3DMode = VM3D_ZOOM;
		CheckRadioButton( IDC_RADIO_MOVE, IDC_RADIO_ZOOM, IDC_RADIO_ZOOM );
	}
}

void C3DDlg::OnLButtonDown(UINT nFlags, CPoint point)
{
	if ( m_aibo3D != NULL ) {
		CPoint pt = point;

		// Check if clicked in 3DView.
		CRect rect;
		CWnd	*wnd = GetDlgItem(IDC_STATIC_3D);
		wnd->GetWindowRect(&rect);
		this->ScreenToClient(&rect);

		if( ::PtInRect( &rect, pt ) ) {
			// Start capture.
			SetCapture();

			// Hold mouse position.
			m_ptMouse = point;

			m_b3DViewChange = TRUE;
		}
	}

	CDialog::OnLButtonDown(nFlags, point);
}

void C3DDlg::OnLButtonUp(UINT nFlags, CPoint point)
{
	if ( m_b3DViewChange ) {
		::ReleaseCapture();
	}
	m_b3DViewChange = FALSE;

	CDialog::OnLButtonUp(nFlags, point);
}

void C3DDlg::OnMouseMove(UINT nFlags, CPoint point)
{
	if ( m_aibo3D == NULL ) {
		return;
	}

	if ( m_b3DViewChange ) {
		CPoint pt = (m_ptMouse - point);
		switch(m_3DMode){
		case VM3D_MOVE:
			m_d3DPosX -= (pt.x * 0.005f);
			m_d3DPosY += (pt.y * 0.005f);

			m_aibo3D->MoveCamera( m_d3DPosX, m_d3DPosY );
			break;
		case VM3D_TURN:
			m_d3DPitch += (pt.y * 0.5f);
			m_d3DYaw += (pt.x * 0.5f);

			m_aibo3D->RotateCamera(m_d3DPitch, m_d3DYaw);
			break;
		case VM3D_ZOOM:
			// Stay in the range of -4.0 to -0.30 .
			m_d3DPosZ -= (pt.y * 0.01f);

			if(m_d3DPosZ < -4.0f)
				m_d3DPosZ = -4.0f;
			else if ( m_d3DPosZ > -0.30f )
				m_d3DPosZ = -0.30f;
			m_aibo3D->ZoomCamera(m_d3DPosZ);
			break;
		default:
			break;
		}

		m_ptMouse = point;
	}

	CDialog::OnMouseMove(nFlags, point);
}

/*---------------------------------------------------------
	SetJoints	:	Update the joint angles of the 3D model.

	Argument
		angle	:	Angle array
		num		:	The number of angle elements in the array
	Return value
		NONE
	Explanation
		Each joint is updated to the specified angle and
		and the display is updated.
 ---------------------------------------------------------*/
void C3DDlg::SetJoints( float *angle,int num )
{
	if ( m_aibo3D ) {
		m_aibo3D->RotateJoint( m_jointArray, angle, num,TRUE );
	}
}


void C3DDlg::UpdateSensorValue(unsigned long numOfData, SensorRec *sensorRecAry)
{
	DWORD		i;
	int			j;

	if ( m_aibo3D == NULL ) return;

	for ( i = 0; i < numOfData; i++ ) {
		int	ix = m_sensorArray[sensorRecAry[i].sensorID];
		if ( ix != -1 ) {	// Joint information.
			m_sensor[ix] = sensorRecAry[i].value;
		}
		else {
			if ( sensorRecAry[i].sensorID == PSDWithXY ) {	// PSD data
				// Describe PSD data displaying process.
			} else if ( sensorRecAry[i].sensorID == Acc1 ) {
				QueueIn(0,(int)sensorRecAry[i].value);
			} else if ( sensorRecAry[i].sensorID == Acc2 ) {
				QueueIn(1,(int)sensorRecAry[i].value);
			} else if ( sensorRecAry[i].sensorID == Acc3 ) {
				QueueIn(2,(int)sensorRecAry[i].value);
			}
		}
	}
	// Make an array of floating point angles.
	float	*angle = (float*)malloc(sizeof(float)*SENSOR_MAX/*m_sensorNum*/);
	for ( j = SENSOR_HEAD_TILT; j < SENSOR_MAX/*m_sensorNum*/; j++ ) {
		angle[j] = (float)RadToDegree(m_sensor[j]);
	}
	SetJoints( angle, SENSOR_MAX/*m_sensorNum*/ );		// renew 3D joints

	/*
		Find the rotation angle of the body from the acceleration sensor values.
	*/

	float	x, y, z;
	float	pitch, roll;

	x = (float)GetAverage(1); 	// Acc2
	y = (float)GetAverage(0);	// Acc1
	z = (float)GetAverage(2);	// Acc3

	CalcRotation( x, y, z, &pitch, &roll );		// Rotation angle calculation
	m_aibo3D->GetBodyAngle( &x, &y, &z );
	m_aibo3D->RotateBody( pitch, y, roll, FALSE );

	free(angle);
}

int	C3DDlg::RadToDegree( int rad )
{
	return (int)((float)rad / 3141592 * 180.0);
}

// Add value to the end of the queue.
void C3DDlg::QueueIn( int ix,int value )
{
	int		i;
	for ( i=1 ; i<ACC_QUEUE_MAX ; i++ )
		m_accQueue[ix][i-1] = m_accQueue[ix][i];
	m_accQueue[ix][ACC_QUEUE_MAX-1] = value;
}

int	C3DDlg::GetAverage( int ix )
{
	int		i;
	float	total;

	total = 0.0f;
	for ( i=0 ; i<ACC_QUEUE_MAX ; i++ ) total += (float)m_accQueue[ix][i];
	return (int)(total / (float)ACC_QUEUE_MAX);
}

/*---------------------------------------------------------
	CalcRotation	:	The calculation of the rotation angle

	Argument
		x,y,z	:	Acceleration sensor values
		pitch	:	The front-back rotation angle pointer
		roll	:	The left-right rotation angle pointer
	Return value
		NONE
	Explanation
		Find the rotation angle of the body from the acceleration sensor values.
 ---------------------------------------------------------*/
void C3DDlg::CalcRotation( float x,float y,float z,float *pitch,float *roll )
{
	/*
		x, y and z must be scaled to range from -1.0 to 1.0 before asin and acos are calculated.
	*/
	x /= 9806650.0f; y /= 9806650.0f; z /= 9806650.0f;
	if ( x > 1.0f ) x = 1.0f; else if ( x < -1.0f ) x = -1.0f;
	if ( y > 1.0f ) y = 1.0f; else if ( y < -1.0f ) y = -1.0f;
	if ( z > 1.0f ) z = 1.0f; else if ( z < -1.0f ) z = -1.0f;

	/*
		The calculation of the front-back rotation angle
	*/
	*pitch = -(float)asin(y) * 180.0f / 3.141592f;

	/*
		The calculation of the left-right rotation angle
	*/
	*roll = (float)asin(x) * 180.0f / 3.141592f;

	/*
		When 'z' is positive, the pitch or roll (whichever is further away from 0)
		will range from -90 to -180 or from 90 to 180 (depending if the original value is greater than 0)

	*/
	if ( z > 0.0f ) {
		if ( abs((int)*pitch) > abs((int)*roll) ) {
			if ( *pitch < 0.0f )
				*pitch = -180.0f - *pitch;	// -90 .. -180
			else
				*pitch = 180.0f - *pitch;	// 90 .. 180
		}
		else {
			if ( *roll < 0.0f )
				*roll = -180.0f - *roll;	// -90 .. -180
			else
				*roll = 180.0f - *roll;		// 90 .. 180
		}
	}
}

void C3DDlg::SetPosture( int posture )
{
	m_posture = posture;

	if ( m_aibo3D ) {
		m_aibo3D->Set3DPosture( posture );
	}
}


void	C3DDlg::OnBtnFloor()
{
	if ( m_aibo3D == NULL ) {
		return;
	}

	// Choose a bitmap file.
	CString		strFile;
	CFileDialog	dlg( TRUE, NULL, NULL, OFN_FILEMUSTEXIST, "bitmap file(*.bmp)|*.bmp", this );
	if ( dlg.DoModal() == IDOK ) {
		strFile = dlg.GetPathName();
		// Set the floor surface texture
		m_aibo3D->SetFloorTexture( strFile.GetBuffer( 0 ) );
	}
}

void	C3DDlg::OnBtnBack()
{
	if ( m_aibo3D == NULL ) {
		return;
	}

	// Choose a bitmap file.
	CString		strFile;
	CFileDialog	dlg( TRUE, NULL, NULL, OFN_FILEMUSTEXIST, "bitmap file(*.bmp)|*.bmp", this );
	if ( dlg.DoModal() == IDOK ) {
		strFile = dlg.GetPathName();
		// Set the background texture
		m_aibo3D->SetBackTexture( strFile.GetBuffer( 0 ) );
	}
}

void	C3DDlg::OnBtnFloor2()
{
	if ( m_aibo3D == NULL ) {
		return;
	}

	// Delete the floor surface texture
	m_aibo3D->DeleteFloorTexture();
}

void	C3DDlg::OnBtnBack2()
{
	if ( m_aibo3D == NULL ) {
		return;
	}

	// Delete the background texture
	m_aibo3D->DeleteBackTexture();
}

void	C3DDlg::MoveAIBO( int nWalk, float fSpeed )
{
	if ( m_aibo3D == NULL ) {
		return;
	}


	DWORD dwDelay;
	switch ( m_posture ) {
		case POSE_3D_STANDUP:
		case POSE_3D_WALKING:
			dwDelay = 0;
			break;
		case POSE_3D_NORMAL:
		case POSE_3D_LIE:
			dwDelay = 4000;
			break;
		case POSE_3D_SITDOWN:
			dwDelay = 10000;
			break;
		default:
			dwDelay = 0;
			break;
	}


	switch ( nWalk ) {
		// STOP
		case 0:
			m_aibo3D->RotateAIBOAnimation( 0.0f );			// Stop rotation.
			m_aibo3D->MoveAIBOAnimation( FALSE, 0.0f );		// Stop movement.
			m_aibo3D->PlayAnimation( 0, TRUE, 4000 );		// Animate to express a stop (there is a delay)
			break;
		// WALK
		case 1:
			m_aibo3D->RotateAIBOAnimation( 0.0f );			// Stop rotation.
			m_aibo3D->MoveAIBOAnimation( TRUE, fSpeed );	// Movement
			m_aibo3D->PlayAnimation( -1, TRUE, dwDelay );	// Animate to express a movement
			break;
		// ROTATE
		case 2:
			m_aibo3D->MoveAIBOAnimation( TRUE, 0.0f );		// Stop movement.
			m_aibo3D->RotateAIBOAnimation( fSpeed );		// Rotation
			m_aibo3D->PlayAnimation( -1, TRUE, dwDelay );	// Animate to express a rotation
			break;
		default:
			break;
	}
}