//////////////////////////////////////////////////////////////////////////////
//
// Asura_PS2_ControlPortDevice.cpp
//
// Created by Neil Massey.
//
// Copyright (c) 2002 Rebellion. All rights reserved.
//
// Implementation of control port device
//
//////////////////////////////////////////////////////////////////////////////

#include "Core/Asura.h"

// This file's header.
#include "Input/Asura_PS2_ControlPortDevice.h"

// Engine independent.
#include "Camera/Asura_Camera.h"
#include "Core/Asura_Maths.h"
#include "Core/Asura_PlatformBasics.h"
#include "Core/Asura_Timers.h"
#include "Debugging/Asura_Assert.h"
#include "Input/Asura_PlatformInput.h"
#include "Rendering/Asura_Viewport.h"

#include <libpad.h>

// Locations of data in the buffer returned from scePadRead
enum DualshockBufferLocations {DS_COM_CHECK = 0,
							   DS_PAD_ID = 1,
							   DS_DIGITAL_BUTTON_HI = 2,
							   DS_DIGITAL_BUTTON_LO = 3,
							   DS_ANALOG_RSTICK_H = 4,
							   DS_ANALOG_RSTICK_V = 5,
							   DS_ANALOG_LSTICK_H = 6,
							   DS_ANALOG_LSTICK_V = 7,
							   DS_PRESSURE_LEFT = 8,
							   DS_PRESSURE_RIGHT = 9,
							   DS_PRESSURE_UP = 10,
							   DS_PRESSURE_DOWN = 11,
							   DS_PRESSURE_TRIANGLE = 12,
							   DS_PRESSURE_CIRCLE = 13,
							   DS_PRESSURE_CROSS = 14,
							   DS_PRESSURE_SQUARE = 15,
							   DS_PRESSURE_L1 = 16,
							   DS_PRESSURE_R1 = 17,
							   DS_PRESSURE_L2 = 18,
							   DS_PRESSURE_R2 = 19};

// Bitwise locations of data in the DS_DIGITAL_BUTTON_HI and _LO in the buffer returned from scePadRead
enum DigitalPadMasksHi {DP_LEFT_MASK = 1<<7,
						DP_DOWN_MASK = 1<<6,
						DP_RIGHT_MASK = 1<<5,
						DP_UP_MASK = 1<<4,
						DP_START_MASK = 1<<3,
						DP_R3_MASK = 1<<2,
						DP_L3_MASK = 1<<1,
						DP_SELECT_MASK = 1};

enum DigitalPadMasksLo {DP_SQUARE_MASK = 1<<7,
						DP_CROSS_MASK  = 1<<6,
						DP_CIRCLE_MASK = 1<<5,
						DP_TRIANGLE_MASK = 1<<4,
						DP_R1_MASK = 1<<3,
						DP_L1_MASK = 1<<2,
						DP_R2_MASK = 1<<1,
						DP_L2_MASK = 1};

enum GunConBufferLocations {GC_COM_CHECK = 0,
							GC_PAD_ID = 1,
							GC_DIGITAL_BUTTON_HI = 2,
							GC_DIGITAL_BUTTON_LO = 3,
							GC_POSITION_X_LO = 4,
							GC_POSITION_X_HI = 5,
							GC_POSITION_Y_LO = 6,
							GC_POSITION_Y_HI = 7};

enum GunConMasksHi {GC_LEFT_MASK  = 1<<7,
					GC_DOWN_MASK  = 1<<6,
					GC_RIGHT_MASK = 1<<5,
					GC_UP_MASK    = 1<<4,
					GC_A_MASK	  = 1<<3};

enum GunConMasksLo {GC_B_MASK   = 1<<6,
				    GC_TRG_MASK = 1<<5};

enum NegConBufferLocations {NC_COM_CHECK = 0,
							NC_PAD_ID = 1,
							NC_DIGITAL_BUTTON_HI = 2,
							NC_DIGITAL_BUTTON_LO = 3,
							NC_TWIST = 4,
							NC_BUTTON_I = 5,
							NC_BUTTON_II = 6,
							NC_BUTTON_L = 7};

