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

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

#use rcm3000.lib    		//sample library used for this demo
#class auto

#define DS1 6		//led, port G bit 6
#define DS2 7		//led, port G bit 7
#define S2  1		//switch, port G bit 1
#define S3  0		//switch, port G bit 0
#define RIGHT 0
#define LEFT  1
#define CONVERT 40

// Port A pins
#define DIR_R 		0
#define DRIVE_R 	1
#define DRIVE_L 	2
#define DIR_L 		3
#define INITULTRA	4
#define LOOK_D		5
#define LOOK_L		6
#define LOOK_R		7

///////
// change serial buffer name and size here
// for example:  CINBUFSIZE to DINBUFSIZE
///////
#define CINBUFSIZE  15
#define COUTBUFSIZE 15

#define BINBUFSIZE  15
#define BOUTBUFSIZE 15
///////
// change serial baud rate here
///////
#ifndef PORTBBAUD
#define PORTBBAUD 115200
#endif
#ifndef PORTCBAUD
#define PORTCBAUD 19200
#endif

/* Function definitions */
void init_motor();
void init_camera_lcd();
scofunc void drive_motor( int dir_right, int dir_left, int dist_right, int dist_left );
scofunc void track_ball( char *command , int *mx , int *my , int *conf );
void send_command( char *command );
void send_command_lite( char *command , char *buffer );
scofunc void safe_debug_lcd( char *output );
void debug_lcd( char *output);


// Main loop
main()
{
	auto int nIn1, nIn2;
	auto char cOut;
   auto char buffer[64];
   auto char settings[5][32];
   auto int i;
   auto int mx, my, end_x, end_y, start_x, start_y, confidence, pixels;
   auto int conf;
   auto int x;
   auto int dir_r, dir_l, dis_r, dis_l;
   auto int cal;  //Which set of calibration values should be used
   auto int best;		// the highest confidence for the setting, calibration
   auto int motor_enable;

   // Basic init functions
   init_motor();	// Set port A bits, and port F
   init_camera_lcd(); // setup the CMUCam and the LCD

   // Set up the various calibration parameters
   // Base line          Red     Green  Blue
   sprintf( settings[0] , "TC 200 241 20 100 16 18" );
   sprintf( settings[1] , "TC 180 241 20 100 16 18" );
   sprintf( settings[2] , "TC 200 241 20 120 16 18" );
   sprintf( settings[3] , "TC 180 241 20 120 16 18" );
   sprintf( settings[4] , "TC 150 241 20 200 16 18" );
   cal =0;	// Default to the baseline
   motor_enable = 0;	// Start with the motors off.

   // Loop through forever.
   while( 1 )
   {
   	costate {
      	if (!BitRdPortI(PGDR, S2))		//switch S3 press
         {
         	if ( motor_enable )
            {
	            x = wfd safe_debug_lcd( "Motor Disable" );
               motor_enable = 0;
            } else {
               x = wfd safe_debug_lcd( "Motor Enable" );
               motor_enable = 1;
            }
            waitfor ( DelayMs(1000) );		//delay 1 second
         }
      	if (!BitRdPortI(PGDR, S3))		//switch S3 press
         {
          	//If the s3 is pressed, then attempt to adjust the
            // the tracking settings
            cal = 0;
            best = 0;

            // Try them all
            for( i = 0 ; i < 5 ; i++ )
            {
	            x = wfd track_ball( settings[i] , &mx , &my , &conf );
   	         printf ( "C%d: %d\n" , i, conf );

               if ( conf > best )
               {
               	// if the current setting is better than the last
                  // use it.
                  cal = i;
                  best = conf;
               }
            } // loop till all the settings have been tried

            waitfor ( DelayMs(1000) );		//delay 1 second
         }
      	// Get a ball fix.
   		x = wfd track_ball( settings[cal] , &mx , &my , &conf );

         if ( conf > 2)
         {
          	// Strait
            if ( mx > 30 && mx < 50 )
            {
            	x = wfd safe_debug_lcd( "ST" );
            	dir_r = 0;  dis_r = 10;
               dir_l = 0;  dis_l = 10;
            } else if ( mx >10 && mx < 30 ) {
            	x = wfd safe_debug_lcd( "TR" );
            	dir_r = 0;  dis_r = 0;
               dir_l = 0;  dis_l = 10;
            } else if ( mx <= 10 ) {
            	x = wfd safe_debug_lcd( "SR" );
            	dir_r = 1;  dis_r = 10;
               dir_l = 0;  dis_l = 10;
            } else if ( mx >= 70 ) {
            	x = wfd safe_debug_lcd( "SL" );
            	dir_r = 0;  dis_r = 10;
               dir_l = 1;  dis_l = 10;
            } else if ( mx > 50 && mx < 70 ) {
            	x = wfd safe_debug_lcd( "TL" );
            	dir_r = 0;  dis_r = 10;
               dir_l = 0;  dis_l = 0;
            }
            // drive the motor there
            if ( motor_enable )
	            x = wfd drive_motor( dir_r, dir_l, dis_r, dis_l );
         }
   	}
   }

}

