/*/////////////////////////////////////////////////////////////////////////////

	Project:		Sea Grid
	
	Module:			Sonar Control
	
	Description:	
	
	Author:			Magnus Ottosson, it3otma@ituniv.se
	
	Co Authors:		N/A
	
	Created:		2005-03-31

//////////////////////////////////////////////////////////////////////////////*/

#include <stdlib.h>
#include <stdio.h>
#include <cyg/hal/hal_io.h>
#include <cyg/kernel/kapi.h>
#include <cyg/io/devtab.h>
#include "sigSonarControl/sigSonarControl.h"

#define KEYBOARD 0x60

void drawRootMenu(positions* currentPositions);
void drawPositionsMenu(positions* currentPositions);

void drawRootMenu(positions* currentPositions){
	
	printf("\033[K");
	printf("  -------------------------------------- \n");
	printf(" |        ++ Sonar Server ++            |\n");
	printf(" |           ~~~~~~~~~~~~               |\n");
	printf(" |     (1) See positions                |\n");
	printf(" |     (q) Quit                         |\n");
	printf(" |                                      |\n");
	printf(" |            Make a choice             |\n");
	printf(" |                                      |\n");
	printf("  -------------------------------------- \n");
	
	unsigned int input = 0;

	while (1) {
		
		HAL_READ_UINT8(KEYBOARD, input);

		if (input == 130) {
			drawPositionsMenu(currentPositions);
			break;
		} else if (input == 129) {
			// QUIT;
		}
	}
}

void drawPositionsMenu(positions* currentPositions){
	
	unsigned int input = 0;
	
	while (1) {
		
		HAL_READ_UINT8(KEYBOARD, input);
		
		printf("\033[K");
		printf("  -------------------------------------- \n");
		printf(" | ++ Sonar Server Current Positions ++ |\n");
	  	printf(" |           ~~~~~~~~~~~~               |\n");
	  	printf(" |     Lorry				            |\n");
	  	printf(" |     ----------------                 |\n");
	  	printf(" |     Front X: %d                      |\n",currentPositions -> currentLorryPos.frontX);
	  	printf(" |     Front Y: %d                      |\n",currentPositions -> currentLorryPos.frontY);
	  	printf(" |     Rear  X: %d                      |\n",currentPositions -> currentLorryPos.rearX);
	  	printf(" |     Rear  Y: %d                      |\n",currentPositions -> currentLorryPos.rearY);
	  	printf(" |     Angle  : %f               |\n",currentPositions -> currentLorryPos.angle);
	  	printf(" |                                      |\n");
	  	printf(" |     Barge                            |\n");
	  	printf(" |     ----------------                 |\n");
	  	printf(" |     Front X: %d                      |\n",currentPositions -> currentBargePos.frontX);
	  	printf(" |     Front Y: %d                      |\n",currentPositions -> currentBargePos.frontY);
	  	printf(" |     Rear  X: %d                      |\n",currentPositions -> currentBargePos.rearX);
	  	printf(" |     Rear  Y: %d                      |\n",currentPositions -> currentBargePos.rearY);
	  	printf(" |     Angle  : %f               |\n",currentPositions -> currentBargePos.angle);
	  	printf(" |                                      |\n");
	  	printf(" |   Press ESC to return to root menu   |\n");
	  	printf(" |                                      |\n");
	  	printf("  -------------------------------------- \n");

		if (input == 129) {
			drawRootMenu(currentPositions);
			break;
		}
	}
}


int main(void) {
	
	// Create the positions struct
	// Allocate the memory for the position struct.
	currentPositions = (positions*) malloc(sizeof(positions));
	// Initialize the values to -1
	currentPositions -> currentBargePos.angle = 1.0;
	currentPositions -> currentBargePos.frontX = 1;
	currentPositions -> currentBargePos.frontY = 1;
	currentPositions -> currentBargePos.rearX = 1;
	currentPositions -> currentBargePos.rearY = 1;
	
	currentPositions -> currentLorryPos.angle = 1.0;
	currentPositions -> currentLorryPos.frontX = 1;
	currentPositions -> currentLorryPos.frontY = 1;
	currentPositions -> currentLorryPos.rearX = 1;
	currentPositions -> currentLorryPos.rearY = 1;
	
	// TODO: Thread
	drawRootMenu(currentPositions);
	
	// TODO: Thread
	// startbeacons();
	
	// TODO: Thread
	// listenToRadioReceiver(currentPositions);	
	
	
	return 0;
}