enum NegConMasksHi {NC_LEFT_MASK = 1<<7,
					NC_DOWN_MASK = 1<<6,
					NC_RIGHT_MASK = 1<<5,
					NC_UP_MASK = 1<<4,
					NC_START_MASK = 1<<3};

enum NegConMasksLo {NC_A_MASK = 1<<5,
					NC_B_MASK = 1<<4,
					NC_R_MASK = 1<<3};

// State machine for dualshock II pads
enum ControlPhase {CP_DISCONNECTED=0, 
				   CP_BUSY, 
				   CP_GET_ID,
				   CP_GET_EX_ID, 
				   CP_SET_UP_ACTUATORS, 
				   CP_SET_PRESSURE_SENSITIVE,
				   CP_FORCE_ANALOG,
#if ASURA_PS2_SUPPORT_MULTITAP
				   CP_CHECK_MULTITAP,
#endif
				   CP_READY};

// Dead zone location
DEBUGVAR float dbg_fAnalogStickDeadZone = 32.0f;

u_long128* Asura_PS2_ControlPortDevice::s_uPadDmaBuffer[8];

Asura_PS2_ControlPortDevice::Asura_PS2_ControlPortDevice(int iPort, int iSlot, bool bEnablePressure)
{
	m_bAvail = false;
	m_iPortNumber = iPort;
	m_iSlotNumber = iSlot;
	m_iPhase = m_iNextPhase = CP_DISCONNECTED;
	m_iCtrlId = m_iCtrlOldId = 0;
	m_bEnablePressure = bEnablePressure;

	// Allocate aligned data for pad
	m_iNActuators = 0;
	// Acuator alignment data
	m_ucActAlign[0] = 0x00;
	m_ucActAlign[1] = 0x01;
	for(int i=2; i<6; ++i)
	{
		m_ucActAlign[i] = 0xFF;
	}

	// Actuator drive data
	for(int i=0; i<6; ++i)
	{
		m_ucActDirect[i] = 0x00;
	}

	for(int i=0; i<3; ++i)
	{
		m_fCalibrationData[0][i] = m_fCalibrationData[1][i] = 0.f;
	}
	m_bReadyToTranslate = false;
}

Asura_PS2_ControlPortDevice::~Asura_PS2_ControlPortDevice(void)
{
	scePadPortClose(m_iPortNumber, m_iSlotNumber);
	Asura_DeallocateAlign(s_uPadDmaBuffer[m_iPortNumber * 4 + m_iSlotNumber]);
	s_uPadDmaBuffer[m_iPortNumber * 4 + m_iSlotNumber] = NULL;
}

int	 Asura_PS2_ControlPortDevice::GetId(void)
{
	return m_iCtrlId;
}

u_long128* Asura_PS2_ControlPortDevice::GetMemoryOfBuffer(int iPort, int iSlot)
{
	s_uPadDmaBuffer[iPort * 4 + iSlot] = static_cast<u_long128*>(Asura_AllocateAlign(scePadDmaBufferMax * sizeof(u_long128), ASURA_ALIGN_64_BYTE));
	return s_uPadDmaBuffer[iPort * 4 + iSlot];
}

