/*
todo standards
*/

//#include "RSPiX.h"
#include <math.h>
#include "maple.h"
#include "../RSPiX/Src/GREEN/InputEvent/InputEvent.h"


/// <summary>
///	todo
/// </summary>
void MAPLE::cMapleController::Init()
{	
	// Init the controller device.	
	m_pDeviceController = maple_enum_dev(m_cControllerId, MAPLE_CONTROLLER_PORT);		
	
	if (m_pDeviceController)
	{				
		m_bExtended = false;
		if (m_cControlSchemeId == 4)
		{
			m_cControlSchemeId = 0;
		}

		if (m_pDeviceController->info.functions & MAPLE_FUNC_CONTROLLER)
		{
			m_bConnected = true;						
		}
		
		uint32_t cap = CONT_CAPABILITY_DPAD2_UP;
		cap = ((cap >> 24) & 0xFF) | ((cap >> 8) & 0xFF00) | ((cap & 0xFF00) << 8) | ((cap & 0xFF) << 24);
		if (m_pDeviceController->info.function_data[0] & cap)
		{					
			m_bExtended = true;			
			m_cControlSchemeId = 4;
		}				
	}
	else
	{		
		m_bConnected = false;
	}
}

/// <summary>
///	todo
/// </summary>
int8_t MAPLE::cMapleController::Status()
{		
	if (m_bConnected && maple_dev_valid(m_pDeviceController->port, m_pDeviceController->unit))
	{		
		m_ulDisconnectedCycles = 0;		
		m_pState = (cont_state_t*) maple_dev_status(m_pDeviceController);
		
		// Update the memory card status.
		int8_t status = m_pMemCards[MAPLE_MEMCARD_1]->Status();		
		if (status == cMapleModule::Disconnected || status == cMapleModule::Error)
		{
			return VmuDisconnected;
		}
		else if (status == cMapleModule::Uninitialised)
		{
			m_pMemCards[MAPLE_MEMCARD_1]->Init();
		}
	
		status = m_pMemCards[MAPLE_MEMCARD_2]->Status();		
		if (status == cMapleModule::Disconnected || status == cMapleModule::Error)
		{
			return VmuDisconnected;
		}
		else if (status == cMapleModule::Uninitialised)
		{
			m_pMemCards[MAPLE_MEMCARD_2]->Init();
		}
		
		return ControllerConnected;
	}	
	else
	{	
		if (m_bConnected)
		{
			ClearMapleInputState(m_cControllerId);
		}

		m_bConnected = false;
		m_pDeviceController = NULL;
		m_pState = NULL;		
		m_ulDisconnectedCycles++;
		// Try and re-initialise ready for the next cycle.		
		Init();	
	}	

	return ControllerDisconnected;
}

/// <summary>
///	todo
/// </summary>
void MAPLE::cMapleController::InitVirtualStick(int8_t cStick, int8_t cRate, int16_t sButtonUp, int16_t sButtonDown, int16_t sButtonLeft, int16_t sButtonRight)
{
	m_pVirtualSticks[cStick] = new cMapleVirtualStick(this, float(cRate) / 10.f);
	m_pVirtualSticks[cStick]->SetButtons(sButtonUp, sButtonDown, sButtonLeft, sButtonRight);
}

/// <summary>
///	todo
/// </summary>
void MAPLE::cMapleController::CanReboot()
{
	cont_btn_callback(maple_addr(m_cControllerId, 0), CONT_A | CONT_B | CONT_X | CONT_Y | CONT_START, (cont_btn_callback_t)arch_reboot);	
}

/// <summary>
///	todo
/// </summary>
bool MAPLE::cMapleController::ButtonDown(uint16_t button)
{				
	if (m_pState && (m_pState->buttons & button))
		return true;		
	return false;
}

/// <summary>
///	todo
/// </summary>
void MAPLE::cMapleController::GetAxis(int16_t* x, int16_t* y, int8_t cStickNumber) 
{			
	// Virtual stick	
	if (cStickNumber > 0)
	{	
		m_pVirtualSticks[cStickNumber - 1]->GetAxis(x, y);
	}
	else
	{
		// Actual analogue
		if (m_pState)
		{
			*x = m_pState->joyx;
			*y = m_pState->joyy;							
		}
		else
		{
			*x = 0;
			*y = 0;
		}	
	}		
}

/// <summary>
///	todo
/// </summary>
bool MAPLE::cMapleController::TriggerPulled(bool trigger)
{		
	if (m_pState && (0x7f < ((!trigger) ? m_pState->ltrig : m_pState->rtrig)))
	{	
		return true;
	}		
	return false;
}

/// <summary>
///	todo
/// </summary>
bool MAPLE::cMapleController::GetStickAngle(float *fAngle, int8_t stick)
{
	int16_t axis_X, axis_Y;
	GetAxis(&axis_X, &axis_Y, stick);

	if (axis_X == 0 && axis_Y == 0)
	{
		return false;
	}
	
	if (axis_X != 0)
	{
		*fAngle = atan((float)-axis_Y / (float)axis_X) * 180.f / M_PI;
		if (axis_X < 0)
			*fAngle -= 180.f;
	}
	else if (axis_Y < 0)
		*fAngle = 90.f;
	else if (axis_Y > 0)
		*fAngle = -90.f;
	else
		*fAngle = 0;

	return true;
}

