#include "common.h"
#include "input.h"
#include "time.h"
#include "display.h"

#include <psxpad.h>

static u_char padBuffer[2][34];
static u_short prevButtons = 0xFFFF;

static const int moveSpeed = 1024;
static const short rotSpeed = 1024;
static const char deadZone = 0x30;

static u_char dispmode = 0;
static u_char widescreen = 0;

void input_init()
{
	InitPAD(padBuffer[0], 34, padBuffer[1], 34);
	padBuffer[0][0] = padBuffer[0][1] = 0xFF;
	padBuffer[1][0] = padBuffer[1][1] = 0xFF;
	StartPAD();
	ChangeClearPAD(1);	// Avoid issues with VSync timeouts caused by StartPAD
}

void input_process()
{
	// Check if we have a connected controller on port 1
	if (padBuffer[0][0] != 0)
		return;

	// Determine direction vectors of the camera
	MATRIX cam_mtx;
	RotMatrixQ(&cam_rot, &cam_mtx);

    int deltaTime = time_getDeltaTime();
    int moveInterval = (moveSpeed * deltaTime) >> 12;
    int rotInterval = (moveSpeed * deltaTime) >> 12;

	u_short buttons = *((u_short*)(padBuffer[0]+2));
	if (!(buttons & PAD_UP))
	{
		// Move forward (+Y)
		cam_pos.vx += (cam_mtx.m[0][1] * moveInterval) >> 12;
		cam_pos.vy += (cam_mtx.m[1][1] * moveInterval) >> 12;
		cam_pos.vz += (cam_mtx.m[2][1] * moveInterval) >> 12;
	}
	if (!(buttons & PAD_DOWN))
	{
		// Move backward (-Y)
		cam_pos.vx -= (cam_mtx.m[0][1] * moveInterval) >> 12;
		cam_pos.vy -= (cam_mtx.m[1][1] * moveInterval) >> 12;
		cam_pos.vz -= (cam_mtx.m[2][1] * moveInterval) >> 12;
	}
	if (!(buttons & PAD_LEFT))
	{
		// Rotate left
		cam_rot.vz += rotInterval;
	}
	if (!(buttons & PAD_RIGHT))
	{
		// Rotate right
		cam_rot.vz -= rotInterval;
	}
	if (!(buttons & PAD_TRIANGLE))
	{
		// Move up (+Z)
		cam_pos.vz += moveInterval;
	}
	if (!(buttons & PAD_CROSS))
	{
		// Move down (-Z)
		cam_pos.vz -= moveInterval;
	}
	if (!(buttons & PAD_L1))
	{
		// Strafe left (-X)
		cam_pos.vx -= (cam_mtx.m[0][0] * moveInterval) >> 12;
		cam_pos.vy -= (cam_mtx.m[1][0] * moveInterval) >> 12;
	}
	if (!(buttons & PAD_R1))
	{
		// Strafe right (+X)
		cam_pos.vx += (cam_mtx.m[0][0] * moveInterval) >> 12;
		cam_pos.vy += (cam_mtx.m[1][0] * moveInterval) >> 12;
	}
	if (!(buttons & PAD_L2))
	{
		// Look down
		cam_rot.vx -= rotInterval;
	}
	if (!(buttons & PAD_R2))
	{
		// Look up
		cam_rot.vx += rotInterval;
	}
    if (!(buttons & PAD_SELECT) && (prevButtons & PAD_SELECT))
    {
        // Switch display mode
		//dispmode = (dispmode + 1) & 0x3;
		//display_reset(!!(dispmode & 2), !!(dispmode & 1), widescreen, NULL);
		display_reset(!GetVideoMode(), 0, widescreen, NULL);
    }
    if (!(buttons & PAD_START) && (prevButtons & PAD_START))
    {
		widescreen = !widescreen;
		display_reset(GetVideoMode(), 0, widescreen, NULL);
    }
	if (!(buttons & PAD_CIRCLE) && (prevButtons & PAD_CIRCLE))
	{
		enableTexturing = !enableTexturing;
	}
	
	// Check for analog controller
	u_char padType = padBuffer[0][1] >> 4;
	if (padType == 0x5 || padType == 0x7)
	{
		short rightJoyX = (short)(padBuffer[0][4]) - 0x80;
		if (rightJoyX > -deadZone && rightJoyX < deadZone) rightJoyX = 0;
		short rightJoyY = (short)(padBuffer[0][5]) - 0x80;
		if (rightJoyY > -deadZone && rightJoyY < deadZone) rightJoyY = 0;
		short leftJoyX = (short)(padBuffer[0][6]) - 0x80;
		if (leftJoyX > -deadZone && leftJoyX < deadZone) leftJoyX = 0;
		short leftJoyY = (short)(padBuffer[0][7]) - 0x80;
		if (leftJoyY > -deadZone && leftJoyY < deadZone) leftJoyY = 0;

		// Move forward/backward
		cam_pos.vx -= (cam_mtx.m[0][1] * leftJoyY * moveInterval) >> 19;
		cam_pos.vy -= (cam_mtx.m[1][1] * leftJoyY * moveInterval) >> 19;
		cam_pos.vz -= (cam_mtx.m[2][1] * leftJoyY * moveInterval) >> 19;

		// Strafe left/right
		cam_pos.vx += (cam_mtx.m[0][0] * leftJoyX * moveInterval) >> 19;
		cam_pos.vy += (cam_mtx.m[1][0] * leftJoyX * moveInterval) >> 19;

		// Rotate left/right
		cam_rot.vz -= (rightJoyX * rotInterval) >> 7;

		// Look up/down
		cam_rot.vx -= (rightJoyY * rotInterval) >> 7;
	}

	prevButtons = buttons;
}