void Asura_PS2_ControlPortDevice::Update(void)
{
	// get the state of the pad from the sony libraries
	int iCurrentState = scePadGetState(m_iPortNumber, m_iSlotNumber);

	// Check for disconnection
	if (iCurrentState == scePadStateDiscon || iCurrentState == scePadStateError)
		m_iPhase = m_iNextPhase = CP_DISCONNECTED;

	// Get the id of the pad - this is useful for when there is a mode switch
	m_iCtrlId = scePadInfoMode(m_iPortNumber, m_iSlotNumber, InfoModeCurID, 0);

	switch (m_iPhase)
	{
		case CP_DISCONNECTED:
			m_bAvail = false;
			m_bReadyToTranslate = false;
			// Wait until a pad is connected
			if (iCurrentState == scePadStateStable || iCurrentState == scePadStateFindCTP1)
			{
				m_iPhase = CP_BUSY;
				m_bAvail = true;
#if ASURA_PS2_SUPPORT_MULTITAP
				m_iNextPhase = CP_CHECK_MULTITAP;
#else
				m_iNextPhase = CP_GET_ID;
#endif
			}
		break;

		case CP_BUSY:
			// Wait until pad is ready then go to next phase
			if (iCurrentState != scePadStateExecCmd)
				m_iPhase = m_iNextPhase;
		break;

#if ASURA_PS2_SUPPORT_MULTITAP
		case CP_CHECK_MULTITAP:
			if (m_iSlotNumber == 0 &&
				m_iPortNumber < ASURA_PS2_MAX_MULTITAPS)
			{
				Asura_PS2_Input::CheckForMultitap(m_iPortNumber);
				m_iPhase = CP_BUSY;
				m_iNextPhase = CP_GET_ID;
			}
			else
			{
				m_iPhase = CP_GET_ID;
			}
		break;
#endif

		case CP_GET_ID:
			if (m_iCtrlId != 0)
			{
				// Get the extended id for Dualshock II etc.
				m_iPhase = CP_BUSY;
				m_iNextPhase = CP_GET_EX_ID;
			}
			else
				m_iPhase = m_iNextPhase = CP_DISCONNECTED;

			// If the device is an Analog Joystick or NegCon and the maximum throw / twist is 0.0f then set it to 1.0f
			if (m_iCtrlId == DT_NEGCON && m_fCalibrationData[0][1] == 0.f)
				m_fCalibrationData[0][1] = 1.f;
			else if (m_iCtrlId == DT_ANALOG_JSTICK)
			{
				if(m_fCalibrationData[0][2] == 0.f)
					m_fCalibrationData[0][2] = 1.f;
				if(m_fCalibrationData[1][2] == 0.f)
					m_fCalibrationData[1][2] = 1.f;
			}
		break;

		case CP_GET_EX_ID:
		{
			int iExtCtrlId = scePadInfoMode(m_iPortNumber, m_iSlotNumber, InfoModeCurExID, 0);
			if (iExtCtrlId>0)
			{
				m_iCtrlId = iExtCtrlId;
			}
			m_iPhase = CP_BUSY;
			m_iNextPhase = CP_FORCE_ANALOG;
		}
		break; 

		case CP_FORCE_ANALOG:
            // Got control ID - check against Konami Gun as we don't support this
            if(m_iCtrlId == 0x03)
            {
                m_iPhase = CP_BUSY;
                m_iNextPhase = CP_DISCONNECTED;
                break;
            }
			scePadSetMainMode(m_iPortNumber, m_iSlotNumber, 1, 3);
			m_iPhase = CP_BUSY;
			m_iNextPhase = CP_SET_UP_ACTUATORS;
		break;

		case CP_SET_UP_ACTUATORS:
			// Check for compatibility first
			m_iNActuators = scePadInfoAct(m_iPortNumber, m_iSlotNumber, -1, 0); // gets number of actuators
			if (m_iNActuators > 0)
			{
				int iReturn = scePadSetActAlign(m_iPortNumber, m_iSlotNumber, m_ucActAlign);
				Asura_Assert(iReturn == 1, "Actuator alignment not completed");
			}
			m_iPhase = CP_BUSY;
			m_iNextPhase = CP_SET_PRESSURE_SENSITIVE;
		break;

		case CP_SET_PRESSURE_SENSITIVE:
			// Only valid if scePadInfoPressMode returns 1
			if(m_bEnablePressure && scePadInfoPressMode(m_iPortNumber, m_iSlotNumber) == 1)
			{
				scePadEnterPressMode(m_iPortNumber, m_iSlotNumber);
			}
			m_iPhase = CP_BUSY;
			m_iNextPhase = CP_READY;
		break;

		case CP_READY:
			if(m_iCtrlId != m_iCtrlOldId)
			{
				m_iCtrlOldId = m_iCtrlId;
				m_iPhase = m_iNextPhase = CP_DISCONNECTED;
			}
			else
			{
				m_bReadyToTranslate = true;

				// Read the data
			    int iDataLen = scePadRead(m_iPortNumber, m_iSlotNumber, acDataBuffer);
				if (iDataLen == 0)
				{ 
					m_iPhase = CP_DISCONNECTED;
					return; 
				}
                ProcessRumble();
			}
		break;
	}
}

