/*
 *  walk_around.c - implements simple motion model
 *
 *  The joystick is for turning left, right and moving back and forth. 
 *  Up and down on the crosshair tilts the view up and down; left and 
 *  right on the crosshair 'sidestep' to the left and right. The six 
 *  'fire buttons' accomplish the same suite of movements.
 */
#include <ultra64.h>

/*
 * local includes: 
 */
#include "app.h"
#include "walk_around.h"
#include "vector.h"
#include "matrix.h"

/*
 * The slop factor is the amount of slop we allow in the joystick
 * before registering a movement.
 */
#define SLOP_FACTOR 12

/*
 *  Max number of major modes
 */
#define MAX_MODES	7

/*
 *  Globals
 */
int MyMode = 0;
int MyPalette = 0;
int DoParseRdp = 0;
int DoParseGbi = 0;

/*
 *  Controller globals
 */
static OSMesgQueue contMessageQ;
static OSMesg   dummyMessage;
static OSContStatus statusdata[MAXCONTROLLERS];
static OSContPad controllerdata[MAXCONTROLLERS];
static int      controller;

static int      firstcall = 1;

static int      lastx;
static int      lasty;
static int      lastbutton = 0;
static int      press = 0;
static int      press_up = 0;
static int      press_down = 0;

/*
 *  empirically determined
 */
#define deltarot		0.2f
#define deltatrans		0.2f
#define stickdeltarot	0.001f
#define stickdeltatrans	0.1f

/*
 * User's eye in world coords
 */
static vec3     eyept;

/*
 * Used to generate incRot Mtx
 */
static float    phi;

/*
 * Used to generate incRot Mtx
 */
static float    axis[3];

/*
 *  translation increments
 */
static float    dx = 0,
                dy = 0,
                dz = 0;

/*
 * rotation increments
 */
static float    rx = 0,
                ry = 0,
                rz = 0;

/*
 * incremental rotation matrix
 */
static Mtx      incRot;

/*
 * incremental translation matrix
 */
static Mtx      incTrans;
static Mtx      incMat;

/*
 * viewing matrix and inverse
 */
static FMatrix  viewMat;

/*
static FMatrix  invMat;
*/

/*
 * last frame's viewing mat
 */
static Mtx      lastViewMat;

/*
 *  user's initial position
 */
static float    initial_pos[3] =
{10, 10, 200};


/*
 *  Print routines
 */
static void
print_palette(int pal) {
		rmonPrintf("        pallette = %d\n", MyPalette);
}

static void
print_initial(void) {
		rmonPrintf("======= Color Index and Load Tile Test ========\n\n");
		rmonPrintf("   Use L key to increment case\n");
		rmonPrintf("   Use R key to decrement case\n");
		rmonPrintf("   Use Joy Stick and Arrow keys to move around\n");
		rmonPrintf("   Use Start Key to Stop\n");
}

static void
print_case(int mode) {
	switch(mode) {
		case 0:
			rmonPrintf("Mirrored 16-bit texture, load_block\n");
			break;

		case 1:
			rmonPrintf("Shaded quad\n");
			break;

		case 2:
			rmonPrintf("16-entry palette\n");
			rmonPrintf("    Use A key to increase palette\n");
			rmonPrintf("    Use B key to decrease palette\n");
			break;

		case 3:
			rmonPrintf("256-entry palette, gray scale\n");
			break;

		case 4:
			rmonPrintf("4-bit I, LERP between ENV and PRIM color, load_tile\n");
			break;

		case 5:
			rmonPrintf("8-bit CI, load_tile\n");
			break;

		case 6:
			rmonPrintf("16-bit RGBA, load_tile\n");
			break;

		case 7:
			rmonPrintf("32-bit RGBA, load_tile\n");
			break;
	}
}

/*
 *
 * Return the lowest number controller connected to system
 */
int
walkAroundInit(float x, float y, float z)
{
	int             i;
	u8				pattern;
	
	initial_pos[0] = x;
	initial_pos[1] = y;
	initial_pos[2] = z;

	osCreateMesgQueue(&contMessageQ, &dummyMessage, 1);
	osSetEventMesg(OS_EVENT_SI, &contMessageQ, (OSMesg) 0);

	osContInit(&contMessageQ, &pattern, &statusdata[0]);


	for (i = 0; i < MAXCONTROLLERS; i++) {
		if ((pattern & (1 << i)) &&
			!(statusdata[i].errno & CONT_NO_RESPONSE_ERROR)) {
			osContStartReadData(&contMessageQ);
			controller = i;
			print_initial();
			return i;
		}
	}
	controller = -1;
	return -1;
}

