#include "mungal4.h"
#pragma hdrstop

//###########################################################################
// L4PADRIO -- the hardware-less cockpit device (BT_GLASS only; this TU is
// only in the build when the gate is on -- see CMakeLists.txt).
// Design + input model: L4PADRIO.h.
//###########################################################################

#include "l4padrio.h"
#include "l4ctrl.h"

#include <windows.h>
#include <xinput.h>
#include <stdlib.h>
#include <string.h>

#pragma comment(lib, "xinput9_1_0.lib")

PadRIO *PadRIO::activeInstance = NULL;

//
// XInput normalization: thumbs to -1..1 past the stock deadzone, triggers
// to 0..1 past the stock threshold.
//
static float
	NormalizeThumb(int value, int dead_zone)
{
	float sign = (value < 0) ? -1.0f : 1.0f;
	float magnitude = (float)(value < 0 ? -value : value);
	if (magnitude <= (float)dead_zone)
	{
		return 0.0f;
	}
	if (magnitude > 32767.0f)
	{
		magnitude = 32767.0f;
	}
	return sign * (magnitude - dead_zone) / (32767.0f - dead_zone);
}

static float
	NormalizeTrigger(int value)
{
	if (value <= XINPUT_GAMEPAD_TRIGGER_THRESHOLD)
	{
		return 0.0f;
	}
	return (float)(value - XINPUT_GAMEPAD_TRIGGER_THRESHOLD)
		/ (float)(255 - XINPUT_GAMEPAD_TRIGGER_THRESHOLD);
}

//
// The keyboard is live only while a window of THIS process is foreground
// (the mech4.cpp focus-guard idiom) -- alt-tabbed developers must not
// drive the mech.
//
static int
	ProcessHasFocus()
{
	HWND foreground = GetForegroundWindow();
	if (foreground == NULL)
	{
		return 0;
	}
	DWORD process_id = 0;
	GetWindowThreadProcessId(foreground, &process_id);
	return process_id == GetCurrentProcessId();
}

//###########################################################################
// Construction
//###########################################################################

PadRIO::PadRIO():
	RIOBase(),
	eventHead(0),
	eventTail(0),
	lastPollMilliseconds(0),
	lastPadProbeMilliseconds(0),
	padIndex(-1),
	previousPadButtons(0)
{
	memset(previousKeyHeld, 0, sizeof(previousKeyHeld));
	memset(channelValue, 0, sizeof(channelValue));
	memset(lampState, 0, sizeof(lampState));

	bindings.Load();

	//
	// Per-channel spring return rate = the fastest deflect rate bound to
	// the channel (a channel with no deflect bindings never auto-centers).
	//
	for (int c = 0; c < PadBindingProfile::ChannelCount; ++c)
	{
		channelReturnRate[c] = 0.0f;
	}
	for (int k = 0; k < bindings.keyBindingCount; ++k)
	{
		const PadBindingProfile::Action &action = bindings.keyBindings[k].action;
		if (action.kind == PadBindingProfile::ActionAxisDeflect)
		{
			float rate = action.rate < 0.0f ? -action.rate : action.rate;
			if (rate > channelReturnRate[action.channel])
			{
				channelReturnRate[action.channel] = rate;
			}
		}
	}

	flipStickAxes =
		(getenv("L4PADFLIP") != NULL && *getenv("L4PADFLIP") != '0');

	//
	// Never revision 0.0 -- some diagnostics print it; give the synthetic
	// board a recognizable version.
	//
	MajorRevision = 9;
	MinorRevision = 9;

	activeInstance = this;
	DEBUG_STREAM << "[padrio] PadRIO up (XInput probe pending; keyboard "
		<< "live on focus; L4PADFLIP=" << flipStickAxes << ")\n" << std::flush;
}

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

//###########################################################################
// Event queue
//###########################################################################

void
	PadRIO::PushEvent(const RIOEvent &event)
{
	int next = (eventHead + 1) % EventQueueSize;
	if (next == eventTail)
	{
		DEBUG_STREAM << "[padrio] event queue overflow -- event dropped\n"
			<< std::flush;
		return;
	}
	eventQueue[eventHead] = event;
	eventHead = next;
}

void
	PadRIO::EmitButton(int address, int pressed)
{
	RIOEvent event;
	event.Type = pressed ? ButtonPressedEvent : ButtonReleasedEvent;
	event.Data.Unit = address;
	PushEvent(event);
}

void
	PadRIO::EmitKeypad(int unit, int key)
{
	RIOEvent event;
	event.Type = KeyEvent;
	event.Data.Keyboard.Unit = unit;
	event.Data.Keyboard.Key = key;
	PushEvent(event);
}

//###########################################################################
// The poll -- one pass per frame (time-gated so the manager's drain loop
// terminates; an AnalogEvent is emitted every pass to keep the manager's
// five-scalar push running, matching the serial board's analog cadence).
//###########################################################################

