//__attribute__((naked)) //To avoid gcc from putting garbage before and after main function, especially with stuffs using stack...
int main(int argc, char **argv)
{
  serial_init();
  serial_puts("\r\n\r\nHello world!\n\r\n\rBrought to you by Florent Guignabert & Axel Schumacher\r\n\r\n");
  char c = serial_getc();
  while (c != 27 /*echap*/)
  {
    serial_putc(c);
    c = serial_getc();
  }
  serial_puts("\n\r");
  return EXIT_SUCCESS;
}

int
console_wait_for_key(void)
{
	union key key;
	key.ax = serial_getc(true);
	return key.code.ascii;
}

void TaskStart (void *data)
{
	unsigned char i;
	char key;
	data = data;                            /* Prevent compiler warning */
	
	JZ_StartTicker(OS_TICKS_PER_SEC);	/* os_cfg.h */
	printf("uC/OS-II, The Real-Time Kernel MIPS Ported version\n");
	printf("EXAMPLE #1 %s %s\n",__DATE__,__TIME__);
	while(0)
	{
		if(serial_tstc())
		{
			key = serial_getc();
			printf("you pressed: %c\n",key);
			if(key == 0x1B) 
			{        /* see if it's the ESCAPE key */
				printf(" Escape display of statistic\n");
				os_TaskDelete(TASK_START_PRIO);
			}
		}
		printf("time: %d\n",os_TimeGet());
		os_TimeDelayHMSM(0, 0, 1, 0);     /* Wait one second */
	}
	NandTest();
}

int do_ramdump(cmd_tbl_t *cmdtp, int flag, int argc, char *argv[])
{

	exynos_usbd_dn_addr = USBD_DOWN_ADDR;	/* Default Address */

	exynos_receive_done = 0;

	exynos_usbctl_init();
	exynos_usbc_activate();

	printf("Now, Waiting for USB RAMDUMP\n");

	while (1) {
		if (EXYNOS_USBD_DETECT_IRQ()) {
			exynos_udc_int_hndlr();
			EXYNOS_USBD_CLEAR_IRQ();
		}

		if (exynos_receive_done)
			break;

		if (serial_tstc()) {
			serial_getc();
			break;
		}
	}

	/* when operation is done, usbd must be stopped */
	exynos_usb_stop();

	printf("Finished RAMDUMP\n");

	return 0;
}

void gets(char *s)
{
	char c;
	while((c = serial_getc()) != '\n')
		*s++ = c;
	*s = 0;
}

int getDebugChar(void)
{
	if (kgdb_debugport == 0)
		kgdb_debugport = serial_init(KGDB_PORT, NULL);

	return(serial_getc(kgdb_debugport));
}

int main() {
	
	//initialize all necessary sensors and utilities
	lcd_init();
	timer1_init();
	timer3_init();
	move_servo(90);
 	ADC_init();
	USART_Init(MYUBRR);
	init_push_buttons();
	
	oi_t *sensor_data = oi_alloc();
	oi_init(sensor_data);
	
	audioInit(sensor_data);
	//oi_play_song(1);
	
	while(1) {
		//empty currentObjects before proceeding by setting all stored objects to "invalid" - ignored by later checks
		for (int i = 0; i < 20; i++) {
			currentObjects[i].isValid = 0;
		}
		char received = serial_getc(); //take keyboard input from putty
		takeDirectionInput(received, currentObjects); //translate keyboard input into functionality
	}
	
	return 0;
}

int main(int argc)
{
	// Allocate memory and setup sensorData struct
	sensorData = oi_alloc();
	initAll(sensorData);


	serial_puts("Press 's' to initiate connection with robot\n\r");
	while(start != 's') // Wait for Start Command
	{
		start = serial_getc();
	}
	serial_puts("Robot communication initiated.\n\r\n\r");
	display_help();
	running_LED();
	while(1)
	{
		read_user_input_string(sensorData);
	}
	//START TRAVERSING CODE

	while("NOT DONE") //While the robot is not in the Final Zone
	{
		//TODO Interface and Movement
	}

	//TODO FLASH LEDS WHEN DONE


}