void send_command( char *command )
{
	auto int nIn1;
   auto int i, len;

   printf ( "Sending command: %s\n" , command );

   len = strlen( command );
   for ( i = 0 ; i < len ; i++ )
   {
    	serBputc( command[i] );
      printf ( "sss %c" , command[i] );
   }
	serBputc( 13 );
   nIn1 = 'X';
   i = 0;

   // Wait for the :
	while ( nIn1 != ':' )
	{
      while ((nIn1=serBgetc()) == -1);			// Wait for character
  	}
}

// scofunced character get from the camera serial port
scofunc void safe_get( int *in1 )
{
	while ((*in1=serBgetc()) == -1);
}

// put a character into the camera serial port
scofunc void safe_put( char tmp )
{
	serBputc( tmp );
   //printf ( "%c" , tmp );
   waitfor( DelayMs(1) );
}

// Write out to the lcd, put a character to the lcd display
scofunc void safe_debug( char tmp )
{
	serCputc( tmp );
}

// Track, get the middle mass, and return confidence
scofunc void track_ball( char *command , int *mx , int *my , int *conf )
{
	auto int nIn1,x;
   auto int i, len;
   auto char tmp[80];

   //send the tracking command to the camera
   len = strlen( command );

   for ( i = 0 ; i < len ; i++ )
    	x = wfd safe_put( command[i] );

   // Send the I'm done return
	x = wfd safe_put( 13 );

   // Wait for stream to start
   while ( nIn1 != 255 )
   	x = wfd safe_get( &nIn1 );			// Wait for character

   // Skip the next character
  	x = wfd safe_get( &nIn1 );			// Wait for character

   //get mx & my
  	x = wfd safe_get( mx );			// Wait for character
  	x = wfd safe_get( my );			// Wait for character

	//Skip the start x&y and end x&y values
  	x = wfd safe_get( &nIn1 );			// Wait for character
  	x = wfd safe_get( &nIn1 );			// Wait for character
  	x = wfd safe_get( &nIn1 );			// Wait for character
  	x = wfd safe_get( &nIn1 );			// Wait for character

   //Get the confidence
  	x = wfd safe_get( conf );			// Wait for character

   // Skip the pixels value
  	x = wfd safe_get( &nIn1 );			// Wait for character

   {
	   sprintf( tmp , "C:%d X:%d Y:%d\r\n" , *conf , *mx , *my );
   	x = wfd safe_debug_lcd( tmp );
   }
}


// Output string to the LCD
scofunc void safe_debug_lcd( char *output )
{
	auto int nIn1;
   auto int i, len, x;

   // If hooked up to the developement station enable
   //printf ( "DEBUG: %s\n" , output );

   len = strlen( output );
   for ( i = 0 ; i < len ; i++ )
   {
    	x = wfd safe_debug( output[i] );
   }
}


// Write out to LCD, when not in the multi-threaded environment
void debug_lcd( char *output )
{
	auto int nIn1;
   auto int i, len, x;

//   printf ( "DEBUG: %s\n" , output );

   len = strlen( output );
   for ( i = 0 ; i < len ; i++ )
   {
      serCputc( output[i] );
   }
}


