#include "trakr.h"

#include "audio.h"

#include "Images/PlayCircle1.o.h"
#include "Images/PlayCircle2.o.h"
#include "Images/PlayTriangle1.o.h"
#include "Images/PlayTriangle2.o.h"
#include "Images/PlaySquare.o.h"
#include "Images/Spec.o.h"
#include "Images/Test1.o.h"
#include "Images/Test2.o.h"

bool Test_Config( );
bool Test_Motors( );
bool Test_ADCMotors( );
bool Test_Controls( );
bool Test_FileIO( );
bool Test_OSD( );
bool Test_Direct_OSD();
bool Test_Timers();
bool Test_ADCs(  );
bool Test_Audio(  );



void SVT_ROsd_ShowSend(void)
{
  SVT_ROsd_Show();
  if(SVT_ROsd_Is_Cmd_Sent()==false)
  {
	if ( SVT_ROsd_Send_Cmd_Times( 10 ) != true ) {
      SVT_Log( "  Frame send error - not successful in 10 attempts" );
      SVT_ROsd_Open();
	}
    return;
  }
}

//void SVT_ROsd_ShowSend(void)
//{
//  SVT_ROsd_Show();
//  if(SVT_ROsd_Is_Cmd_Sent()==false)
//  {
//	SVT_ROsd_Send_Cmd();
//	SVT_ROsd_Open();
//    return;
//  }
//}


bool Start()
{
  SVT_ROsd_Open();

  return true;
}


bool End()
{
  SVT_Graphics_Close();
  return true;
}


bool Run()
{
  SVT_Log( "START TEST" );

  SVT_ROsd_Clear_Screen();
  SVT_ROsd_Draw_Text( 5, 5, "Testing" );
  SVT_ROsd_Draw_Text( 5, 30, "TRAKR" );
  SVT_ROsd_ShowSend();

  SVT_Sleep( 1000 );

 // SVT_ROsd_Clear_Screen();


  Test_Audio();

  // Test_Motors( );
//  Test_OSD( );

/*
   Test_Direct_OSD();

   SVT_Sleep( 1000 );

  Test_Config( );
  Test_Timers();

  Test_ADCs(  );
  Test_FileIO( );
  Test_Controls( );

  Test_Motors( );

  Test_ADCMotors( );
*/
  SVT_Log( "  Frame Count %d", SVT_Graphics_GetFrameCount() );

  SVT_Log( "END TEST" );

  return false;
}

#define TEST_AUDIO_FILENAME "A:\\Test\\Audio.wav"

#define WAV_BUFFER_SIZE       (300*1024)
unsigned char Wave_Buffer[WAV_BUFFER_SIZE];
TAG_HANDLE  Wave;

bool Test_Audio()
{
	int i;

  SVT_ROsd_Clear_Screen();
  SVT_ROsd_Draw_Text( 5, 5, "Testing" );
  SVT_ROsd_Draw_Text( 5, 30, "AUDIO" );
  SVT_ROsd_ShowSend();
  SVT_Log( "Audio" );

  Wave.pRamBuffer=Wave_Buffer;
  Wave.RamMax=WAV_BUFFER_SIZE;

  SVT_ROsd_Clear_Rectangle( 5, 80, 159, 119  );
  SVT_ROsd_Draw_Text( 5, 80, "REC" );
  SVT_ROsd_ShowSend();

  if ( !SVT_WAV_Record( TEST_AUDIO_FILENAME, &Wave ) ) {
    SVT_Log(   "  No Record OPEN" );
    return false;
  }

  SVT_ResetTimer();
  while( SVT_GetTimer() < 2000 ) {
	  SVT_Sleep( 10 );
	  if ( !SVT_WAV_Recording( &Wave ) ) {
		SVT_Log( "  Recording Failed" );
		break;
	  }
  }
  SVT_Log( "  Recording Lasted %d", SVT_GetTimer() );

  SVT_WAV_Stop( &Wave );


  SVT_ROsd_Clear_Rectangle( 5, 80, 159, 119  );
  SVT_ROsd_Draw_Text( 5, 80, "PLAY" );
  SVT_ROsd_ShowSend();


  for ( i = 0; i < 2; i++ ) {
	  SVT_Log( "  Playback %d", i );
	  SVT_WAV_Play( TEST_AUDIO_FILENAME, &Wave );
	  SVT_ResetTimer();
	  while( SVT_WAV_Is_Busy( &Wave ) && SVT_GetTimer() < 15000 ) {
		  SVT_Sleep( 100 );
	  }
	  SVT_Log( "  Playback Lasted %d", SVT_GetTimer() );
  }

//  JAPI_CloseRfAudio();
//  JAPI_CloseRfVideo();
  SVT_Log( "  Audio Done");

  SVT_ROsd_Clear_Rectangle( 5, 80, 159, 119  );
  SVT_ROsd_Draw_Text( 5, 80, "DONE" );
  SVT_ROsd_ShowSend();

  SVT_ROsd_Clear_Screen();
  SVT_ROsd_ShowSend();


  //extern bool SVT_WAV_Play(const char * pFiLename, TAG_HANDLE* pWave);
  //extern bool SVT_WAV_Is_Busy(TAG_HANDLE *Handle);
}