void MAPLE::cMapleVirtualStick::SetButtons(int16_t sButtonUp, int16_t sButtonDown, int16_t sButtonLeft, int16_t sButtonRight)
{	
	m_sButtonUp = sButtonUp;
	m_sButtonDown = sButtonDown;
	m_sButtonLeft =	sButtonLeft;
	m_sButtonRight = sButtonRight;
}

void MAPLE::cMapleVirtualStick::GetAxis(int16_t* x, int16_t* y)
{						
	bool buttonPress = false;	
	int32_t lElapsedTime = 0;
			
	if (m_lLastTime == 0)
	{
		lElapsedTime = 0;
	}
	else
	{
		lElapsedTime = int32_t(timer_ms_gettime64()) - m_lLastTime;
	}

	m_lLastTime = int32_t(timer_ms_gettime64());
	int16_t increment = int16_t((float(lElapsedTime) * m_fRate));
	
	if (m_pController->ButtonDown(m_sButtonLeft))
	{						
		buttonPress = true;
			
		if (m_pController->ButtonDown(m_sButtonUp))
		{
			m_sTargetAxisX = int16_t(-64);
			m_sTargetAxisY = int16_t(-64);				
				m_sCurrDirection = 2;
		}
		else if (m_pController->ButtonDown(m_sButtonDown))
		{
			m_sTargetAxisX = int16_t(-64);
			m_sTargetAxisY = int16_t(64);				
			m_sCurrDirection = 8;
		}
		else
		{				
			m_sTargetAxisX = int16_t(-127);
			m_sTargetAxisY = 0;				
			m_sCurrDirection = 1; // todo
		}									
	}
		
		if (m_pController->ButtonDown(m_sButtonUp))
		{
			buttonPress = true;						

			if (m_pController->ButtonDown(m_sButtonLeft))
			{
				m_sTargetAxisX = int16_t(-64);
				m_sTargetAxisY = int16_t(-64);				
				m_sCurrDirection = 2;
			}
			else if (m_pController->ButtonDown(m_sButtonRight))
			{
				m_sTargetAxisX = int16_t(64);
				m_sTargetAxisY = int16_t(-64);				
				m_sCurrDirection = 4;
			}
			else
			{
				m_sTargetAxisX = 0;
				m_sTargetAxisY = int16_t(-127);				
				m_sCurrDirection = 3;
			}			
		}
	
		if (m_pController->ButtonDown(m_sButtonRight))
		{
			buttonPress = true;						
			if (m_pController->ButtonDown(m_sButtonUp))
			{
				m_sTargetAxisX = int16_t(64);
				m_sTargetAxisY = int16_t(-64);				
				m_sCurrDirection = 4;
			}
			else if (m_pController->ButtonDown(m_sButtonDown))
			{
				m_sTargetAxisX = int16_t(64);
				m_sTargetAxisY = int16_t(64);				
				m_sCurrDirection = 6;
			}
			else
			{
				m_sTargetAxisX = int16_t(127);
				m_sTargetAxisY = int16_t(0);
				m_sCurrDirection = 5;
			}					
		}
		
		if (m_pController->ButtonDown(m_sButtonDown))
		{
			buttonPress = true;			
			if (m_pController->ButtonDown(m_sButtonLeft))
			{				
				m_sTargetAxisX = int16_t(-64);
				m_sTargetAxisY = int16_t(64);
				m_sCurrDirection = 8;
			}
			else if (m_pController->ButtonDown(m_sButtonRight))
			{				
				m_sTargetAxisX = int16_t(64);
				m_sTargetAxisY = int16_t(64);
				m_sCurrDirection = 6;
			}
			else
			{
				m_sTargetAxisX = 0;
				m_sTargetAxisY = int16_t(127);
				m_sCurrDirection = 7;
			}					
		}
				
		if (!m_sPrevDirection)
		{
			m_sPrevDirection = m_sCurrDirection;
		}

		if (m_sPrevDirection == m_sCurrDirection && m_bInitialButtonPress)	
		{
			m_sCurrAxisX= m_sTargetAxisX;
			m_sCurrAxisY = m_sTargetAxisY;
			m_bInitialButtonPress = false;
		}			
		else
		{				
			// todo: time delta
			if (m_sCurrAxisX> m_sTargetAxisX) 
			{
				m_sCurrAxisX -= increment;
			}
			if (m_sCurrAxisX< m_sTargetAxisX)
			{
				m_sCurrAxisX += increment;
			}

			if (m_sCurrAxisY > m_sTargetAxisY)
			{
				m_sCurrAxisY -= increment;
			}
			if (m_sCurrAxisY < m_sTargetAxisY)
			{
				m_sCurrAxisY += increment;
			}	
		}

		if (buttonPress)
		{
			*x = int16_t(m_sCurrAxisX);
			*y = int16_t(m_sCurrAxisY);
		}
		else
		{			
			m_sCurrAxisX= 0;
			m_sCurrAxisY = 0;
		
			m_sPrevDirection = 0;
			m_sCurrDirection = 0;
			m_bInitialButtonPress = true;
			*x = int16_t(m_sCurrAxisX);
			*y = int16_t(m_sCurrAxisY);
		}
}