void Asura_PS2_ControlPortDevice::Translate(u_char *piAsuraInputBufferOffset, float *piAsuraAxisBufferOffset)
{
	if(!m_bReadyToTranslate)
	{
		return;
	}
	// Translate into the buffers passed in, depending on the controller plugged in
	switch (m_iCtrlId)
	{
		case DT_NEGCON:
			TranslateNegCon(piAsuraInputBufferOffset, piAsuraAxisBufferOffset);
		break;

		case DT_DIGITAL:
			TranslateDigital(piAsuraInputBufferOffset, piAsuraAxisBufferOffset);
		break;

		case DT_ANALOG_JSTICK:
			TranslateAnalogJoyStick(piAsuraInputBufferOffset, piAsuraAxisBufferOffset);
		break;

		case DT_GUNCON:
			TranslateGunCon(piAsuraInputBufferOffset, piAsuraAxisBufferOffset);
		break;

		case DT_DUALSHOCK:
			// Also handle dualshock pressure here
			TranslateDualshock(piAsuraInputBufferOffset, piAsuraAxisBufferOffset);
		break;
	}
}

bool Asura_PS2_ControlPortDevice::Available(void)
{
	// SNH: on Rogue we were getting spurious 'controller disconnected' messages at startup - 
	//	this appears to be because the controller ID is incorrect until it reaches the 'ready to translate' state.
	//	Reporting it as available until we're sure it's not supported fixes this.
	if(m_bAvail)
	{
		if(!m_bReadyToTranslate || IsControllerSupported())
		{
			return true;
		}
	}
	return false;
}

