#pragma config(UART_Usage, UART1, uartVEXLCD, baudRate19200, IOPins, None, None)
#pragma config(UART_Usage, UART2, uartNotUsed, baudRate4800, IOPins, None, None)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

typedef enum
{
	noButton = 0,
	leftButton = 1,
	centerButton = 2,
	rightButton = 4
} sLCDButtons;

typedef enum
{
	left = -1,
	right = 1,
	nullPosition = 0,
} pAlignments;

typedef enum
{
	blue = -1,
	red = 1,
	nullColor = 0
} pAllianceColors;

#define numberOfRoutines 5



void LCDWaitForRelease ()
{
	while(nLCDButtons != noButton) wait1Msec(100);
}
//The enterLCDButton function will look for and wait for a button to be pressed, and returns that button's name
sLCDButtons enterLCDButton ( bool useLeft, bool useCenter, bool useRight, int timeoutSec = 10)
{
	ClearTimer(T1);
	bool quit = false;
	sLCDButtons returnButton = noButton;

	while(!quit)
	{
		if(useLeft && nLCDButtons == leftButton)
		{
			quit = true;
			LCDWaitForRelease();
			returnButton = leftButton;
		}
		else if(useCenter && nLCDButtons == centerButton)
		{
			quit = true;
			LCDWaitForRelease();
			returnButton = centerButton;
		}
		else if(useRight && nLCDButtons == rightButton)
		{
			quit = true;
			LCDWaitForRelease();
			returnButton = rightButton;
		}
		else if(time1[T1] > (timeoutSec * 1000))
		{
			quit = true;
			returnButton = noButton;
		}
	}
	return returnButton;
}

void displayLCDAlignedString(int nLine, pAlignments align, const string textToDisplay)
{
	int rightPosition = 16-strlen(textToDisplay);
	writeDebugStreamLine("%i", rightPosition);
	switch(align)
	{
	case left: displayLCDString(nLine,0,textToDisplay); break;
	case right: displayLCDString(nLine,rightPosition, textToDisplay); break;
	}
}


task main()
{
	bLCDBacklight = true;
	sLCDButtons retrievedButton = noButton;

	/* INFORMATION TO BE DETERMINED */
	pAllianceColors allianceColor = nullColor;
	pAlignments alliancePosition = nullPosition;
	int routineToRun = -1;

	displayLCDCenteredString(0,"Loading..."); // random loading screen becase we can
	clearLCDLine(1);
	wait1Msec(100);

	displayLCDCenteredString(0,"Alliance Color?");
	displayLCDAlignedString(1,left,"Blue");
	displayLCDAlignedString(1,right,"Red");
	retrievedButton = enterLCDButton(true,false,true);
	switch(retrievedButton)
	{
	case leftButton:
		allianceColor = blue;
		displayLCDCenteredString(0,"Selected:");
		displayLCDCenteredString(1,"BLUE");
		break;
	case rightButton:
		allianceColor = red;
		displayLCDCenteredString(0,"Selected:");
		displayLCDCenteredString(1,"RED");
		break;
	default:
		allianceColor = nullColor;
		displayLCDCenteredString(0,"Error Selecting");
		displayLCDCenteredString(1,"Terminating"); //Your code would "terminate" the selection process, or you could say "defaulting" if you have a default autonomous
		break;
	}
	wait10Msec(50);
	clearLCDLine(0);
	clearLCDLine(1);
	retrievedButton = noButton;

	if(allianceColor != nullColor)
	{
		displayLCDCenteredString(0,"Alliance Pos?");
		char leftArrow = 200;
		char rightArrow = 199;
		string leftText; sprintf(leftText, "%c Left", leftArrow);
		string rightText; sprintf(rightText, "Right %c", rightArrow);
		displayLCDAlignedString(1,left,leftText);
		displayLCDAlignedString(1,right,rightText);
		retrievedButton = enterLCDButton(true,false,true);
		switch(retrievedButton)
		{
		case leftButton:
			alliancePosition = left;
			displayLCDCenteredString(0,"Selected:");
			displayLCDCenteredString(1,"LEFT");
			break;
		case rightButton:
			alliancePosition = right;
			displayLCDCenteredString(0,"Selected:");
			displayLCDCenteredString(1,"RIGHT");
			break;
		default:
			alliancePosition = nullPosition;
			displayLCDCenteredString(0,"Error Selecting");
			displayLCDCenteredString(1,"Terminating");
			break;
		}
		wait10Msec(50);
		clearLCDLine(0);
		clearLCDLine(1);
		retrievedButton = noButton;

		if(alliancePosition != nullPosition)
		{
			bool quit = false;
			int selectedRoutine = (numberOfRoutines/2);

			displayLCDCenteredString(0,"Select Routine");
			while(!quit)
			{
				switch(selectedRoutine) {
				case 1:
					displayLCDCenteredString(1,"<    R1    >");
					break;
				case 2:
					displayLCDCenteredString(1,"<    R2    >");
					break;
				case 3:
					displayLCDCenteredString(1,"<    R3    >");
					break;
				case 4:
					displayLCDCenteredString(1,"<    R4    >");
					break;
				case 5:
					displayLCDCenteredString(1,"<    R5    >");
					break;
				default:
					displayLCDCenteredString(1,"ERROR");
					break;
				}
				retrievedButton = enterLCDButton(true,true,true);
				switch(retrievedButton) {
				case leftButton:
					if(selectedRoutine <= 1) selectedRoutine = numberOfRoutines;
					else selectedRoutine--;
					break;
				case rightButton:
					if(selectedRoutine >= numberOfRoutines) selectedRoutine = 1;
					else selectedRoutine++;
					break;
				case centerButton:
					routineToRun = selectedRoutine;
					quit = true;
					break;
				}
			}
			displayLCDCenteredString(0,"Selected:");
			switch(routineToRun)
			{
			case 1:
				displayLCDCenteredString(1,"R1");
				break;
			case 2:
				displayLCDCenteredString(1,"R2");
				break;
			case 3:
				displayLCDCenteredString(1,"R3");
				break;
			case 4:
				displayLCDCenteredString(1,"R4");
				break;
			case 5:
				displayLCDCenteredString(1,"R5");
				break;
			default:
				displayLCDCenteredString(1,"ERROR");
				break;
			}
			wait1Msec(500);

			displayLCDCenteredString(0,"Selections:");
			string selections; sprintf(selections, "%i , %i , R%i", allianceColor, alliancePosition, selectedRoutine);
			displayLCDCenteredString(1,selections);
			wait1Msec(1000);
		}
	}
}