bool Test_ADCs(  )
{
  SVT_ROsd_Draw_Text( 5, 5, "Testing" );
  SVT_ROsd_Draw_Text( 5, 30, "ADCS" );
  SVT_ROsd_ShowSend();

  SVT_Log( "Testing ADCs" );
  SVT_Log( "  Battery Voltage: %d",  SVT_GetBatteryVoltage() );
  SVT_Log( "  Left Motor Current: %d", SVT_Motor_GetLeftCurrent() );
  SVT_Log( "  Right Motor Current: %d",  SVT_Motor_GetRightCurrent() );

  SVT_Sleep( 1000 );

  int i;
  int s[ 8 ];
  for ( i = 0; i < 8; i++ ) {
	  s[ i ] = JAPI_ReadADC( i );
	  SVT_Log( "  ACD %d: %d",  i, s[ i ] );
  }

  SVT_ROsd_Draw_Text( 5, 5, "Testing ADCs" );
  for ( i = 0; i < 8; i++ )
	  SVT_ROsd_Draw_Text( 5 + ( i % 4 ) * 40, 30 + ( i / 4 ) * 25, "%d", s[i] );
  SVT_ROsd_ShowSend();

  SVT_Sleep( 2000 );

  return true;
}


bool Test_Timers()
{
  SVT_ROsd_Draw_Text( 5, 5, "Testing" );
  SVT_ROsd_Draw_Text( 5, 30, "Timers" );
  SVT_ROsd_ShowSend();

  SVT_Log( "Testing Timers" );

  int i = 0;

  SVT_ResetTimer();
  SVT_Sleep( 10 );
  SVT_Log( "  10ms Sleep: Timer at %d", SVT_GetTimer() );

  SVT_ResetTimer();
  SVT_Sleep( 1000 );
  SVT_Log( "  1000ms Sleep: Timer at %d", SVT_GetTimer() );

  for ( i = 0; i < 10; i++ ) {
	SVT_ResetTimer();
    SVT_Sleep( 100 );
    SVT_Log( "  100ms Sleep %d: Timer at %d", i, SVT_GetTimer() );
  }

  return true;
}


unsigned int Spec_Buffer[ 40*1024 ];

bool Test_Direct_OSD()
{
  JAPI_OpenRfVideo();
  SVT_Memory_Copy((char *)Spec_Buffer,_binary_Images_Test1_bmp_start, _binary_Images_Test1_bmp_end-_binary_Images_Test1_bmp_start );
  JAPI_SendRfVideo((unsigned long*)Spec_Buffer,_binary_Images_Test1_bmp_end-_binary_Images_Test1_bmp_start);
  SVT_Sleep( 2000 );
  SVT_Memory_Copy((char *)Spec_Buffer,_binary_Images_Test2_bmp_start, _binary_Images_Test2_bmp_end-_binary_Images_Test2_bmp_start );
  JAPI_SendRfVideo((unsigned long*)Spec_Buffer,_binary_Images_Test2_bmp_end-_binary_Images_Test2_bmp_start);
  SVT_Sleep( 2000 );
  JAPI_CloseRfVideo();
  return true;
}