/*
 *  Update controller info
 */
static void
readControllers(void)
{
	OSContPad      *pad;

	if (osRecvMesg(&contMessageQ, &dummyMessage, OS_MESG_NOBLOCK) == 0) {
		osContGetReadData(controllerdata);
		osContStartReadData(&contMessageQ);
	}

	pad = &controllerdata[controller];

	press = lastbutton ^ pad->button;
	press_up = lastbutton & press;
	press_down = pad->button & press;

	lastbutton = pad->button;
	lastx = pad->stick_x;
	lasty = pad->stick_y;
}

/*
 *  implement 'walk around' motion model
 *
 * Now map the controller to movements. The joystick is for
 * turning left, right and moving back and forth. Up and down on the
 * crosshair tilts the view up and down; left and right on the crosshair
 * 'sidestep' to the left and right. 
 *
 */
void
walkAround(Dynamic *dynamicp)
{
	if(controller >= 0)
		readControllers();

	/*
	 * The 'Start' button resets the user's position
	 */
	if(firstcall) {
		eyept[0] = initial_pos[0];
		eyept[1] = initial_pos[1];
		eyept[2] = initial_pos[2];
		guLookAt(&lastViewMat, initial_pos[0], initial_pos[1], initial_pos[2],
				 0, 0, 0, 0, 0, 1);
		firstcall = 0;
	}
	if (press_down & CONT_START) {
		dx = dy = dz = 0.0;
		rx = ry = rz = 0.0;
	}
	if (lastx > SLOP_FACTOR) {
		ry = (float) (lastx - SLOP_FACTOR) * stickdeltarot;
	} else if (lastx < -SLOP_FACTOR) {
		ry = (float) (lastx + SLOP_FACTOR) * stickdeltarot;
	}
	if (lasty > SLOP_FACTOR) {
		dz = (float) (lasty - SLOP_FACTOR) * stickdeltatrans;
	} else if (lasty < -SLOP_FACTOR) {
		dz = (float) (lasty + SLOP_FACTOR) * stickdeltatrans;
	}
	if ((press_down & CONT_UP)) {
		dy = -deltatrans;
	}
	if ((press_down & CONT_DOWN)) {
		dy = deltatrans;
	}
	if (press_down & (CONT_LEFT)) {
		dx = deltatrans;
	}
	if (press_down & (CONT_RIGHT)) {
		dx = -deltatrans;
	}
	if (press_down & CONT_A) {
		MyPalette++;
		if(MyPalette > 15)
			MyPalette = 0;
		print_palette(MyPalette);
	}
	if (press_down & CONT_B) {
		MyPalette--;
		if(MyPalette < 0)
			MyPalette = 15;
		print_palette(MyPalette);
	}
	if (press_down & CONT_C) {
	}
	if (press_down & CONT_D) {
	}
	if (press_down & CONT_E) {
	}
	if (press_down & CONT_F) {
			DoParseGbi = 1;
	} else {
			DoParseGbi = 0;
	}
	if (press_down & CONT_G) {
			DoParseRdp = 1;
	}
	else {
			DoParseRdp = 0;
	}
	if (press_down & CONT_L) {
		MyMode++;
		if(MyMode > MAX_MODES)
			MyMode = 0;
		print_case(MyMode);
	}
	if (press_down & CONT_R) {
		MyMode--;
		if(MyMode < 0)
			MyMode = MAX_MODES;
		print_case(MyMode);
	}
	phi = sqrtf(rx * rx + ry * ry);

#ifdef JUNK
	rmonPrintf("--------\n");
	rmonPrintf("phi = %f\n", phi);
	rmonPrintf("rx = %f\n", rx);
	rmonPrintf("ry = %f\n", ry);
	rmonPrintf("dx = %f\n", dx);
	rmonPrintf("dy = %f\n", dy);
	rmonPrintf("dz = %f\n", dz);
	rmonPrintf("--------\n");
#endif /* JUNK */

	if (phi == 0) {
		makeIdentMtx(&incRot);	
	} else {
		axis[0] = rx / phi;
		axis[1] = ry / phi;
		axis[2] = 0.0;
		NORMALIZE_VEC3(axis);
		guRotate(&incRot, phi * 360.0 / 6.283,
				 axis[0], axis[1], axis[2]);
	}

	guTranslate(&incTrans, dx, dy, dz);
	multMtx(&incMat, &incRot, &incTrans);
	multMtx(&dynamicp->viewing, &lastViewMat, &incMat);
	copyMtx(&lastViewMat, &dynamicp->viewing);

	/*
	 * obtain eye point in world space 
	 */
	guMtxL2F(viewMat, &dynamicp->viewing);
}