// move motors
// direction bits
// distance
// Note this is a blocking function, it will require it to
// 	wait till the distance is traveled till it returns.
scofunc void drive_motor(
	int dir_right, 	// Direction
   int dir_left,
   int dist_right,   // distance in cm
   int dist_left )
{
   // Various temp values needed to progress to the correct location
	auto long reading_r, reading_l; // current encoder value
	auto long final_r, final_l;     // final encoder value
	auto int motor_r, motor_l;		// Active motor
   auto int cancel_motor;

 	/* Set direction bits */
	BitWrPortI( PADR , &PADRShadow , dir_right, DIR_R);		// right motor
	BitWrPortI( PADR , &PADRShadow , dir_left, DIR_L);		// left motor

	// zero quarature values right and left
	qd_zero( RIGHT );
	qd_zero( LEFT );

	// Calculate the distances:
	final_r = dist_right;
	final_l = dist_left;

   // Reverse direction means negitive counter values
   if( dir_right )
     	final_r = final_r * -1;
   if( dir_left )
    	final_l = final_l * -1;

	// Clear the motor avtive flags
	motor_r = 0;
	motor_l = 0;

	/* Start the motors */
	if ( final_r != 0 )
	{
		BitWrPortI( PADR , &PADRShadow , 1, DRIVE_R);		// right motor
		motor_r = 1;
	}
	if ( final_l != 0 )
	{
		BitWrPortI( PADR , &PADRShadow , 1, DRIVE_L);		// left motor
		motor_l = 1;
	}

   //As long as there is an active motor, then keep looping
	while( motor_r || motor_l )
	{
		// check the enconder of each side.
		reading_r = qd_read(RIGHT);
		reading_l = qd_read(LEFT);

      // Stop the right motor
		if(( !dir_right && (reading_r >= final_r) ) ||
          ( dir_right && (reading_r <= final_r) ) )
		{
			motor_r = 0;
			BitWrPortI( PADR , &PADRShadow , 0, DRIVE_R);		// right motor
		}
      // Stop the left motor
		if( ( !dir_left && ( reading_l >= final_l )) ||
	      	( dir_left && ( reading_l <= final_l )) )
		{
			motor_l = 0;
			BitWrPortI( PADR , &PADRShadow , 0, DRIVE_L);		// left motor
		}

      waitfor ( DelayMs(1) );		//delay 1 ms
	}
} // End of the motor execute function

// This function sets up all the various motor bits, and the quadrature encoders.
void init_motor()
{
   qd_init(0);
   qd_init(1);
   brdInit();				//initialize board for this demo

 	/* initialize Port A ... */
	WrPortI(PADR, &PADRShadow, 0x00);               //set to output all high
	WrPortI(SPCR, &SPCRShadow, 0x84);               //ignore SMODE pins, set to output
   BitWrPortI(PADR, &PADRShadow, 0, 0);
   BitWrPortI(PADR, &PADRShadow, 0, 1);
   BitWrPortI(PADR, &PADRShadow, 0, 2);
   BitWrPortI(PADR, &PADRShadow, 0, 3);
   BitWrPortI(PADR, &PADRShadow, 0, 6);
   BitWrPortI(PADR, &PADRShadow, 0, 7);
}

// Init the camera, and get it ready to accept inputs and generate output
void init_camera_lcd()
{
	char buffer[10];

   // Init the various serial ports
 	serBopen(PORTBBAUD);  //set serial rate at 115200
	serCopen(PORTCBAUD);  //set serial rate at 115200

	serBwrFlush();       //flush out the serial write
	serBrdFlush();       //flush out the serial read

	serCwrFlush();       //flush out the serial write
	serCrdFlush();       //flush out the serial read

   // Clear the screen
   buffer[0] = 0x5c;
   buffer[1] = 0x40;
   buffer[2] = 0x20;
   buffer[3] = 0x30;
   buffer[4] = 0;
   debug_lcd( buffer );

   // Setup the camera
   send_command( "" );
   send_command( "GV" );
	send_command( "MM 1" );
   send_command( "PM 1" );
   send_command( "RM 3" );
   send_command( "SW 1 1 80 143" );
}