bool Test_OSD()
{
  TAG_COLOR ColorTP;
  ColorTP.R=0x7F;
  ColorTP.G=0;
  ColorTP.B=0x00;
  ColorTP.Transparent=0;

  SVT_ROsd_Draw_Text( 5, 5, "Testing" );
  SVT_ROsd_Draw_Text( 5, 30, "Graphics" );
  SVT_ROsd_ShowSend();

  SVT_Log( "Testing Graphics" );
  //SVT_Graphics_Open();

  // Wait for the pipeline to start up
  int s = 0;
//  while( ( s++ < 10 ) && ( !SVT_Graphics_GetFrameCount() ) )
//    SVT_Sleep( 1000 );
//  if ( s >= 10 )
//    SVT_Log( "  No frames in %d secs", s );

  SVT_Log( "  Start in %d secs", s );
  SVT_Log( "  Frame Count %d", SVT_Graphics_GetFrameCount() );

  SVT_Sleep( 1000 );

    for ( s = 0; s < 10; s++ ) {
    	  SVT_ResetTimer();
    	  SVT_Log( "  Writing %d", s );
    	  SVT_ROsd_Clear_Screen();
    	  SVT_ROsd_Draw_Text( 5, 30, "%d", s );
    	  SVT_ROsd_ShowSend();
    	  SVT_Log( "    UI Time %d", SVT_GetTimer() );
    	  SVT_Sleep( 1000 );
    }

    for ( s = 0; s < 10; s++ ) {
    	  SVT_ResetTimer();
    	  SVT_Log( "  Writing %d", s );
    	  SVT_ROsd_Clear_Screen();
    	  SVT_ROsd_Draw_Text( 5, 30, "%d", s );
    	  SVT_ROsd_ShowSend();
    	  SVT_Log( "    UI Time %d", SVT_GetTimer() );
    	  SVT_Sleep( 100 );
    }


  SVT_ResetTimer();
  SVT_Log( "  Writing Strings" );
  SVT_ROsd_Clear_Screen();
  SVT_ROsd_Draw_Text( 5, 30, "Test String" );
  SVT_ROsd_ShowSend();
  SVT_Log( "    Format and Show %d", SVT_GetTimer() );
  SVT_Sleep( 1000 );
  SVT_ResetTimer();
  SVT_ROsd_Clear_Screen();
  SVT_ROsd_Draw_Text( 5, 30, "Test %d", SVT_Graphics_GetFrameCount() );
  SVT_ROsd_ShowSend();
  SVT_Log( "    Number and Show %d", SVT_GetTimer() );
  SVT_Sleep( 1000 );
  SVT_ResetTimer();
  SVT_ROsd_Clear_Screen();
  SVT_ROsd_Draw_Text( 5, 30, "Test %s", "STRING" );
  SVT_ROsd_ShowSend();
  SVT_Log( "    String and Number %d", SVT_GetTimer() );
  SVT_Sleep( 1000 );
  SVT_ResetTimer();
  SVT_ROsd_Clear_Screen();
  SVT_ROsd_Draw_Text( 5, 30, "%s", "!@#$%^&*()[]{}.," );
  SVT_ROsd_ShowSend();
  SVT_Log( "    Chars and Number %d", SVT_GetTimer() );
  SVT_Sleep( 1000 );

  ColorTP.R=0xFF;
  ColorTP.G=0xFF;
  ColorTP.B=0xFF;
  ColorTP.Transparent=0x00;
  SVT_ROsd_Clear_Screen();
  SVT_ROsd_Set_Text_Color( &ColorTP );
  SVT_ROsd_Draw_Text( 5, 60, "Trans 0" );
  SVT_ResetTimer();
  ColorTP.G=0x00;
  ColorTP.R=0xFF;
  ColorTP.B=0x00;
  ColorTP.Transparent=0x00;
  SVT_ROsd_Set_Text_Color( &ColorTP );
  SVT_ROsd_Draw_Text( 5, 30, "RED" );
  ColorTP.R=0x00;
  ColorTP.G=0xFF;
  SVT_ROsd_Set_Text_Color( &ColorTP );
  SVT_ROsd_Draw_Text( 60, 30, "GREEN" );
  ColorTP.G=0x00;
  ColorTP.B=0xFF;
  SVT_ROsd_Set_Text_Color( &ColorTP );
  SVT_ROsd_Draw_Text( 120, 30, "BLUE" );
  SVT_ROsd_ShowSend();
  SVT_Log( "    Colors %d", SVT_GetTimer() );
  SVT_Sleep( 2000 );

  ColorTP.R=0xFF;
  ColorTP.G=0xFF;
  ColorTP.B=0xFF;
  ColorTP.Transparent=0x00;
  SVT_ROsd_Clear_Screen();
  SVT_ROsd_Set_Text_Color( &ColorTP );
  SVT_ROsd_Draw_Text( 5, 60, "Trans 3F" );
  SVT_ResetTimer();
  ColorTP.G=0x00;
  ColorTP.R=0x7F;
  ColorTP.B=0x00;
  ColorTP.Transparent=0x3F;
  SVT_ROsd_Set_Text_Color( &ColorTP );
  SVT_ROsd_Draw_Text( 5, 30, "RED" );
  ColorTP.R=0x00;
  ColorTP.G=0x7F;
  SVT_ROsd_Set_Text_Color( &ColorTP );
  SVT_ROsd_Draw_Text( 60, 30, "GREEN" );
  ColorTP.G=0x00;
  ColorTP.B=0x7F;
  SVT_ROsd_Set_Text_Color( &ColorTP );
  SVT_ROsd_Draw_Text( 120, 30, "BLUE" );
  SVT_ROsd_ShowSend();
  SVT_Log( "    Colors %d", SVT_GetTimer() );
  SVT_Sleep( 2000 );

  ColorTP.R=0xFF;
  ColorTP.G=0xFF;
  ColorTP.B=0xFF;
  ColorTP.Transparent=0x00;
  SVT_ROsd_Clear_Screen();
  SVT_ROsd_Set_Text_Color( &ColorTP );
  SVT_ROsd_Draw_Text( 5, 60, "Trans 7F" );
  SVT_ResetTimer();
  ColorTP.G=0x00;
  ColorTP.R=0x7F;
  ColorTP.B=0x00;
  ColorTP.Transparent=0x7F;
  SVT_ROsd_Set_Text_Color( &ColorTP );
  SVT_ROsd_Draw_Text( 5, 30, "RED" );
  ColorTP.R=0x00;
  ColorTP.G=0x7F;
  SVT_ROsd_Set_Text_Color( &ColorTP );
  SVT_ROsd_Draw_Text( 60, 30, "GREEN" );
  ColorTP.G=0x00;
  ColorTP.B=0x7F;
  SVT_ROsd_Set_Text_Color( &ColorTP );
  SVT_ROsd_Draw_Text( 120, 30, "BLUE" );
  SVT_ROsd_ShowSend();
  SVT_Log( "    Colors %d", SVT_GetTimer() );
  SVT_Sleep( 2000 );

  ColorTP.R=0x7F;
  ColorTP.G=0x7F;
  ColorTP.B=0x7F;
  ColorTP.Transparent=0x00;
  SVT_ROsd_Set_Text_Color( &ColorTP );

  SVT_ROsd_Clear_Screen();
  SVT_ROsd_Draw_Text( 5, 30, "Rectangles" );
  SVT_ROsd_ShowSend();
  SVT_Sleep( 1000 );

  SVT_ResetTimer();
  SVT_Log( "  Rectangles" );

  ColorTP.R=0x7F;
  ColorTP.G=0x7F;
  ColorTP.B=0x7F;
  ColorTP.Transparent=0x0;

  SVT_Osd_Set_Linewidth( 10 );
  SVT_ROsd_Draw_Rectangle( 0, 0, 60, 60, &ColorTP );
  SVT_Log( "    Rect 1 %d", SVT_Graphics_GetFrameCount() );

  ColorTP.R=0x7F;
  ColorTP.G=0x7F;
  ColorTP.B=0x00;
  ColorTP.Transparent=0x0;

  SVT_ROsd_Draw_Rectangle( 10, 10, 50, 50, &ColorTP );
  SVT_Log( "    Rect 2 %d", SVT_Graphics_GetFrameCount() );

  SVT_ROsd_Clear_Rectangle( 20, 20, 30, 30 );
  SVT_Log( "    Clear Rect  %d", SVT_Graphics_GetFrameCount() );

  SVT_ROsd_ShowSend();
  SVT_Log( "    Show %d", SVT_Graphics_GetFrameCount() );

  SVT_Sleep( 3000 );

  SVT_ROsd_Clear_Screen();
  SVT_ROsd_Draw_Text( 5, 30, "Bitmaps" );
  SVT_ROsd_ShowSend();
  SVT_Sleep( 1000 );

  SVT_ResetTimer();
  SVT_Log( "  Loading Test Bitmaps" );
  // Send some bitmaps
  SVT_ROsd_Pack_Bmps_Open();
  SVT_Log( "    Pack Open", SVT_GetTimer() );
  SVT_ROsd_Pack_Bmps(_binary_PlayCircle1_bin_start,
                     _binary_PlayCircle1_bin_end-\
                     _binary_PlayCircle1_bin_start);
  SVT_Log( "    PlayCircle %d", s, s, SVT_GetTimer() );
  SVT_ROsd_Pack_Bmps(_binary_PlayTriangle1_bin_start,
                     _binary_PlayTriangle1_bin_end-\
                     _binary_PlayTriangle1_bin_start);
  SVT_Log( "    PlayTriangle1 %d", s, s, SVT_GetTimer() );
  SVT_ROsd_Pack_Bmps(_binary_PlaySquare_bin_start,
                     _binary_PlaySquare_bin_end-\
                     _binary_PlaySquare_bin_start);
  SVT_Log( "    PlaySquare %d", s, s, SVT_GetTimer() );
  SVT_ROsd_Pack_Bmps(_binary_Images_Spec_bmp_start,
                     _binary_Images_Spec_bmp_end-\
                     _binary_Images_Spec_bmp_start);
  SVT_Log( "    Spec %d", s, s, SVT_GetTimer() );
  SVT_ROsd_Pack_Bmps_Send();
  SVT_Log( "    SENT %d", s, s, SVT_GetTimer() );
  SVT_ROsd_Pack_Bmps_Close();

  SVT_Log( "  Drawing Bitmaps");
  for ( s = 0; s < 10; s++ ) {
	SVT_ResetTimer();
	SVT_ROsd_Clear_Screen();
    SVT_Log( "    Iteration %d", s );
    SVT_ROsd_Draw_Bitmap(1, s * 10, 50, &ColorTP);
    SVT_Log( "      Draw 1 %d", SVT_GetTimer() );
    SVT_ROsd_Draw_Bitmap(2, s * 10, 90, &ColorTP);
    SVT_Log( "      Draw 2 %d", SVT_GetTimer() );
    SVT_ROsd_ShowSend();
    SVT_Log( "      Show %d", SVT_GetTimer() );
    if ( SVT_GetTimer() > 10 )
        SVT_Log( "      Two Bitmaps + Show > 10ms" );

    SVT_Sleep( 1000 );
  }

  SVT_Log( "  Done Testing Graphics" );

  return true;
}

