/* usbtest.c */
#include <string.h>

#include "kernel.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"

/* OSEK declarations */
DeclareTask(Task1); 

/* below macro enables run-time USB connection */
#define RUNTIME_CONNECTION

/* LEJOS OSEK hooks */
void ecrobot_device_initialize()
{
#ifndef RUNTIME_CONNECTION
	ecrobot_init_usb(); /* init USB */
#endif
}

void ecrobot_device_terminate()
{
	ecrobot_term_usb();
}

/* LEJOS OSEK hook to be invoked from an ISR in category 2 */
void user_1ms_isr_type2(void){}

#define MAX_NUM_OF_CHAR 16
#define MAX_NUM_OF_LINE 8

static void display_usb_data(U8 *data, int len)
{
	int i;
	static int pos_x = 0;
	static int pos_y = 0;

	/* set LCD postion in x, y */
	if (pos_x >= MAX_NUM_OF_CHAR)
	{
		pos_x = 0;
		pos_y++;
	}

	if (pos_y >= MAX_NUM_OF_LINE)
	{
		pos_x = 0;
		pos_y = 0;
	}
			
	if (pos_x == 0 && pos_y == 0)
	{
		display_clear(0);
	}
	display_goto_xy(pos_x, pos_y);

	for (i = 0; i < len; i++)
	{
		if (data[i] == '\n')
		{
			pos_x = 0;
			pos_y++;
			break;
		}
		else
		{
			display_string((char *)&data[i]);
			display_update();
			if (i == (len - 1))
			{
				pos_x += len;
				break;
			}
		}
	}
}

TASK(Task1)
{
	int len;
	U8 data[MAX_USB_DATA_SIZE];

#ifdef RUNTIME_CONNECTION
	ecrobot_init_usb(); /* init USB */
#endif

	display_clear(0);
	display_goto_xy(0, 0);
	display_string("USB TEST");
	display_goto_xy(0, 1);
	display_string("Run usbhost.exe");
	display_update();
	while(1)
  	{
		memset(data, 0, MAX_USB_DATA_SIZE);
		len = ecrobot_read_usb(data, MAX_USB_DATA_SIZE); /* read USB data */
  		
  		if (len > 0)
  		{
			ecrobot_send_usb(data, len);    /* send USB data */
			display_usb_data(data, len);
  		}
	}
  
	TerminateTask();
}
