//============================================================================
// Variable and Initial Conditions Setup
//============================================================================
#include <Servo.h> // Include the servo library

Servo tiltServo;
Servo panServo;
Servo fwrdServo;
int panAngle = 90;                     // Initial panServo angle
int tiltAngle = 90;                    // Initial tiltServo angle
int tolerance = 40;                    // Tolerance range from x,y axies
int centerTolerance = 5;               // Tolerance range to help recenter servos
int imageX = 640;                      // Image width
int imageY = 480;                      // Image hieght

int count = 0;
int pos = 0;
int x, y, d;                           // x=[COG_X], y=[COG_Y], d=[COG_BOX_SIZE]
int var[3];                            // Stores x,y,d
char command[3];                       // Stores characters 'P'-pan, 'T'-tilt, 'D'-distance

void setup()
{
  Serial.begin(115200);                // Open the serial port
  Serial.println("Serial port ready"); // Print on screen
  panServo.attach(9);                  // Attach panServo to pin-9
  tiltServo.attach(10);                // Attach tiltServo to pin-10
  panServo.write(panAngle);            // Initialize panServo to 90
  tiltServo.write(tiltAngle);          // Initialize tiltServo to 90
}
//============================================================================
// Main Loop
//============================================================================
void loop()
{
  if (Serial.available()){              // Do stuff if serial available
//============================================================================
// Data Parsing Algorithm
// RoboRealm serial send sequence:  P[COG_X];T[COG_Y];D[COG_BOX_SIZE];
//============================================================================
    char ch = Serial.read();
    if(ch >= '0' && ch <= '9')	{	// Check if character is a number
	 pos = pos * 10 + ch - '0';	// If yes, accumulate the value
    }
    else if(ch != ';'){                 // Check for delimiter
	 command[count] = ch;
	 pos = 0;
    }
    if( ch == ';' ){ 
        var[count] = pos;
	if(command[0] == 'P'){          //Pan variable
            x = var[0];
        }
        if(command[1] == 'T'){          //Tilt variable
            y = var[1];
        }
        if(command[2] == 'D'){          //Distance(BoxSize) variable
            d = var[2];
        }
        count++;
        pos = 0;   
     }      
//============================================================================
// Servo/Motor Contol Algorithms
//============================================================================
// Print back parsed data
       if(count == 3){     
            for (int i = 0; i < 3; i++) {
              Serial.print(command[i]);Serial.print(var[i]);Serial.print(' ');
              }
// Pan servo algorithm
        if(x < (imageX/2 - tolerance) && x != 0){        // If x left of y-axis move right
          panAngle--;
          panAngle = constrain(panAngle, 0, 180);
          panServo.write(panAngle);
        }
        else if(x > (imageX/2 + tolerance) && x != 0){   // If x right of y-axis move left
          panAngle++;
          panAngle = constrain(panAngle, 0, 180);      
          panServo.write(panAngle);
        }
// Set panServo back to center if target out of view
        if(x == 0){
          if(panAngle < 90 - centerTolerance){
          panAngle++;
          }
          else if(panAngle > 90 + centerTolerance){
            panAngle--;
        }
          panServo.write(panAngle);
        }
// Tilt servo algorithm
        if(y < (imageY/2 - tolerance) && y != 0){        // If y below x-axis move up
          tiltAngle--;
          tiltAngle = constrain(tiltAngle, 0, 180);
          tiltServo.write(tiltAngle);
        }
        else if(y > (imageY/2 + tolerance) && y != 0){   // If y above x-axis move down
          tiltAngle++;
          tiltAngle = constrain(tiltAngle, 0, 180);
          tiltServo.write(tiltAngle);
        }
 // Set tiltServo back to center if target out of view
        if(y == 0){
          if(tiltAngle < 90 - centerTolerance){
          tiltAngle++;
          }
          else if(tiltAngle > 90 + centerTolerance){
            tiltAngle--;
          }
          tiltServo.write(tiltAngle);
        }
        count = 0;                                       // Reset count to retrieve next data packet
       }     
//============================================================================
// END
//============================================================================ 
  }
}
