#include "mungal4.h"
#pragma hdrstop

#include "l4padrio.h"

#include <XInput.h>
#pragma comment(lib, "xinput9_1_0.lib")

//########################################################################
// Binding tables (vRIO default profile, condensed)
//########################################################################

namespace
{
	struct KeyBinding
	{
		int	virtualKey;
		int	rioUnit;
	};

	const KeyBinding keyboardButtonMap[] =
	{
		{ VK_SPACE,	0x40 },	// ButtonJoystickTrigger
		{ 'R',		0x3F },	// ButtonThrottle1 (reverse thrust)
		{ VK_UP,	0x42 },	// ButtonJoystickHatUp
		{ VK_DOWN,	0x41 },	// ButtonJoystickHatDown
		{ VK_RIGHT,	0x43 },	// ButtonJoystickHatRight
		{ VK_LEFT,	0x44 },	// ButtonJoystickHatLeft
		{ VK_F1,	0x37 },	// ButtonAuxUpperRight1 (config)
		{ VK_F2,	0x36 },	// ButtonAuxUpperRight2 (config)
	};

	struct PadBinding
	{
		unsigned short	padMask;
		int				rioUnit;
	};

	const PadBinding padButtonMap[] =
	{
		{ XINPUT_GAMEPAD_A,					0x40 },	// trigger
		{ XINPUT_GAMEPAD_B,					0x3F },	// reverse thrust
		{ XINPUT_GAMEPAD_X,					0x45 },	// pinky
		{ XINPUT_GAMEPAD_Y,					0x46 },	// thumb low
		{ XINPUT_GAMEPAD_LEFT_SHOULDER,		0x46 },	// thumb low
		{ XINPUT_GAMEPAD_RIGHT_SHOULDER,	0x47 },	// thumb high
		{ XINPUT_GAMEPAD_DPAD_UP,			0x42 },
		{ XINPUT_GAMEPAD_DPAD_DOWN,			0x41 },
		{ XINPUT_GAMEPAD_DPAD_RIGHT,		0x43 },
		{ XINPUT_GAMEPAD_DPAD_LEFT,			0x44 },
		{ XINPUT_GAMEPAD_START,				0x37 },	// config 1
		{ XINPUT_GAMEPAD_BACK,				0x36 },	// config 2
	};

	// Full throttle sweep takes ~1.3 s at full stick deflection.
	const Scalar throttleRatePerSecond = 0.75f;

	Scalar StickValue(int raw, int dead_zone)
	{
		if (raw > -dead_zone && raw < dead_zone)
		{
			return (Scalar) 0;
		}
		Scalar value =
			(raw > 0)
				? (Scalar)(raw - dead_zone) / (Scalar)(32767 - dead_zone)
				: (Scalar)(raw + dead_zone) / (Scalar)(32768 - dead_zone);
		if (value > 1.0f)	value = 1.0f;
		if (value < -1.0f)	value = -1.0f;
		return value;
	}

	Scalar Clamp01(Scalar value)
	{
		if (value < 0.0f)	return 0.0f;
		if (value > 1.0f)	return 1.0f;
		return value;
	}

	Logical KeyDown(int virtual_key)
	{
		return (GetAsyncKeyState(virtual_key) & 0x8000) != 0;
	}
}

//########################################################################
//############################### PadRIO #################################
//########################################################################

PadRIO *PadRIO::activeInstance = NULL;

void
	PadRIO::SetScreenButton(int unit, Logical pressed)
{
	if (activeInstance != NULL && unit >= 0 && unit < buttonUnits)
	{
		activeInstance->screenButton[unit] = pressed ? 1 : 0;
	}
}

int
	PadRIO::GetLampState(int unit)
{
	if (activeInstance != NULL && unit >= 0 && unit < lampCount)
	{
		return activeInstance->lampState[unit];
	}
	return 0;
}

PadRIO::PadRIO()
{
	Check_Pointer(this);

	queueHead = 0;
	queueTail = 0;
	lastPollTick = GetTickCount();
	lastPadCheckTick = 0;
	padIndex = -1;
	padReported = False;
	analogRequested = False;
	throttleAccum = (Scalar) 0;

	sentThrottle = sentLeftPedal = sentRightPedal = (Scalar) 0;
	sentJoystickX = sentJoystickY = (Scalar) 0;

	memset(buttonDown, 0, sizeof(buttonDown));
	memset(lampState, 0, sizeof(lampState));
	memset(screenButton, 0, sizeof(screenButton));

	invertX = False;
	invertY = False;
	const char *flip = getenv("L4PADFLIP");
	if (flip != NULL)
	{
		if (strchr(flip, 'X') || strchr(flip, 'x'))
		{
			invertX = True;
		}
		if (strchr(flip, 'Y') || strchr(flip, 'y'))
		{
			invertY = True;
		}
	}

	// Report as a v4.2 board, like vRIO does
	MajorRevision = 4;
	MinorRevision = 2;

	activeInstance = this;

	DEBUG_STREAM << "PadRIO: virtual RIO active (XInput pad + keyboard)\n" << std::flush;
}