bool Test_Config( )
{
  SVT_Log( "Testing Config" );
  SVT_Log( "  char size : %d", sizeof( char ) );
  SVT_Log( "  short size : %d", sizeof( short ) );
  SVT_Log( "  int size : %d", sizeof( int ) );
  SVT_Log( "  long size : %d", sizeof( long ) );
  return true;
}

#define TEST_FILENAME "A:\\Test\\FileTest.tmp"

bool Test_FileIO(  )
{
  SVT_ROsd_Draw_Text( 5, 5, "Testing" );
  SVT_ROsd_Draw_Text( 5, 30, "FileIO" );
  SVT_ROsd_ShowSend();

  SVT_Log( "Testing FileIO" );
  int i;
  SVT_File_Delete( TEST_FILENAME );
  SVT_FileHandle fh = SVT_File_Create( TEST_FILENAME );
  uint32 written;
  for ( i = 0; i < 10; i++ )
    SVT_File_Write( fh, "Trakr\n", 6, &written );
  if ( written !=6 )
   SVT_Log( "  Wrong length %d != 6", written );
  SVT_File_Write( fh, "Spy\n", 4, &written );
  SVT_File_Close( fh );
  SVT_Log( "  File Written" );

  uint32 read;
  char buffer[ 100 ];
  fh = SVT_File_Open( TEST_FILENAME );
  SVT_Log( "  Reading File" );
  for ( i = 0; i < 10; i++ ) {
    SVT_File_Read( fh, buffer, 6, &read );
    buffer[ 5 ] = 0;
    if ( read != 6 || !SVT_Str_Cmp( buffer, "Trakr") )
      SVT_Log( "  Read Error Size %d != 6, %s != \"Trakr\"", read, buffer );
  }
  SVT_File_Read( fh, buffer, 4, &read );
  buffer[ 3 ] = 0;
  if ( read != 4 || !SVT_Str_Cmp( buffer, "Spy") )
    SVT_Log( "  Read Error Size %d != 4, %s != \"Spy\"", read, buffer );
  SVT_File_Close( fh );

  SVT_Log( "  End Testing FileIO" );
  return true;
}