void Asura_PS2_ControlPortDevice::TranslateNegCon(u_char *piAsuraInputBufferOffset, float *piAsuraAxisBufferOffset)
{
	// Note - this device requires calibration values to be set
	// Translate the digital buttons into Asura
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_NEGCON1_A)		= 255 * ((acDataBuffer[NC_DIGITAL_BUTTON_LO] & NC_A_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_NEGCON1_B)		= 255 * ((acDataBuffer[NC_DIGITAL_BUTTON_LO] & NC_B_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_NEGCON1_UP)	= 255 * ((acDataBuffer[NC_DIGITAL_BUTTON_HI] & NC_UP_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_NEGCON1_DOWN)	= 255 * ((acDataBuffer[NC_DIGITAL_BUTTON_HI] & NC_DOWN_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_NEGCON1_LEFT)	= 255 * ((acDataBuffer[NC_DIGITAL_BUTTON_HI] & NC_LEFT_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_NEGCON1_RIGHT) = 255 * ((acDataBuffer[NC_DIGITAL_BUTTON_HI] & NC_RIGHT_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_NEGCON1_START) = 255 * ((acDataBuffer[NC_DIGITAL_BUTTON_HI] & NC_START_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_NEGCON1_R)		= 255 * ((acDataBuffer[NC_DIGITAL_BUTTON_LO] & NC_R_MASK) == 0);

	// Translate the analog buttons into Asura
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_NEGCON1_I)		= acDataBuffer[NC_BUTTON_I];
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_NEGCON1_II)	= acDataBuffer[NC_BUTTON_II];
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_NEGCON1_L)		= acDataBuffer[NC_BUTTON_L];

	// Translate the twist into Asura_Axis - use calibration values for centre and max twist
	float fTwist = ConvertPS2AxisToAsuraAxis(acDataBuffer[NC_TWIST], 0.0f);

	// Adjust centre point calibration
	fTwist -= m_fCalibrationData[0][0];							// [0][0] is centre offset

	Asura_Assert(m_fCalibrationData[0][1] != 0.0f, "Max twist for NegCon is 0.0f");
	float fTempTwist = fTwist / m_fCalibrationData[0][1];		// [0][1] is maximum twist
	fTwist = fTempTwist < 0.0f ? Asura_Maths::Max(-1.0f, fTempTwist) : Asura_Maths::Min(1.0f, fTempTwist);

	*(piAsuraAxisBufferOffset + ASURA_AXIS_PAD1_LSTICK_H) = fTwist;
}

void Asura_PS2_ControlPortDevice::TranslateGunCon(u_char *piAsuraInputBufferOffset, float *piAsuraAxisBufferOffset)
{
	// Check that the Guncon is in a valid slot, i.e. slot == 0
	if(m_iSlotNumber != 0)
		return;

	int iScreenW = Asura_Viewport::GetWidth();
	int iScreenH = Asura_Viewport::GetHeight();

	float iGunX, iGunY;

	// The 384 and 256 / 240 values are straight outta the sce gun example
	iGunX = acDataBuffer[GC_POSITION_X_LO] + acDataBuffer[GC_POSITION_X_HI]*256 - 0x40;
	iGunY = acDataBuffer[GC_POSITION_Y_LO] + acDataBuffer[GC_POSITION_Y_HI]*256 - 0x28;

	iGunX = (iGunX * iScreenW) / 384;

#ifdef ASURA_PS2_PAL
	iGunY = (iGunY * iScreenH) / 256;
#else
	iGunY = (iGunY * iScreenH) / 240;	// Neil - Should this be 224 for NTSC screen height?
#endif

	// Normalize to 0.0f .. 1.0f screen co-ords
	float fGunX = static_cast<float>(iGunX) / static_cast<float>(iScreenW);
	float fGunY = static_cast<float>(iGunY) / static_cast<float>(iScreenH);

	// Use calibration data to adjust centre position
	fGunX -= m_fCalibrationData[0][0];			// [0][0] is screen offset X
	fGunY -= m_fCalibrationData[0][1];			// [0][1] is screen offset Y

	*(piAsuraAxisBufferOffset) = fGunX;
	*(piAsuraAxisBufferOffset+1) = fGunY;

	// Write the trigger and action buttons to the asura buffer
	*(piAsuraInputBufferOffset)   = 255 * ((acDataBuffer[GC_DIGITAL_BUTTON_LO] & GC_TRG_MASK) == 0); 
	*(piAsuraInputBufferOffset+1) = 255 * ((acDataBuffer[GC_DIGITAL_BUTTON_HI] & GC_A_MASK) == 0); 
	*(piAsuraInputBufferOffset+2) = 255 * ((acDataBuffer[GC_DIGITAL_BUTTON_LO] & GC_B_MASK) == 0); 


}

void Asura_PS2_ControlPortDevice::TranslateDigital(u_char *piAsuraInputBufferOffset, float *piAsuraAxisBufferOffset)
{
	// Make sure that pointer arithmetic will still be valid
	Asura_Assert(ASURA_PS2_KEYID_PAD1_CROSS + ASURA_PS2_NUM_BUTTONS_IN_PAD == ASURA_PS2_KEYID_PAD2_CROSS, 
				"Number of buttons on pad has changed!");

	// Translate buttons into Asura
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_CROSS)	= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_CROSS_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_CIRCLE)	= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_CROSS_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_SQUARE)	= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_SQUARE_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_TRIANGLE)	= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_TRIANGLE_MASK) == 0);

	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_R2)		= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_R2_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_L2)		= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_L2_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_START)	= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_HI] & DP_START_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_SELECT)	= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_HI] & DP_SELECT_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_L1)		= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_L1_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_R1)		= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_R1_MASK) == 0);

	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_UP)		= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_HI] & DP_UP_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_DOWN)		= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_HI] & DP_DOWN_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_LEFT)		= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_HI] & DP_LEFT_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_RIGHT)	= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_HI] & DP_RIGHT_MASK) == 0);

	// Translate Stick Axis into Asura - all zero for a digital pad
	*(piAsuraAxisBufferOffset + ASURA_AXIS_PAD1_LSTICK_H)		= 0.0f;
	*(piAsuraAxisBufferOffset + ASURA_AXIS_PAD1_LSTICK_V)		= 0.0f;
	*(piAsuraAxisBufferOffset + ASURA_AXIS_PAD1_RSTICK_H)		= 0.0f;
	*(piAsuraAxisBufferOffset + ASURA_AXIS_PAD1_RSTICK_V)		= 0.0f;
}