void
	PadRIO::Poll()
{
	unsigned long now = timeGetTime();
	float dt = (lastPollMilliseconds == 0)
		? 0.0f
		: (float)(now - lastPollMilliseconds) * 0.001f;
	if (dt > 0.1f)
	{
		dt = 0.1f;		// resumed from a stall -- don't slam the integrators
	}
	lastPollMilliseconds = now;

	//
	//-----------------------------------------------------------------
	// XInput: hot-plug probe every ~3 s, then read the connected pad.
	//-----------------------------------------------------------------
	//
	XINPUT_STATE pad_state;
	int pad_connected = 0;
	if (padIndex >= 0)
	{
		if (XInputGetState(padIndex, &pad_state) == ERROR_SUCCESS)
		{
			pad_connected = 1;
		}
		else
		{
			DEBUG_STREAM << "[padrio] XInput pad " << padIndex
				<< " disconnected\n" << std::flush;
			padIndex = -1;
			previousPadButtons = 0;
		}
	}
	if (padIndex < 0 && (lastPadProbeMilliseconds == 0 ||
		now - lastPadProbeMilliseconds >= 3000))
	{
		lastPadProbeMilliseconds = now;
		for (int slot = 0; slot < 4; ++slot)
		{
			if (XInputGetState(slot, &pad_state) == ERROR_SUCCESS)
			{
				padIndex = slot;
				pad_connected = 1;
				DEBUG_STREAM << "[padrio] XInput pad found in slot "
					<< slot << "\n" << std::flush;
				break;
			}
		}
	}

	//
	//-----------------------------------------------------------------
	// Keyboard bindings: edges fire button/keypad events; held keys
	// accumulate axis motion.  All keys read as RELEASED without focus
	// so held buttons let go when the developer alt-tabs.
	//-----------------------------------------------------------------
	//
	int focused = ProcessHasFocus();
	float slewDelta[PadBindingProfile::ChannelCount];
	int deflectHeld[PadBindingProfile::ChannelCount];
	memset(slewDelta, 0, sizeof(slewDelta));
	memset(deflectHeld, 0, sizeof(deflectHeld));

	for (int k = 0; k < bindings.keyBindingCount; ++k)
	{
		const PadBindingProfile::KeyBinding &binding = bindings.keyBindings[k];
		int held = focused &&
			(GetAsyncKeyState(binding.virtualKey) & 0x8000) != 0;
		int was_held = previousKeyHeld[k];
		previousKeyHeld[k] = (unsigned char)held;

		switch (binding.action.kind)
		{
		case PadBindingProfile::ActionButton:
			if (held != was_held)
			{
				EmitButton(binding.action.address, held);
			}
			break;

		case PadBindingProfile::ActionKeypad:
			if (held && !was_held)
			{
				EmitKeypad(binding.action.address, binding.action.key);
			}
			break;

		case PadBindingProfile::ActionAxisDeflect:
			if (held)
			{
				deflectHeld[binding.action.channel] = 1;
				channelValue[binding.action.channel] +=
					binding.action.rate * dt;
			}
			break;

		case PadBindingProfile::ActionAxisSlew:
			if (held)
			{
				slewDelta[binding.action.channel] +=
					binding.action.rate * dt;
			}
			break;

		case PadBindingProfile::ActionAxisSet:
			if (held && !was_held)
			{
				channelValue[binding.action.channel] = binding.action.rate;
			}
			break;
		}
	}

	//
	// Spring return: a deflect-managed channel with no deflect key held
	// re-centers at its fastest bound rate.
	//
	for (int c = 0; c < PadBindingProfile::ChannelCount; ++c)
	{
		channelValue[c] += slewDelta[c];
		if (!deflectHeld[c] && channelReturnRate[c] > 0.0f)
		{
			float step = channelReturnRate[c] * dt;
			if (channelValue[c] > step)
			{
				channelValue[c] -= step;
			}
			else if (channelValue[c] < -step)
			{
				channelValue[c] += step;
			}
			else
			{
				channelValue[c] = 0.0f;
			}
		}
	}

	//
	//-----------------------------------------------------------------
	// Pad: button edges + axis writes (direct absolute past the
	// deadzone; slew axes integrate).
	//-----------------------------------------------------------------
	//
	if (pad_connected)
	{
		unsigned buttons = pad_state.Gamepad.wButtons;
		for (int b = 0; b < bindings.padButtonBindingCount; ++b)
		{
			const PadBindingProfile::PadButtonBinding &binding =
				bindings.padButtonBindings[b];
			int held = (buttons & binding.buttonMask) != 0;
			int was_held = (previousPadButtons & binding.buttonMask) != 0;
			if (held == was_held)
			{
				continue;
			}
			if (binding.action.kind == PadBindingProfile::ActionButton)
			{
				EmitButton(binding.action.address, held);
			}
			else if (binding.action.kind == PadBindingProfile::ActionKeypad
				&& held)
			{
				EmitKeypad(binding.action.address, binding.action.key);
			}
		}
		previousPadButtons = buttons;

		for (int a = 0; a < bindings.padAxisBindingCount; ++a)
		{
			const PadBindingProfile::PadAxisBinding &binding =
				bindings.padAxisBindings[a];
			float raw = 0.0f;
			switch (binding.axis)
			{
			case PadBindingProfile::PadAxisLX:
				raw = NormalizeThumb(pad_state.Gamepad.sThumbLX,
					XINPUT_GAMEPAD_LEFT_THUMB_DEADZONE);
				break;
			case PadBindingProfile::PadAxisLY:
				raw = NormalizeThumb(pad_state.Gamepad.sThumbLY,
					XINPUT_GAMEPAD_LEFT_THUMB_DEADZONE);
				break;
			case PadBindingProfile::PadAxisRX:
				raw = NormalizeThumb(pad_state.Gamepad.sThumbRX,
					XINPUT_GAMEPAD_RIGHT_THUMB_DEADZONE);
				break;
			case PadBindingProfile::PadAxisRY:
				raw = NormalizeThumb(pad_state.Gamepad.sThumbRY,
					XINPUT_GAMEPAD_RIGHT_THUMB_DEADZONE);
				break;
			case PadBindingProfile::PadAxisLT:
				raw = NormalizeTrigger(pad_state.Gamepad.bLeftTrigger);
				break;
			case PadBindingProfile::PadAxisRT:
				raw = NormalizeTrigger(pad_state.Gamepad.bRightTrigger);
				break;
			}
			if (binding.invert)
			{
				raw = -raw;
			}
			if (binding.slew)
			{
				channelValue[binding.channel] += raw * binding.slewRate * dt;
			}
			else if (raw != 0.0f)
			{
				//
				// Direct absolute: a deflected pad axis owns the channel;
				// centered (inside the deadzone) it leaves the keyboard
				// integration alone.
				//
				channelValue[binding.channel] = raw;
			}
		}
	}

	//
	//-----------------------------------------------------------------
	// Clamp and publish the control surface.  Throttle is the 0..1
	// lever the mapper detents at 1.0; the rest are -1..1.
	//-----------------------------------------------------------------
	//
	for (int c = 0; c < PadBindingProfile::ChannelCount; ++c)
	{
		float low = (c == PadBindingProfile::ChannelThrottle) ? 0.0f : -1.0f;
		if (channelValue[c] < low)   channelValue[c] = low;
		if (channelValue[c] > 1.0f)  channelValue[c] = 1.0f;
	}

	float stick_sign = flipStickAxes ? -1.0f : 1.0f;
	Throttle   = (Scalar)channelValue[PadBindingProfile::ChannelThrottle];
	JoystickX  = (Scalar)(stick_sign *
		channelValue[PadBindingProfile::ChannelJoystickX]);
	JoystickY  = (Scalar)(stick_sign *
		channelValue[PadBindingProfile::ChannelJoystickY]);
	LeftPedal  = (Scalar)channelValue[PadBindingProfile::ChannelLeftPedal];
	RightPedal = (Scalar)channelValue[PadBindingProfile::ChannelRightPedal];

	//
	// The analog heartbeat: tells the manager to run the five-scalar
	// push this frame (LBE4ControlsManager::Execute gates the push on
	// new_RIO_values).
	//
	RIOEvent analog;
	analog.Type = AnalogEvent;
	analog.Data.Unit = 0;
	PushEvent(analog);
}