bool Test_ADCMotors(  )
{
  SVT_ROsd_Draw_Text( 5, 5, "Testing" );
  SVT_ROsd_Draw_Text( 5, 30, "ADC Rapid" );
  SVT_ROsd_ShowSend();

  SVT_Log( "Testing Motor Current" );
  SVT_Sleep( 1000 );

  SVT_ROsd_Clear_Screen();

  int s[ 8 ];

  int i;
  int j;
  for ( j = 0; j < 50; j++ ) {
	  for ( i = 0; i < 8; i++ ) {
		  s[ i ] = JAPI_ReadADC( i );
	  }
	  SVT_Log( "  Key: %x ACD %d, %d, %d, %d, %d, %d, %d, %d", SVT_Key_GetStatus(), s[ 0 ], s[ 1 ], s[ 2 ], s[ 3 ], s[ 4 ], s[ 5 ], s[ 6 ], s[ 7 ] );
	  SVT_Sleep( 100 );
  }

  j = 0;
  while ( j < 5 )
  {
	  for ( i = 0; i < 8; i++ ) {
		  s[ i ] = JAPI_ReadADC( i );
		  SVT_Log( "  ACD %d, %d, %d, %d, %d, %d, %d, %d", s[ 0 ], s[ 1 ], s[ 2 ], s[ 3 ], s[ 4 ], s[ 5 ], s[ 6 ], s[ 7 ] );
	  }

	  SVT_ROsd_Clear_Screen();
	  SVT_ROsd_Draw_Text( 5, 5, "Testing ADCs" );
	  for ( i = 0; i < 8; i++ )
		  SVT_ROsd_Draw_Text( 5 + ( i % 4 ) * 40, 30 + ( i / 4 ) * 25, "%d", s[i] );
	  SVT_ROsd_ShowSend();

      SVT_Sleep( 1000 );

      if ( SVT_Key_GetStatus() == 0 )
    	  j++;
      else
    	  j = 0;
  }
  return true;
}