void Asura_PS2_ControlPortDevice::TranslateAnalogJoyStick(u_char *piAsuraInputBufferOffset, float *piAsuraAxisBufferOffset)
{
	// Note - this device requires calibration values to be set

	// Buttons are same as Dualshock but without pressure sensitivity - i.e. a Dualshock I / Digital
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_CROSS)	= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_CROSS_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_CIRCLE)	= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_CROSS_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_SQUARE)	= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_SQUARE_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_TRIANGLE)	= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_TRIANGLE_MASK) == 0);

	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_R2)		= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_R2_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_L2)		= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_L2_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_START)	= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_HI] & DP_START_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_SELECT)	= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_HI] & DP_SELECT_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_L1)		= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_L1_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_R1)		= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_R1_MASK) == 0);

	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_UP)		= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_HI] & DP_UP_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_DOWN)		= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_HI] & DP_DOWN_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_LEFT)		= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_HI] & DP_LEFT_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_RIGHT)	= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_HI] & DP_RIGHT_MASK) == 0);

	// Translate axis info like the Dualshock but calibrate using 
	// a centring position offset and a maximum throw for each stick
	float fLeftStickX  = ConvertPS2AxisToAsuraAxis(acDataBuffer[DS_ANALOG_LSTICK_H], 0.0f);
	float fLeftStickY  = ConvertPS2AxisToAsuraAxis(acDataBuffer[DS_ANALOG_LSTICK_V], 0.0f);
	float fRightStickX = ConvertPS2AxisToAsuraAxis(acDataBuffer[DS_ANALOG_RSTICK_H], 0.0f);
	float fRightStickY = ConvertPS2AxisToAsuraAxis(acDataBuffer[DS_ANALOG_RSTICK_V], 0.0f);

	// Adjust centre values using calibration info
	fLeftStickX -= m_fCalibrationData[0][0];					// [0][0] is centre X on left stick
	fLeftStickY -= m_fCalibrationData[0][1];					// [0][1] is centre Y on left stick

	fRightStickX -= m_fCalibrationData[1][0];					// [1][0] is centre X on right stick
	fRightStickY -= m_fCalibrationData[1][1];					// [1][1] is centre Y on right stick

	// Div zero check
	Asura_Assert(m_fCalibrationData[0][2] != 0.0f, "Max throw for Left Analog JoyStick is 0.0f");
	Asura_Assert(m_fCalibrationData[1][2] != 0.0f, "Max throw for Right Analog JoyStick is 0.0f");

	// Adjust maximum throw using calibration info
	float fTempX = fLeftStickX / m_fCalibrationData[0][2];		// [0][2] is maximum throw on left stick
	fLeftStickX = fTempX < 0.0f ? Asura_Maths::Max(-1.0f, fTempX) : Asura_Maths::Min(1.0f, fTempX);

	float fTempY = fLeftStickY / m_fCalibrationData[0][2];
	fLeftStickY = fTempY < 0.0f ? Asura_Maths::Max(-1.0f, fTempX) : Asura_Maths::Min(1.0f, fTempY);

	fTempX = fRightStickX / m_fCalibrationData[1][2];			// [1][2] is maximum throw on right stick
	fRightStickX = fTempX < 0.0f ? Asura_Maths::Max(-1.0f, fTempX) : Asura_Maths::Min(1.0f, fTempX);

	fTempY = fRightStickY / m_fCalibrationData[1][2];
	fRightStickY = fTempY < 0.0f ? Asura_Maths::Max(-1.0f, fTempY) : Asura_Maths::Min(1.0f, fTempY);

	// Load these values into the Asura_Axis buffer
	*(piAsuraAxisBufferOffset + ASURA_AXIS_PAD1_LSTICK_H) = fLeftStickX;
	*(piAsuraAxisBufferOffset + ASURA_AXIS_PAD1_LSTICK_V) = fLeftStickY;
	*(piAsuraAxisBufferOffset + ASURA_AXIS_PAD1_RSTICK_H) = fRightStickX;
	*(piAsuraAxisBufferOffset + ASURA_AXIS_PAD1_RSTICK_V) = fRightStickY;
}

float Asura_PS2_ControlPortDevice::ConvertPS2AxisToAsuraAxis(u_char ucPS2Axis, float fDeadZone)
{
	// Take into acount deadzones etc.
	float fA = static_cast<float>(ucPS2Axis) - 127.f;

	if(fA > fDeadZone)
		fA = (fA - fDeadZone) * (128.0f / (128.0f - fDeadZone));
	else if(fA < - fDeadZone)
		fA = (fA + fDeadZone) * (127.0f / (127.0f - fDeadZone));
	else
        return 0.f;
	
	// Normalise
	fA *= fA < 0.0f ? 1.0f / 127.0f : 1.0f / 128.0f;
	return fA;		// now in range -1.f to 1.f
}

