#include "AdeptHeaders.hpp"

#include "Joystick.hpp"

//############################################################################
//############################### Input Filters ##############################
//############################################################################

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// Construct a Unipolar input filter
//
UnipolarFilter::UnipolarFilter(
	Scalar minimum,
	Scalar maximum,
	Scalar dead_band,
	Direction direction,
	LimitType limit_type
):
	inputAverage(5,0.0f)
{
	positiveDirection = direction;
	Calibrate(minimum, maximum, dead_band);
	limitType = limit_type;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
UnipolarFilter::~UnipolarFilter()
{
	Check_Object(this);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Scalar
	UnipolarFilter::Update(Scalar value)
{
	Check_Object(this);

	//
	//------------------------------------------------
	// Generate average, use for min/max determination
	//------------------------------------------------
	//
	switch (limitType)
	{
	case PanningLimits:
		{
			inputAverage.Add(value);
			Scalar average = inputAverage.CalculateOlympicAverage();
			if (average < inputMin)
			{
				inputMax -= inputMin - average;
				inputMin = average;
			}
			else if (average > inputMax)
			{
				inputMin += average - inputMax;
				inputMax = average;
			}
		}
		break;
	case FloatingLimits:
		{
			inputAverage.Add(value);
			Scalar average = inputAverage.CalculateOlympicAverage();
			if (average < inputMin)
			{
				Calibrate(average, inputMax, deadBand);
			}
			else if (average > inputMax)
			{
				Calibrate(inputMin, average, deadBand);
			}
		}
		break;
	}

	//
	//-------------------------------------------------------------------------
	// In the case of a unipolar filter, subtract out the minimum value and the
	// deadband, the scale the remainder base on the range
	//-------------------------------------------------------------------------
	//
	Scalar result = 0.0f;
	value -= inputMin + lowerDeadband;
	if (value>0 && lowerRange>0)
	{
		if (value > lowerRange)
		{
			result = 1.0f;
		}
		else
		{
			result = static_cast<Scalar>(value)/lowerRange;
			Verify(result >= 0.0f && result <= 1.0f);
		}
	}
	if (positiveDirection == DecreasingPositive)
	{
		result = 1.0f - result;
	}
	return result;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// Recalibrates the filter
//
void
	UnipolarFilter::Calibrate(
		Scalar minimum,
		Scalar maximum,
		Scalar dead_band
	)
{
	Check_Object(this);

	Verify(dead_band >= 0.0f);
	Verify(maximum >= minimum);

	deadBand = dead_band;
	inputMin = minimum;
	inputMax = maximum;

	lowerDeadband = (inputMax-inputMin)*deadBand + 0.5f;
	lowerRange = (inputMax-lowerDeadband) - (inputMin+lowerDeadband);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	UnipolarFilter::BeginAlignment()
{
	Check_Object(this);
	Scalar average = inputAverage.CalculateAverage();
	Calibrate(average, average, deadBand);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	UnipolarFilter::EndAlignment()
{
	Check_Object(this);

	//
	//----------------------------------------------------------
	// Move the edges of the filter ranges back by three percent
	//----------------------------------------------------------
	//
	Scalar delta = (inputMax - inputMin) * 0.03f;
	inputMax -= delta;
	inputMin += delta;
	Calibrate(inputMin, inputMax, deadBand);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	UnipolarFilter::SetDeadBand(Scalar percent)
{
	Check_Object(this);
	Calibrate(inputMin, inputMax, percent);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// Construct a Bipolar input filter
//
BipolarFilter::BipolarFilter(
	Scalar minimum,
	Scalar center,
	Scalar maximum,
	Scalar dead_band,
	Direction direction,
	LimitType limit_type
):
	UnipolarFilter(minimum, maximum, dead_band, direction, limit_type)
{
	Verify(limit_type != PanningLimits);
	Calibrate(minimum, center, maximum, dead_band);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
BipolarFilter::~BipolarFilter()
{
	Check_Object(this);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Scalar
	BipolarFilter::Update(Scalar value)
{
	Check_Object(this);

	//
	//------------------------------------------------
	// Generate average, use for min/max determination
	//------------------------------------------------
	//
	switch (limitType)
	{
	case FloatingLimits:
		{
			inputAverage.Add(value);
			Scalar average = inputAverage.CalculateOlympicAverage();
			if (average < inputMin)
			{
				Calibrate(average, inputCenter, inputMax, deadBand);
			}
			else if (average > inputMax)
			{
				Calibrate(inputMin, inputCenter, average, deadBand);
			}
		}
		break;
	}

	//
	//-----------------------------------------------------------
	// Figure out which half of the input we will be dealing with
	//-----------------------------------------------------------
	//
	Scalar result = 0.0f;
	value -= inputCenter;

	//
	//------------------------------------------------------------------
	// If we are in the lower half of the input and beyond the deadband,
	// scale the input against the lower half range
	//------------------------------------------------------------------
	//
	if (value < 0)
	{
		value += lowerDeadband;
		if (value<0 && lowerRange>=0)
		{
			if (value <= -lowerRange)
			{
				result = -1.0f;
			}
			else
			{
				result = value/lowerRange;
				Verify(result >= -1.0f && result < 0.0f);
			}
		}
	}

	//
	//--------------------------------------------------------------------
	// Otherwise, we are in the upper half of the input.  If we are beyond
	// the deadband, scale the input against the upper half range
	//--------------------------------------------------------------------
	//
	else
	{
		value -= upperDeadband;
		if (value>0 && upperRange>=0)
		{
			if (value >= upperRange)
			{
				result = 1.0f;
			}
			else
			{
				result = value/upperRange;
				Verify(result >= 0.0f && result <= 1.0f);
			}
		}
	}
	if (positiveDirection == DecreasingPositive)
	{
		result = -result;
	}
	return result;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// Recalibrates the filter
//
void
	BipolarFilter::Calibrate(
		Scalar minimum,
		Scalar center,
		Scalar maximum,
		Scalar dead_band
	)
{
	Check_Object(this);

	Verify(dead_band >= 0.0f);
	Verify(minimum <= maximum);
	Verify(center >= minimum);
	Verify(center <= maximum);

	deadBand = dead_band;
	inputMin = minimum;
	inputCenter = center;
	inputMax = maximum;

	lowerDeadband = (inputCenter-inputMin)*deadBand;
	lowerRange = center - (inputMin+lowerDeadband);
	upperDeadband = (inputMax-inputCenter)*deadBand;
	upperRange = (inputMax-upperDeadband) - inputCenter;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	BipolarFilter::BeginAlignment()
{
	Check_Object(this);
	Scalar average = inputAverage.CalculateAverage();
	Calibrate(average, average, average, deadBand);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	BipolarFilter::EndAlignment()
{
	Check_Object(this);

	//
	//---------------------------------------------------------------------
	// Calculate the new center point by the current joystick average, then
	// move the edges of the joystick ranges back by three percent
	//---------------------------------------------------------------------
	//
	Scalar average = inputAverage.CalculateAverage();
	inputMax -= (inputMax - average) * 0.03f;
	inputMin += (average - inputMin) * 0.03f;
	Calibrate(inputMin, average, inputMax, deadBand);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	BipolarFilter::SetDeadBand(Scalar percent)
{
	Check_Object(this);
	Calibrate(inputMin, inputCenter, inputMax, percent);
}

//###########################################################################
//############################ Joystick classes #############################
//###########################################################################

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Joystick::Joystick(
	DWORD stick_id,
	Scalar x_dead_band,
	Scalar y_dead_band,
	Scalar throttle_dead_band,
	Scalar rudder_dead_band
)
{
	Check_Pointer(this);

	buttonCount = CalculateButtonCount(stick_id);
	joystickID = stick_id;
	m_ShiftButton = -1;

	//
	//--------------------------------
	// Initialize the member variables
	//--------------------------------
	//
	hatValue = -1.0f;
	lastHatValue = -1.0f;
	hatStatus = NoHat;

	//
	//------------------------------
	// Build the filterpointer array
	//------------------------------
	//
	memset(axisValues,0,sizeof(axisValues));
	memset(axisFilters,0,sizeof(axisFilters));

	//
	//---------------------------------------------------
	// Set up the filters for X and Y, which we will have
	//---------------------------------------------------
	//
	gosJoystick_Info ji;
	gosJoystick_GetInfo(stick_id, &ji);
	m_ForceFeedback = ji.bIsForceFeedback;
	Verify(ji.bAxisValid & (1<<JOY_XAXIS));
	joystickXInput = JOY_XAXIS;
	UnipolarFilter *filter =
		new BipolarFilter(
			-1.0f,
			0.0f,
			1.0f,
			x_dead_band,
			BipolarFilter::IncreasingPositive,
			BipolarFilter::FixedLimits
		);
	AdoptFilter(XAxis, filter);

	Verify(ji.bAxisValid & (1<<JOY_YAXIS));
	joystickYInput = JOY_YAXIS;
	filter =
		new BipolarFilter(
			-1.0f,
			0.0f,
			1.0f,
			y_dead_band,
			BipolarFilter::IncreasingPositive,
			BipolarFilter::FixedLimits
		);
	AdoptFilter(YAxis, filter);

	//
	//------------------
	// Deal with the hat
	//------------------
	//
	if (ji.nPOVs > 0)
	{
		hatStatus = HasAHat;
	}

	//
	//------------------------
	// Add the throttle device
	//------------------------
	//
	if (ji.bAxisValid & (1<<JOY_THROTTLE))
	{
		throttleInput = JOY_THROTTLE;
		filter =
			new ThrottleFilter(
				-1.0f,
				1.0f,
				throttle_dead_band
			);
		AdoptFilter(ThrottleAxis, filter);
	}
	else
	if (ji.bAxisValid & (1<<JOY_SLIDER1))
	{
		throttleInput = JOY_SLIDER1;
		filter =
			new ThrottleFilter(
				-1.0f,
				1.0f,
				throttle_dead_band
			);
		AdoptFilter(ThrottleAxis, filter);
	}
	else
	{
		throttleInput = static_cast<GOSJoystickAxis>(12);			//ELEMENTS(ji.bAxisValid));
	}


	//
	//----------------------
	// Add the rudder device
	//----------------------
	//
	if (ji.bAxisValid & (1<<JOY_RUDDER))
	{
		rudderInput = JOY_RUDDER;
		filter =
			new BipolarFilter(
				-1.0f,
				0.0f,
				1.0f,
				rudder_dead_band,
				BipolarFilter::IncreasingPositive,
				BipolarFilter::FixedLimits
			);
		AdoptFilter(RudderAxis, filter);
	}
	else
	{
		rudderInput = static_cast<GOSJoystickAxis>(12);			//ELEMENTS(ji.bAxisValid));
	}

	//
	//-------------------------------
	// Read the initial button states
	//-------------------------------
	//
	buttonValues = new bool[buttonCount];
	Register_Pointer(buttonValues);
	for (DWORD i=0; i<buttonCount; ++i)
	{
		buttonValues[i] = gosJoystick_ButtonStatus(joystickID, i);
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Joystick::~Joystick()
{
	Check_Object(this);

	//
	//------------------------------
	// Delete any additional filters
	//------------------------------
	//
	Verify(axisFilters);
	int i;
	for (i=0; i<AxisCount; ++i)
	{
		if (axisFilters[i])
		{
			Unregister_Object(axisFilters[i]);
			delete axisFilters[i];
		}
	}

	Unregister_Pointer(buttonValues);
	delete [] buttonValues;
	gosJoystick_SetPolling(joystickID, false);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Joystick::FilterInput(
		int axis,
		Scalar value
	)
{
	Check_Object(this);
	Verify(static_cast<unsigned>(axis) < AxisCount);
	Check_Object(axisFilters[axis]);
	axisValues[axis] = axisFilters[axis]->Update(value);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Joystick::AdoptFilter(
		int axis,
		UnipolarFilter *filter
	)
{
	Check_Object(this);
	Verify(static_cast<unsigned>(axis) < AxisCount);

	Register_Object(filter);
	axisFilters[axis] = filter;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Joystick::SetDeadBand(Scalar percent)
{
	Check_Object(this);
	for (int i=0; i<AxisCount; ++i)
	{
		if (axisFilters[i])
		{
			Check_Object(axisFilters[i]);
			axisFilters[i]->SetDeadBand(percent);
		}
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Joystick::SetXDeadBand(Scalar percent)
{
	Check_Object(this);
	
	if (axisFilters[XAxis])
	{
		Check_Object(axisFilters[XAxis]);
		axisFilters[XAxis]->SetDeadBand(percent);
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Joystick::SetYDeadBand(Scalar percent)
{
	Check_Object(this);
	
	if (axisFilters[YAxis])
	{
		Check_Object(axisFilters[YAxis]);
		axisFilters[YAxis]->SetDeadBand(percent);
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Joystick::SetThrottleDeadBand(Scalar percent)
{
	Check_Object(this);
	
	if (axisFilters[ThrottleAxis])
	{
		Check_Object(axisFilters[ThrottleAxis]);
		axisFilters[ThrottleAxis]->SetDeadBand(percent);
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Joystick::SetRudderDeadBand(Scalar percent)
{
	Check_Object(this);
	
	if (axisFilters[RudderAxis])
	{
		Check_Object(axisFilters[RudderAxis]);
		axisFilters[RudderAxis]->SetDeadBand(percent);
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Joystick::Update(
		int mode_mask,
		ControlsUpdateManagerOf<Scalar> *joystick_axis_group,
		ControlsUpdateManagerOf<int> *joystick_button_group,
		ControlsUpdateManagerOf<int> *joystick_hat
	)
{
	Check_Object(this);
	Check_Pointer(joystick_axis_group);
	Check_Pointer(joystick_button_group);

	//
	//----------------------------------
	// First update the joystick buttons
	//----------------------------------
	//
	Check_Pointer(buttonValues);
	for (DWORD i=0; i<buttonCount; ++i)
	{
		if (i == m_ShiftButton)				// button m_ShiftButton is the shift button on the joystick, nothing should be mapped to it.
			continue;
		int button = ControlsManager::JoystickButtonIDBase + i;
		Check_Object(&joystick_button_group[i]);
		bool pressed = gosJoystick_ButtonStatus(joystickID, i);
//		if (pressed != buttonValues[i])
		{
			if (pressed)
			{
//				button = -button;
//				joystick_button_group[i].Update(&button, ControlsManager::AlwaysActive);
//				button = -button;
				joystick_button_group[i].Update(&button, mode_mask);
			}
			else
			{
				button = -button;
				joystick_button_group[i].Update(&button, ControlsManager::AlwaysActive);
			}
			buttonValues[i] = pressed;
		}
	}

	//
	//----------------------------------
	// Update the standard joystick axes
	//----------------------------------
	//
	Verify(static_cast<unsigned>(joystickXInput) <= JOY_SLIDER2);
	FilterInput(XAxis, gosJoystick_GetAxis(joystickID, joystickXInput));
	Check_Object(&joystick_axis_group[XAxis]);
	joystick_axis_group[XAxis].Update(&axisValues[XAxis], 1);

	Verify(static_cast<unsigned>(joystickYInput) <= JOY_SLIDER2);
	FilterInput(YAxis, gosJoystick_GetAxis(joystickID, joystickYInput));
	Check_Object(&joystick_axis_group[YAxis]);
	joystick_axis_group[YAxis].Update(&axisValues[YAxis], 1);

	//
	//------------------
	// Read the throttle
	//------------------
	//
	if (static_cast<unsigned>(throttleInput) <= JOY_SLIDER2)
	{
		FilterInput(
			ThrottleAxis,
			gosJoystick_GetAxis(joystickID, throttleInput)
		);
		Check_Object(&joystick_axis_group[ThrottleAxis]);
		joystick_axis_group[ThrottleAxis].Update(
			&axisValues[ThrottleAxis],
			1
		);
	}

	//
	//----------------
	// Read the rudder
	//----------------
	//
	if (static_cast<unsigned>(rudderInput) <= JOY_SLIDER2)
	{
		FilterInput(
			RudderAxis,
			gosJoystick_GetAxis(joystickID, rudderInput)
		);
		Check_Object(&joystick_axis_group[RudderAxis]);
		joystick_axis_group[RudderAxis].Update(
			&axisValues[RudderAxis],
			1
		);
	}

	//
	//----------------------
	// Read the joystick hat
	//----------------------
	//
	if (HasHat())
	{
		axisValues[HatAxis] = gosJoystick_GetAxis(joystickID, JOY_HAT1);
		Check_Object(&joystick_axis_group[HatAxis]);
		joystick_axis_group[HatAxis].Update(
			&axisValues[HatAxis],
			1
		);

		hatValue = axisValues[HatAxis];

		if (hatValue != lastHatValue) {
			Check_Object(joystick_hat);
			int temp = 1;
			if (hatValue == -1.0f) {}
			// Left down
			else if ((hatValue>= 0.625f) && (hatValue <= 0.875f))
				joystick_hat[0].Update(&temp, 1);
			// Right down
			else if ((hatValue>= 0.125f) && (hatValue <= 0.375f))
				joystick_hat[1].Update(&temp, 1);
			// Bottom down
			else if ((hatValue> 0.375f) && (hatValue < 0.625f))
				joystick_hat[3].Update(&temp, 1);
			// Top down
			else if ((hatValue > 0.875f) || (hatValue < 0.125f))
				joystick_hat[2].Update(&temp, 1);

			temp = 0;
			if (lastHatValue == -1.0f) {}
			// Left up
			else if ((lastHatValue>= 0.625f) && (lastHatValue <= 0.875f))
				joystick_hat[0].Update(&temp, 1);
			// Right up
			else if ((lastHatValue>= 0.125f) && (lastHatValue <= 0.375f))
				joystick_hat[1].Update(&temp, 1);
			// Bottom up
			else if ((lastHatValue> 0.375f) && (lastHatValue < 0.625f))
				joystick_hat[3].Update(&temp, 1);
			// Top up
			else if ((lastHatValue > 0.875f) || (lastHatValue < 0.125f))
				joystick_hat[2].Update(&temp, 1);
			lastHatValue = hatValue;
		}
	}
	else
	{
		hatValue = -1.0f;
	}

#if 0
	//
	//---------------------------------------------------
	// Check to see which button values have been pressed
	//---------------------------------------------------
	//
	int state = buttonsPressed;
	int button;
	for (i=0; state; ++i, state>>=1)
	{
		if (state&1)
		{
			button = ControlsManager::JoystickButtonIDBase + i;
			Check_Pointer(joystick_button_group);
			Check_Object(&joystick_button_group[i]);
			joystick_button_group[i].Update(&button, mode_mask);
		}
	}

	//
	//----------------------------------------------------
	// Check to see which button values have been released
	//----------------------------------------------------
	//
	state = buttonsReleased;
	for (i=0; state; ++i, state>>=1)
	{
		if (state&1)
		{
			button = -ControlsManager::JoystickButtonIDBase - i;
			Check_Pointer(joystick_button_group);
			Check_Object(&joystick_button_group[i]);
			joystick_button_group[i].Update(&button, mode_mask);
		}
	}
#endif
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
int
	Joystick::CalculateButtonCount(DWORD stick_id)
{
	gosJoystick_Info ji;
	gosJoystick_GetInfo(stick_id, &ji);
	return ji.nButtons;
}

bool Joystick::ShiftButton (void)
{
	if (m_ShiftButton == -1)
		return false;
	if (buttonCount <= m_ShiftButton)
		return false;
	return gosJoystick_ButtonStatus(joystickID, m_ShiftButton);
}