bool Test_Motors(  )
{
  int i;
  int lc;
  int rc;

  SVT_ROsd_Draw_Text( 5, 5, "Testing" );
  SVT_ROsd_Draw_Text( 5, 30, "Motors" );
  SVT_ROsd_ShowSend();

  SVT_Log( "Testing Motors" );
  SVT_Sleep( 1000 );

  SVT_Log( "  Motors Open" );
  SVT_Motor_Open();


  SVT_Log( "New API" );

  JAPI_SetMotorLeft( 0 );
  JAPI_SetMotorRight( 0 );
  SVT_Sleep( 1000 );

  for ( i = 0; i < 10000; i += 100 ) {
	  JAPI_SetMotorLeft( i );
	  JAPI_SetMotorRight( i );
	  SVT_Sleep( 100 );
  }

  for ( i = 10000; i > -10000; i -= 100 ) {
	  JAPI_SetMotorLeft( i );
	  JAPI_SetMotorRight( i );
	  SVT_Sleep( 100 );
  }

  SVT_Log( "Old API" );

  SVT_Motor_SetLeft( 0 );
  SVT_Motor_SetRight( 0 );
  SVT_Sleep( 1000 );
  lc = SVT_Motor_GetLeftCurrent();
  rc = SVT_Motor_GetRightCurrent();
  SVT_Log( "    LC %d RC %d", lc, rc );
  SVT_ROsd_Draw_Text( 5, 30, "L %d", lc );
  SVT_ROsd_Draw_Text( 80, 30, "R %d", rc );
  SVT_ROsd_ShowSend();
  SVT_Sleep( 1000 );

  SVT_Motor_SetLeft( 100 );
  SVT_Log( "  Motor Set Left 100" );
  SVT_Sleep( 1000 );
  lc = SVT_Motor_GetLeftCurrent();
  rc = SVT_Motor_GetRightCurrent();
  SVT_Log( "    LC %d RC %d", lc, rc );
  SVT_ROsd_Draw_Text( 5, 30, "L %d", lc );
  SVT_ROsd_Draw_Text( 80, 30, "R %d", rc );
  SVT_ROsd_ShowSend();

  SVT_Motor_SetLeft( -100 );
  SVT_Log( "  Motor Set Left -100" );
  SVT_Sleep( 1000 );
  lc = SVT_Motor_GetLeftCurrent();
  rc = SVT_Motor_GetRightCurrent();
  SVT_ROsd_Draw_Text( 5, 30, "L %d", lc );
  SVT_ROsd_Draw_Text( 80, 30, "R %d", rc );
  SVT_ROsd_ShowSend();
  SVT_Log( "    LC %d RC %d", lc, rc );

  SVT_Log( "  Motor Set Left 0" );
  SVT_Motor_SetLeft( 0 );
  SVT_Sleep( 1000 );

  lc = SVT_Motor_GetLeftCurrent();
  rc = SVT_Motor_GetRightCurrent();
  SVT_ROsd_Draw_Text( 5, 30, "L %d", lc );
  SVT_ROsd_Draw_Text( 80, 30, "R %d", rc );
  SVT_ROsd_ShowSend();
  SVT_Log( "    LC %d RC %d", lc, rc );

  SVT_Log( "  Motor Set Right 100" );
  SVT_Motor_SetRight( 100 );
  SVT_Sleep( 1000 );
  lc = SVT_Motor_GetLeftCurrent();
  rc = SVT_Motor_GetRightCurrent();
  SVT_ROsd_Draw_Text( 5, 30, "L %d", lc );
  SVT_ROsd_Draw_Text( 80, 30, "R %d", rc );
  SVT_ROsd_ShowSend();
  SVT_Log( "    LC %d RC %d", lc, rc );

  SVT_Log( "  Motor Set Right -100" );
  SVT_Motor_SetRight( -100 );
  SVT_Sleep( 1000 );
  lc = SVT_Motor_GetLeftCurrent();
  rc = SVT_Motor_GetRightCurrent();
  SVT_ROsd_Draw_Text( 5, 30, "L %d", lc );
  SVT_ROsd_Draw_Text( 80, 30, "R %d", rc );
  SVT_ROsd_ShowSend();
  SVT_Log( "    LC %d RC %d", lc, rc );


  SVT_Log( "  Motor Set Right 0" );
  SVT_Motor_SetRight( 0 );
  SVT_Sleep( 1000 );

  SVT_Log( "  Motor Set Left & Right 100" );
  SVT_Motor_SetRight( 100 );
  SVT_Motor_SetLeft( 100 );
  SVT_Sleep( 1000 );

  lc = SVT_Motor_GetLeftCurrent();
  rc = SVT_Motor_GetRightCurrent();
  SVT_ROsd_Draw_Text( 5, 30, "L %d", lc );
  SVT_ROsd_Draw_Text( 80, 30, "R %d", rc );
  SVT_ROsd_ShowSend();
  SVT_Log( "    LC %d RC %d", lc, rc );


  SVT_Log( "  Motor Set Left & Right -100" );
  SVT_Motor_SetRight( -100 );
  SVT_Motor_SetLeft( -100 );
  SVT_Sleep( 1000 );

  lc = SVT_Motor_GetLeftCurrent();
  rc = SVT_Motor_GetRightCurrent();
  SVT_ROsd_Draw_Text( 5, 30, "L %d", lc );
  SVT_ROsd_Draw_Text( 80, 30, "R %d", rc );
  SVT_ROsd_ShowSend();
  SVT_Log( "    LC %d RC %d", lc, rc );

  SVT_Log( "  Motor Set Left & Right 0" );
  SVT_Motor_SetRight( 0 );
  SVT_Motor_SetLeft( 0 );

  SVT_Sleep( 1000 );

  lc = SVT_Motor_GetLeftCurrent();
  rc = SVT_Motor_GetRightCurrent();
  SVT_ROsd_Draw_Text( 5, 30, "L %d", lc );
  SVT_ROsd_Draw_Text( 80, 30, "R %d", rc );
  SVT_ROsd_ShowSend();
  SVT_Log( "    LC %d RC %d", lc, rc );

  SVT_Motor_Close();

  return true;
}