/* 'inline' - We have to do it fast */
static inline void mdm_readline(char *buf, int bufsiz)
{
	char c;
	char *p;
	int n;

	n = 0;
	p = buf;
	for(;;) {
		c = serial_getc();

		/*		dbg("(%c)", c); */

		switch(c) {
		case '\r':
			break;
		case '\n':
			*p = '\0';
			return;

		default:
			if(n++ > bufsiz) {
				*p = '\0';
				return; /* sanity check */
			}
			*p = c;
			p++;
			break;
		}
	}
}

static void serial_isr_func(int n)
{
	uint32_t c;
	
	c = serial_getc();
	mbox_post(&console_mbox, &c);
}

int do_pause (cmd_tbl_t * cmdtp, int flag, int argc, char *argv[])
{
	int pause_time;
	unsigned int delay_time;
	int break_loop = 0;

	/* display value if no argument */
	if (argc < 2) {
		pause_time = 1;
	}

	else if (argc > 2) {
		printf ("Too many arguments\n");
		return -1;
	} else {
		pause_time = simple_strtoul (argv[1], NULL, 0);
	}

	printf ("Pausing with a poll time of %d, press any key to reactivate\n", pause_time);
	delay_time = pause_time * ONE_SECOND;
	while (break_loop == 0) {
		udelay (delay_time);
		if (serial_tstc () != 0) {
			break_loop = 1;
			/* eat user key presses */
			while (serial_tstc () != 0) {
				serial_getc ();
			}
		}
	}

	return 0;
}

/*
 *  * Initialise the serial port with the given baudrate. The settings
 *   * are always 8 data bits, no parity, 1 stop bit, no start bits.
 *    *
 *     */
int serial_init (void)
{
    SIO_Open(&gUart1Channel, 115200);
    /* clear input buffer */
    if(serial_tstc())
        serial_getc();
    return 0;
}

void jshIdle() {
  /*static bool foo = false;
  foo = !foo;
  jshPinSetValue(LED1_PININDEX, foo);*/

  while (serial_readable(&mbedSerial[0])>0)
        jshPushIOCharEvent(EV_SERIAL1, serial_getc(&mbedSerial[0]));
}

int spl_start_uboot(void)
{
	/* break into full u-boot on 'c' */
	if (serial_tstc() && serial_getc() == 'c')
		return 1;

	return 0;
}

int getc (void)
{
	if (gd->flags & GD_FLG_DEVINIT) {
		
		return fgetc (stdin);
	}

	return serial_getc ();
}

void CONSOL_GetChar(unsigned char *ch)
{
	int r;
	r = serial_getc();
	if (r > 0)
		*ch = (unsigned char)r;
	else
		*ch = 0;
}

void BufferedSerial::rxIrq(void)
{
    // read from the peripheral and make sure something is available
    if(serial_readable(&_serial)) {
        _rxbuf = serial_getc(&_serial); // if so load them into a buffer
    }

    return;
}

int getc (void)
{
	if (gd->flags & GD_FLG_DEVINIT) {
		/* Get from the standard input */
		return fgetc (stdin);
	}

	/* Send directly to the handler */
	return serial_getc ();
}

int jz_std_gets(unsigned char *s,unsigned int count)
{
  unsigned int i = 0;
  if(jz_std_sem == NULL) jz_std_sem = os_SemaphoreCreate(1);
  OP_STD_LOCK();	
  while(i++ < count)
   	*s++ = serial_getc();
  OP_STD_UNLOCK();
  return (count);
}

static int ameba_uart_getc (struct rt_serial_device *serial)
{
    struct device_uart* uart = serial->parent.user_data;

	if(!serial_readable(&uart->serial))
        return -1;

    /* Receive Data Available */
    return serial_getc(&uart->serial);
}