void Asura_PS2_ControlPortDevice::TranslateDualshock(u_char *piAsuraInputBufferOffset, float *piAsuraAxisBufferOffset)
{
	// Translate buttons into Asura, first check for pressure sensitivity
	// translate digital in if no pressure sensitive data
	// otherwise translate pressure sensitive data in

	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_CROSS)	= acDataBuffer[DS_PRESSURE_CROSS] == 0 ?
																  255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_CROSS_MASK) == 0) :
																  acDataBuffer[DS_PRESSURE_CROSS];

	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_CIRCLE)	= acDataBuffer[DS_PRESSURE_CIRCLE] == 0 ?
																  255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_CIRCLE_MASK) == 0) :
																  acDataBuffer[DS_PRESSURE_CIRCLE];

	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_SQUARE)	= acDataBuffer[DS_PRESSURE_SQUARE] == 0 ?
																  255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_SQUARE_MASK) == 0) :
																  acDataBuffer[DS_PRESSURE_SQUARE];

	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_TRIANGLE)	= acDataBuffer[DS_PRESSURE_TRIANGLE] == 0 ?
																  255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_TRIANGLE_MASK) == 0) :
																  acDataBuffer[DS_PRESSURE_TRIANGLE];

	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_R2)		= acDataBuffer[DS_PRESSURE_R2] == 0 ?
																  255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_R2_MASK) == 0) :
																  acDataBuffer[DS_PRESSURE_R2];

	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_L2)		= acDataBuffer[DS_PRESSURE_L2] == 0 ?
																  255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_L2_MASK) == 0) :
																  acDataBuffer[DS_PRESSURE_L2];

	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_START)	= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_HI] & DP_START_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_SELECT)	= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_HI] & DP_SELECT_MASK) == 0);;

	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_L1)		= acDataBuffer[DS_PRESSURE_L1] == 0 ?
																  255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_L1_MASK) == 0) :
																  acDataBuffer[DS_PRESSURE_L1];

	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_R1)		= acDataBuffer[DS_PRESSURE_R1] == 0 ?
																  255 * ((acDataBuffer[DS_DIGITAL_BUTTON_LO] & DP_R1_MASK) == 0) :
																  acDataBuffer[DS_PRESSURE_R1];

	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_L3)		= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_HI] & DP_L3_MASK) == 0);
	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_R3)		= 255 * ((acDataBuffer[DS_DIGITAL_BUTTON_HI] & DP_R3_MASK) == 0);

	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_UP)		= acDataBuffer[DS_PRESSURE_UP] == 0 ?
																  255 * ((acDataBuffer[DS_DIGITAL_BUTTON_HI] & DP_UP_MASK) == 0) :
																  acDataBuffer[DS_PRESSURE_UP];

	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_DOWN)		= acDataBuffer[DS_PRESSURE_DOWN] == 0 ?
																  255 * ((acDataBuffer[DS_DIGITAL_BUTTON_HI] & DP_DOWN_MASK) == 0) :
																  acDataBuffer[DS_PRESSURE_DOWN];

	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_LEFT)		= acDataBuffer[DS_PRESSURE_LEFT] == 0 ?
																  255 * ((acDataBuffer[DS_DIGITAL_BUTTON_HI] & DP_LEFT_MASK) == 0) :
																  acDataBuffer[DS_PRESSURE_LEFT];

	*(piAsuraInputBufferOffset + ASURA_PS2_KEYID_PAD1_RIGHT)	= acDataBuffer[DS_PRESSURE_RIGHT] == 0 ?
																  255 * ((acDataBuffer[DS_DIGITAL_BUTTON_HI] & DP_RIGHT_MASK) == 0) :
																  acDataBuffer[DS_PRESSURE_RIGHT];

	// Translate Stick Axis into Asura - negate verticals to match Asura
	*(piAsuraAxisBufferOffset + ASURA_AXIS_PAD1_LSTICK_H) = ConvertPS2AxisToAsuraAxis(acDataBuffer[DS_ANALOG_LSTICK_H], dbg_fAnalogStickDeadZone);
	*(piAsuraAxisBufferOffset + ASURA_AXIS_PAD1_LSTICK_V) = -ConvertPS2AxisToAsuraAxis(acDataBuffer[DS_ANALOG_LSTICK_V],dbg_fAnalogStickDeadZone);
	*(piAsuraAxisBufferOffset + ASURA_AXIS_PAD1_RSTICK_H) = ConvertPS2AxisToAsuraAxis(acDataBuffer[DS_ANALOG_RSTICK_H], dbg_fAnalogStickDeadZone);
	*(piAsuraAxisBufferOffset + ASURA_AXIS_PAD1_RSTICK_V) = -ConvertPS2AxisToAsuraAxis(acDataBuffer[DS_ANALOG_RSTICK_V],dbg_fAnalogStickDeadZone);
}