bool Test_Controls(  )
{
  SVT_Log( "Testing Controls" );

  SVT_Motor_Open();

  TAG_COLOR ColorTP;
  ColorTP.R=0;
  ColorTP.G=0;
  ColorTP.B=0x80;
  ColorTP.Transparent=0;

  SVT_ROsd_Draw_Text( 5, 5, "Testing" );
  SVT_ROsd_Draw_Text( 5, 30, "Controls" );
  SVT_ROsd_ShowSend();
  SVT_Sleep( 1000 );


  SVT_ROsd_Draw_Text( 30, 5, "R B" );
  SVT_ROsd_Draw_Bitmap( 2, 100, 80, &ColorTP);
  SVT_ROsd_ShowSend();

  SVT_Sleep( 1000 );

  int count = 0;

  while ( !( SVT_Key_GetStatus() & SVT_KEY_RIGHT_BACK ) && ( count++ < 50 ))
    SVT_Sleep( 100 );

  if ( count >= 50 )
    SVT_Log( "  Failed to Receive Right Back" );
  else
    SVT_Log( "  Right Back" );

  SVT_ROsd_Draw_Text( 30, 5, "R F" );
  SVT_ROsd_Draw_Bitmap( 2, 100, 30, &ColorTP);
  SVT_ROsd_ShowSend();

  count = 0;

  while ( !( SVT_Key_GetStatus() & SVT_KEY_RIGHT_FORWARD ) && ( count++ < 50 ))
    SVT_Sleep( 100 );

  if ( count >= 50 )
    SVT_Log( "  Failed to Receive Right Forward" );
  else
    SVT_Log( "  Right Forward" );

  // LEFT

  SVT_ROsd_Draw_Text( 30, 5, "L B" );
  SVT_ROsd_Draw_Bitmap( 2, 20, 80, &ColorTP);
  SVT_ROsd_ShowSend();

  count = 0;

  while ( !( SVT_Key_GetStatus() & SVT_KEY_LEFT_BACK ) && ( count++ < 50 ))
    SVT_Sleep( 100 );

  if ( count >= 50 )
    SVT_Log( "  Failed to Receive Left Back" );
  else
    SVT_Log( "  Left Back" );

  SVT_ROsd_Draw_Text( 30, 5, "L F" );
  SVT_ROsd_Draw_Bitmap( 2, 20, 30, &ColorTP);
  SVT_ROsd_ShowSend();

  count = 0;

  while ( !( SVT_Key_GetStatus() & SVT_KEY_LEFT_FORWARD ) && ( count++ < 50 ))
    SVT_Sleep( 100 );

  if ( count >= 50 )
    SVT_Log( "  Failed to Receive Left Forward" );
  else
    SVT_Log( "  Left Forward" );

  SVT_ROsd_Draw_Text( 30, 5, "0ff" );
  SVT_ROsd_ShowSend();

  count = 0;

  while ( ( SVT_Key_GetStatus() ) && ( count++ < 50 ))
     SVT_Sleep( 100 );

   if ( count >= 50 )
     SVT_Log( "  Failed to Return to no key" );
   else
     SVT_Log( "  No Keys" );

   SVT_Motor_Open();

  return true;
}