#include <ultra64.h>

#include <ramrom.h>

#include <stdlib.h>
#include <stdio.h>
#include <string.h>

/* app specific includes */
#include "perf.h"
#include "rdpfuncs.h"
#include "rspfuncs.h"
#include "monkeyfuncs.h"

OSMesgQueue	controllerMsgQ;
OSMesg		controllerMsgBuf;

int initControllers(void);
OSContStatus     statusdata[MAXCONTROLLERS];
OSContPad        controllerdata[MAXCONTROLLERS];


/*
 *
 * Return the lowest number controller connected to system
 */
int initControllers()
{
    OSMesgQueue     serialMsgQ;
    OSMesg          serialMsg;
    int             i;
    u8              pattern;

    osCreateMesgQueue(&serialMsgQ, &serialMsg, 1);
    osSetEventMesg(OS_EVENT_SI, &serialMsgQ, (OSMesg)1);

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

    osCreateMesgQueue(&controllerMsgQ, &controllerMsgBuf, 1);
    osSetEventMesg(OS_EVENT_SI, &controllerMsgQ, (OSMesg)0);

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

void RDPController()
{
  static u16             button, lastbutton;
    
  osContStartReadData(&controllerMsgQ);
  (void)osRecvMesg(&controllerMsgQ, NULL, OS_MESG_BLOCK);
  osContGetReadData(controllerdata);
  button = controllerdata[0].button;
    
  if (button & CONT_L) 
    {
      TestFuncNum--;
    }
	    
  if (button & CONT_R) 
    {
      TestFuncNum++;
    }

  if ((button & CONT_A) && !(lastbutton & CONT_A))
    {
      TestFuncNum--;
    }
	    
  if ((button & CONT_E)  && !(lastbutton & CONT_E))
    {
      TestFuncNum++;
    }

  if ((button & CONT_F)  && !(lastbutton & CONT_F))
    {
      LoopTestFuncNum = 1 - LoopTestFuncNum;
      if (verbose) rmonPrintf("LoopTestFuncNum = %d \n", LoopTestFuncNum);
    }

  if ((button & CONT_UP) && !(lastbutton & CONT_UP))
    {
      UseAAMode = 1 - UseAAMode;
      if (verbose) rmonPrintf("UseAAMode = %d \n", UseAAMode);
    }

  if (TestFuncNum < 0)
    TestFuncNum = NUMRDPFUNCS - 1;
  else if (TestFuncNum == NUMRDPFUNCS)
    TestFuncNum = 0;
	        
  lastbutton = button;
}

void RSPController()
{
  static u16             button, lastbutton;
    
  osContStartReadData(&controllerMsgQ);
  (void)osRecvMesg(&controllerMsgQ, NULL, OS_MESG_BLOCK);
  osContGetReadData(controllerdata);
  button = controllerdata[0].button;
    
  if (button & CONT_L) 
    {
      TestFuncNum--;
    }
	    
  if (button & CONT_R) 
    {
      TestFuncNum++;
    }

  if ((button & CONT_A) && !(lastbutton & CONT_A))
    {
      TestFuncNum--;
    }
	    
  if ((button & CONT_E)  && !(lastbutton & CONT_E))
    {
      TestFuncNum++;
    }

  if ((button & CONT_F)  && !(lastbutton & CONT_F))
    {
      LoopTestFuncNum = 1 - LoopTestFuncNum;
      if (verbose) rmonPrintf("LoopTestFuncNum = %d \n", LoopTestFuncNum);
    }

  if (button & CONT_C)
    {
      VAROPS++;
      if (VAROPS > MAXVAROPS)
	VAROPS = 2;
	
      if (verbose) rmonPrintf("Using %d VarOps \n", VAROPS);
    }

  if (button & CONT_D)
    {
      VAROPS--;
	    
      if (VAROPS < 2)
	VAROPS = MAXVAROPS;
	    
      if (verbose) rmonPrintf("Using %d VarOps \n", VAROPS);
    }

  if (TestFuncNum < 0)
    TestFuncNum = NUMRSPFUNCS - 1;
  else if (TestFuncNum == NUMRSPFUNCS)
    TestFuncNum = 0;
	        
  lastbutton = button;
}


void MonkeyController0()
{
  static u16             button, lastbutton;
    
  osContStartReadData(&controllerMsgQ);
  (void)osRecvMesg(&controllerMsgQ, NULL, OS_MESG_BLOCK);
  osContGetReadData(controllerdata);
  button = controllerdata[0].button;
    
  if (button & CONT_L) 
    {
      TestFuncNum--;
    }
	    
  if (button & CONT_R) 
    {
      TestFuncNum++;
    }

  if ((button & CONT_A) && !(lastbutton & CONT_A))
    {
      TestFuncNum--;
    }

  if ((button & CONT_C) && !(lastbutton & CONT_C))
    {
      if (BalancedAreaDelta == 0)
	BalancedAreaDelta = -AREADELTA;
      else
	BalancedAreaDelta = 0;
    }

  if ((button & CONT_D) && !(lastbutton & CONT_D))
    {
      if (BalancedAreaDelta == 0)
	BalancedAreaDelta = AREADELTA;
      else
	BalancedAreaDelta = 0;
    }

	    
  if ((button & CONT_E)  && !(lastbutton & CONT_E))
    {
      TestFuncNum++;
    }

  if ((button & CONT_F)  && !(lastbutton & CONT_F))
    {
      LoopTestFuncNum = 1 - LoopTestFuncNum;
      if (verbose) rmonPrintf("LoopTestFuncNum = %d \n", LoopTestFuncNum);
    }

  if (button & CONT_LEFT)
    {
      BalancedArea-= 
	(BalancedArea < 10) ? 1 : 
	((BalancedArea > 200) ? BalancedArea/2 : BalancedArea / 10);
	    
      if (BalancedArea < 1)
	BalancedArea = 1;
	    
      if (verbose) rmonPrintf("BalancedArea %d \n", BalancedArea);
    }

  if (button & CONT_RIGHT)
    {
      BalancedArea+= 
	(BalancedArea < 10) ? 1 : 
	((BalancedArea > 200) ? BalancedArea : BalancedArea / 10);
      
      if (verbose) rmonPrintf("BalancedArea %d \n", BalancedArea);      
    }

  if (TestFuncNum < 0)
    TestFuncNum = NUMMONKEYFUNCS - 1;
  else if (TestFuncNum == NUMMONKEYFUNCS)
    TestFuncNum = 0;
	        
  lastbutton = button;
}

void MonkeyController1()
{
  static u16             button, lastbutton;
    
  osContStartReadData(&controllerMsgQ);
  (void)osRecvMesg(&controllerMsgQ, NULL, OS_MESG_BLOCK);
  osContGetReadData(controllerdata);
  button = controllerdata[0].button;
    
  if (button & CONT_L) 
    {
      TestFuncNum--;
    }
	    
  if (button & CONT_R) 
    {
      TestFuncNum++;
    }

  if (button & CONT_A)
    {
      StripArea-= 
	(StripArea < 10) ? 1 : 
	((StripArea > 200) ? StripArea/10 : StripArea / 10);
	    
      if (StripArea < 1)
	StripArea = 1;
	    
      if (verbose) rmonPrintf("StripArea %d \n", StripArea);
    }

  if (button & CONT_B)
    {
      StripArea+= 
	(StripArea < 10) ? 1 : 
	((StripArea > 200) ? StripArea/10 : StripArea / 10);
      
      if (verbose) rmonPrintf("StripArea %d \n", StripArea);      
    }

  if (button & CONT_C)
    {
      StripHeight-= 
	(StripHeight < 10) ? 1 : 
	((StripHeight > 200) ? StripHeight/10 : StripHeight / 10);
	    
      if (StripHeight < 1)
	StripHeight = 1;
	    
      if (verbose) rmonPrintf("StripHeight %d \n", StripHeight);
    }

  if (button & CONT_D)
    {
      StripHeight+= 
	(StripHeight < 10) ? 1 : 
	((StripHeight > 200) ? StripHeight/10 : StripHeight / 10);

      if (StripHeight > 2 * StripArea)
	StripHeight = 2 * StripArea;
            
      if (verbose) rmonPrintf("StripHeight %d \n", StripHeight);      
    }

  if ((button & CONT_E) && !(lastbutton & CONT_E))
    {
      if (StripHeightDelta == 0)
	StripHeightDelta = -1;
      else
	StripHeightDelta = 0;
    }

  if ((button & CONT_F) && !(lastbutton & CONT_F))
    {
      if (StripHeightDelta == 0)
	StripHeightDelta = 1;
      else
	StripHeightDelta = 0;
    }


  if (button & CONT_LEFT)
    {
      AnchorX--;      

      if (AnchorX < 1) AnchorX = 1;
	    
      if (verbose) rmonPrintf("AnchorX %d \n", AnchorX);
    }

  if (button & CONT_RIGHT)
    {
      AnchorX++;      

      if (verbose) rmonPrintf("AnchorX %d \n", AnchorX);
    }

  if (button & CONT_DOWN)
    {
      AnchorY--;      

      if (AnchorY < 1) AnchorY = 1;
	    
      if (verbose) rmonPrintf("AnchorY %d \n", AnchorY);
    }

  if (button & CONT_UP)
    {
      AnchorY++;      

      if (verbose) rmonPrintf("AnchorY %d \n", AnchorY);
    }


  if (TestFuncNum < 0)
    TestFuncNum = NUMMONKEYFUNCS - 1;
  else if (TestFuncNum == NUMMONKEYFUNCS)
    TestFuncNum = 0;
	        
  lastbutton = button;
}