void printDebug(STATE state){
  LCD_MoveCursor(1,1);
  char stringToPrint[16];
  sprintf(stringToPrint,"%4d%4d%4d%4d",IRSensor_GetLeftFront(), IRSensor_GetCenterLeft(), IRSensor_GetCenterRight(), IRSensor_GetRightFront());
  LCD_PrintString(stringToPrint);
  LCD_MoveCursor(1,2);
  sprintf(stringToPrint,"OC:%5d:%5d %1d",OC2RS,OC4RS,state);
  LCD_PrintString(stringToPrint);
};
void GS_SetCursorPosition( int x, int y )
{
	cursor_vars.locCursorX = x;
	cursor_vars.locCursorY = y;
	
	if( cursor_vars.cursorStatus != CURSOR_STATUS_OFF ) {
#ifdef _HARDWARE_CURSOR
		LCD_MoveCursor( cursor_vars.locCursorX, cursor_vars.locCursorY );
#endif
	}
}
void GS_SetCursorStatus( cursor_status_t status )
{	
	/*
	 * NOTE: We change cursor's behavior again.
	 */
	if( cursor_vars.cursorStatus == status )
		return;		/* unchange status */

	if( status == CURSOR_STATUS_OFF_NODRAW ) {
		if( cursor_vars.cursorStatus == CURSOR_STATUS_OFF )
			return;

		status = CURSOR_STATUS_OFF;
		goto label_no_erase_cursor;
	}

#ifndef _HARDWARE_CURSOR
	if( cursor_vars.bCursorBlinkStatus )
		GS_EraseCursor( cursor_vars.locCursorX, cursor_vars.locCursorY, cursor_vars.cursorStatus );
#endif

label_no_erase_cursor:

	switch( cursor_vars.cursorStatus = status ) {
	case CURSOR_STATUS_OFF:
		cursor_vars.bCursorBlinkAction = 0;
		
#ifdef _HARDWARE_CURSOR
		LCD_CursorOnOff( 0, 0 );
#endif
		break;
		
	case CURSOR_STATUS_SQUARE:
#ifdef _HARDWARE_CURSOR
		cursor_vars.bCursorBlinkAction = 0;
		
		LCD_MoveCursor( cursor_vars.locCursorX, cursor_vars.locCursorY );
		LCD_CursorOnOff( 1, 1 );
#else
		cursor_vars.bCursorBlinkAction = 1;
		cursor_vars.bCursorBlinkStatus = 1;
		cursor_vars.cursorTime = GetUptimeInMillisecond();

		GS_DrawCursor( cursor_vars.locCursorX, cursor_vars.locCursorY, CURSOR_STATUS_SQUARE );
#endif
		break;
		
	case CURSOR_STATUS_ON:
	default:
		cursor_vars.bCursorBlinkAction = 1;
		cursor_vars.bCursorBlinkStatus = 1;
		cursor_vars.cursorTime = GetUptimeInMillisecond();
		
#ifdef _HARDWARE_CURSOR
		LCD_MoveCursor( cursor_vars.locCursorX, cursor_vars.locCursorY );
		LCD_CursorOnOff( 1, 0 );
#else
		GS_DrawCursor( cursor_vars.locCursorX, cursor_vars.locCursorY, CURSOR_STATUS_ON );
#endif
		break;
	}	
}