void Asura_PS2_ControlPortDevice::SetCalibration(float fData1, float fData2)
{
	// Just load in the calibration data
	// What the device does with it is determined in the respective translate procedure
	m_fCalibrationData[0][0] = fData1;
	m_fCalibrationData[0][1] = fData2;
}

void Asura_PS2_ControlPortDevice::SetCalibration(int iSide, float fData1, float fData2, float fData3)
{
	// Load in calibration data for a device that has "sides" e.g. Analog Joystick
	m_fCalibrationData[iSide][0] = fData1;
	m_fCalibrationData[iSide][1] = fData2;
	m_fCalibrationData[iSide][2] = fData3;
}

void Asura_PS2_ControlPortDevice::SetRumble(u_char ucAmountSmall, u_char ucAmountLarge)
{
    m_ucActDirect[ASURA_PS2_VM_SMALL_MOTOR] = 0;
    m_ucActDirect[ASURA_PS2_VM_LARGE_MOTOR] = 0;
	switch(m_iCtrlId)
	{
		// Do nothing cases
		case DT_NEGCON:
		case DT_DIGITAL:
		case DT_ANALOG_JSTICK:
		case DT_GUNCON:
		break;

		// Do something cases
		case DT_DUALSHOCK:
		case DT_DUALSHOCK_PRESSURE:
		{
            m_ucActDirect[ASURA_PS2_VM_SMALL_MOTOR] = ucAmountSmall;
            m_ucActDirect[ASURA_PS2_VM_LARGE_MOTOR] = ucAmountLarge;
		}
		break;
	}
}

DEBUGVAR bool dbg_bDoVibration = true;
DEBUGVAR bool dbg_bShowVibration = false;
// See if we exeeded the limit of consumption
void Asura_PS2_ControlPortDevice::ProcessRumble(void)
{
	// Send the vibration
    if(dbg_bShowVibration)
    {
        if(m_ucActDirect[ASURA_PS2_VM_SMALL_MOTOR] || 
            m_ucActDirect[ASURA_PS2_VM_LARGE_MOTOR])
        {
            printf("Frame : %i\n", Asura_Timers::GetFrameNumber());
            printf("  Slot : %i, Small : %i, Large : %i \n", m_iSlotNumber, m_ucActDirect[ASURA_PS2_VM_SMALL_MOTOR],
                                                           m_ucActDirect[ASURA_PS2_VM_LARGE_MOTOR]);
            printf("*********************\n");
        }
    }
    if(dbg_bDoVibration)
    {
		Asura_Assert(Asura_Camera::GetNumberOfCameras() < 3 || m_ucActDirect[ASURA_PS2_VM_LARGE_MOTOR] == 0, "You are likely to run out of power by rumbling the large motors when using more than 2 pads");
        int iReturn = scePadSetActDirect(m_iPortNumber, m_iSlotNumber, m_ucActDirect);	//don't care whether it is succesfull or not
        Asura_Assert(iReturn == 1, "Actuator drive not successfull");
    }
}

bool Asura_PS2_ControlPortDevice::IsControllerSupported()
{
	switch(m_iCtrlId)
	{
		// (AW - 22.12.2003) - Unsupported
		case DT_NEGCON:
		case DT_DIGITAL:
		case DT_ANALOG_JSTICK:
		case DT_GUNCON:
		default:
			return false;

		// (AW - 22.12.2003) - Supported
		case DT_DUALSHOCK:
		case DT_DUALSHOCK_PRESSURE:
			return true;
	}
}
