//////////////////////////////////////////////////////////////////////////////////////////////
//                CMUCAM Operation with VEX and FRC Controllers
//
// Short program to demonstrate operation of the CMUCAM camera.
//////////////////////////////////////////////////////////////////////////////////////////////

// Load the "configuration program" generated camera  settings
#include "CameraConfiguration.h"

task main()
{
	//bMotorReflected[port2] = true;

  int horizontal;
  int vertical;
	int nTotalNumbOfPackets = 0;

	// Wait for the camera to initialize.
	while (nCameraFields[camTrackingStatus] != camStatusTracking)
	{
		// Wait for the camera to begin tracking
		wait1Msec(10);
	}
	wait1Msec(3000);   // Let the camera stabilize
	time1[T1] = 0;     // Reset timer

	// This is the main loop. It will loop forever polling the camera for new data and acting on it.

	while (true)
	{
		// Same packet that was previously received. No processing required. We should wait until new data is received.
		while (nTotalNumbOfPackets == nCameraFields[camNumbTrackingRecords])
		{
			wait1Msec(5); // Wait a little time so that we don't use all CPU time.
		}
		// Store the coordinates of the blob detected by the camera. The center of the screen is normalized to point (0, 0)
		horizontal = nCameraFields[camHorizontal];  //Assigns the Horizontal value to a variable - Helps with debugging
		vertical   = nCameraFields[camVertical];  //Assings the Vertical value to a variable - Helps with debugging
		nTotalNumbOfPackets = nCameraFields[camNumbTrackingRecords];

		//If the camera cannot find an object, it will report a (0,0) coordinate point. This is basically a "no echo" response
		//The code below shows an example of 


		///////////////////////////////////////////////////////////
		//   Place User Code Below to Make the Robot Track an Object
		///////////////////////////////////////////////////////////

		if(horizontal == 0 && vertical == 0)  //No Object Found
		{
			motor[port1] = 0;   //Turn slowly so we can try and find the blob
			motor[port2] = 35;
		}
		if(horizontal != 0 && vertical > -30)  //Object found but not close
		{
			motor[port1] = 30;  //Found a blob and it's not in the upper part of our screen, so move forward
			motor[port2] = 30;
		}
		if(horizontal != 0 && vertical < -30)  //Object close
		{
			motor[port1] = 0;  //Found a blob and it's at the top of screen (as if we are underneath it), so stop.
			motor[port2] = 0;
		}

		///////////////////////////////////////////////////////////
		//   End User Code
		///////////////////////////////////////////////////////////
	}
}