PadRIO::~PadRIO()
{
	Check_Pointer(this);
	if (activeInstance == this)
	{
		activeInstance = NULL;
	}
}

Logical
	PadRIO::TestInstance() const
{
	return True;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// The controls manager drains events every frame; sampling lives here so
// button latency does not depend on the analog request cadence (which is
// 15 s outside of missions).
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Logical
	PadRIO::GetNextEvent(RIOEvent *destinationPointer)
{
	Check_Pointer(this);
	Check_Pointer(destinationPointer);

	PollInputs();

	if (queueTail == queueHead)
	{
		return False;
	}
	*destinationPointer = eventQueue[queueTail];
	queueTail = (queueTail + 1) % queueSize;
	return True;
}

void
	PadRIO::RequestAnalogUpdate()
{
	Check_Pointer(this);
	analogRequested = True;
}

void
	PadRIO::GeneralReset()
{
	Check_Pointer(this);
	throttleAccum = (Scalar) 0;
	Throttle   = (Scalar) 0;
	LeftPedal  = (Scalar) 0;
	RightPedal = (Scalar) 0;
	JoystickX  = (Scalar) 0;
	JoystickY  = (Scalar) 0;
	analogRequested = True;
	memset(lampState, 0, sizeof(lampState));
}

void
	PadRIO::ResetThrottle()
{
	Check_Pointer(this);
	throttleAccum = (Scalar) 0;
	Throttle = (Scalar) 0;
	analogRequested = True;
}

void
	PadRIO::SetLamp(int lampNumber, int state)
{
	Check_Pointer(this);
	if (lampNumber >= 0 && lampNumber < lampCount)
	{
		lampState[lampNumber] = (unsigned char) state;
	}
}

void
	PadRIO::QueueEvent(const RIOEvent &an_event)
{
	int next = (queueHead + 1) % queueSize;
	if (next == queueTail)
	{
		// full: drop the oldest event
		queueTail = (queueTail + 1) % queueSize;
	}
	eventQueue[queueHead] = an_event;
	queueHead = next;
}

void
	PadRIO::PollInputs()
{
	unsigned long now = GetTickCount();
	if (now - lastPollTick < 10)
	{
		return;
	}
	Scalar delta_t = (Scalar)(now - lastPollTick) / 1000.0f;
	if (delta_t > 0.25f)
	{
		delta_t = 0.25f;
	}
	lastPollTick = now;

	//---------------------------------------------------------------
	// Find / keep the XInput pad. Probing empty slots is slow, so an
	// absent pad is only re-probed every 3 seconds.
	//---------------------------------------------------------------
	XINPUT_STATE pad;
	memset(&pad, 0, sizeof(pad));
	Logical pad_live = False;

	if (padIndex >= 0)
	{
		pad_live = (XInputGetState((DWORD) padIndex, &pad) == ERROR_SUCCESS);
		if (!pad_live)
		{
			DEBUG_STREAM << "PadRIO: controller " << padIndex << " disconnected\n" << std::flush;
			padIndex = -1;
		}
	}
	if (padIndex < 0 && (now - lastPadCheckTick) >= 3000)
	{
		lastPadCheckTick = now;
		for (DWORD i = 0; i < 4; ++i)
		{
			if (XInputGetState(i, &pad) == ERROR_SUCCESS)
			{
				padIndex = (int) i;
				pad_live = True;
				DEBUG_STREAM << "PadRIO: controller " << padIndex << " connected\n" << std::flush;
				break;
			}
		}
		if (padIndex < 0 && !padReported)
		{
			padReported = True;
			DEBUG_STREAM << "PadRIO: no controller found - keyboard only\n" << std::flush;
		}
	}

	//---------------------------------------------------------------
	// Buttons: build the desired state across pad + keyboard, then
	// diff against what we last reported.
	//---------------------------------------------------------------
	unsigned char desired[buttonUnits];
	memset(desired, 0, sizeof(desired));

	if (pad_live)
	{
		for (int i = 0; i < (int)(sizeof(padButtonMap)/sizeof(padButtonMap[0])); ++i)
		{
			if (pad.Gamepad.wButtons & padButtonMap[i].padMask)
			{
				desired[padButtonMap[i].rioUnit] = 1;
			}
		}
	}
	for (int i = 0; i < (int)(sizeof(keyboardButtonMap)/sizeof(keyboardButtonMap[0])); ++i)
	{
		if (KeyDown(keyboardButtonMap[i].virtualKey))
		{
			desired[keyboardButtonMap[i].rioUnit] = 1;
		}
	}
	for (int i = 0; i < buttonUnits; ++i)
	{
		if (screenButton[i])
		{
			desired[i] = 1;
		}
	}

	for (int unit = 0; unit < buttonUnits; ++unit)
	{
		if (desired[unit] != buttonDown[unit])
		{
			buttonDown[unit] = desired[unit];

			RIOEvent an_event;
			an_event.Type = desired[unit] ? ButtonPressedEvent : ButtonReleasedEvent;
			an_event.Data.Unit = unit;
			QueueEvent(an_event);
		}
	}

	//---------------------------------------------------------------
	// Axes
	//---------------------------------------------------------------
	Scalar x = (Scalar) 0, y = (Scalar) 0;
	Scalar left_pedal = (Scalar) 0, right_pedal = (Scalar) 0;
	Scalar throttle_rate = (Scalar) 0;

	if (pad_live)
	{
		x += StickValue(pad.Gamepad.sThumbLX, XINPUT_GAMEPAD_LEFT_THUMB_DEADZONE);
		y += StickValue(pad.Gamepad.sThumbLY, XINPUT_GAMEPAD_LEFT_THUMB_DEADZONE);
		throttle_rate += StickValue(pad.Gamepad.sThumbRY, XINPUT_GAMEPAD_RIGHT_THUMB_DEADZONE);

		if (pad.Gamepad.bLeftTrigger > XINPUT_GAMEPAD_TRIGGER_THRESHOLD)
		{
			left_pedal += (Scalar)(pad.Gamepad.bLeftTrigger) / 255.0f;
		}
		if (pad.Gamepad.bRightTrigger > XINPUT_GAMEPAD_TRIGGER_THRESHOLD)
		{
			right_pedal += (Scalar)(pad.Gamepad.bRightTrigger) / 255.0f;
		}
	}

	// keyboard: WASD stick deflect, Q/E pedals, PgUp/PgDn throttle
	if (KeyDown('A'))		x -= 1.0f;
	if (KeyDown('D'))		x += 1.0f;
	if (KeyDown('W'))		y += 1.0f;
	if (KeyDown('S'))		y -= 1.0f;
	if (KeyDown('Q'))		left_pedal += 1.0f;
	if (KeyDown('E'))		right_pedal += 1.0f;
	if (KeyDown(VK_PRIOR))	throttle_rate += 1.0f;	// PgUp
	if (KeyDown(VK_NEXT))	throttle_rate -= 1.0f;	// PgDn

	if (x > 1.0f)	x = 1.0f;
	if (x < -1.0f)	x = -1.0f;
	if (y > 1.0f)	y = 1.0f;
	if (y < -1.0f)	y = -1.0f;

	throttleAccum = Clamp01(throttleAccum + throttle_rate * throttleRatePerSecond * delta_t);

	Throttle   = throttleAccum;
	LeftPedal  = Clamp01(left_pedal);
	RightPedal = Clamp01(right_pedal);
	// The pod's stick convention is opposite the XInput axes on both X and
	// Y (confirmed in playtest), so the default is negated; L4PADFLIP
	// flips back per axis.
	JoystickX  = invertX ? x : -x;
	JoystickY  = invertY ? y : -y;

	//---------------------------------------------------------------
	// Emit an analog event when asked to, or when anything moved
	//---------------------------------------------------------------
	Logical changed =
		(Throttle   != sentThrottle)   ||
		(LeftPedal  != sentLeftPedal)  ||
		(RightPedal != sentRightPedal) ||
		(JoystickX  != sentJoystickX)  ||
		(JoystickY  != sentJoystickY);

	if (analogRequested || changed)
	{
		analogRequested = False;
		sentThrottle   = Throttle;
		sentLeftPedal  = LeftPedal;
		sentRightPedal = RightPedal;
		sentJoystickX  = JoystickX;
		sentJoystickY  = JoystickY;

		RIOEvent an_event;
		an_event.Type = AnalogEvent;
		an_event.Data.Unit = 0;
		QueueEvent(an_event);
	}
}