//###########################################################################
// RIOBase surface
//###########################################################################

Logical
	PadRIO::GetNextEvent(RIOEvent *destinationPointer)
{
	Check_Pointer(destinationPointer);

	if (eventTail == eventHead)
	{
		//
		// Queue drained: poll at most once per millisecond tick so the
		// manager's per-frame drain loop terminates (the poll always
		// enqueues the analog heartbeat).
		//
		unsigned long now = timeGetTime();
		if (now == lastPollMilliseconds)
		{
			return False;
		}
		Poll();
	}
	if (eventTail == eventHead)
	{
		return False;
	}
	*destinationPointer = eventQueue[eventTail];
	eventTail = (eventTail + 1) % EventQueueSize;
	return True;
}

void
	PadRIO::SetLamp(int lampNumber, int state)
{
	if (lampNumber >= 0 && lampNumber < LampCount)
	{
		lampState[lampNumber] = state;
	}
}

//###########################################################################
// The on-screen panel entries
//###########################################################################

Logical
	PadRIO::IsActive()
{
	return activeInstance != NULL;
}

void
	PadRIO::SetScreenButton(int unit, int pressed)
{
	if (activeInstance == NULL)
	{
		return;
	}
	if (unit < 0 || unit >= LBE4ControlsManager::ButtonCount)
	{
		return;
	}
	activeInstance->EmitButton(unit, pressed);
}

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