// Louis Simons
// Localization, Communications, and Pathfinding framework for Heterogenous Robotic Systems
// Last Updated: 8.10.05
//
// Notes:	i and j usually refer to horizontal and vertical positions in an array respectively
//			in image analysis funcitons, stepcount is usually used to speed up processing by using
//				alternating lines (much like tv interlaced scan-lines)

#include "cv.h"
#include "highgui.h"
#include "cvcam.h"
#include <stdio.h>
#include <vector>
#include <list>
#include <algorithm>
#include <bitset>

#define STRICT					// For Serial Communications
#include <tchar.h>
#include <windows.h>
#include <iostream>
#include "Serial.h"

//#include "point.cpp"			// Included through CheifRobot.cpp
#include "position.cpp"			// Contains "position" struct which is used to store x and y coordinates
//#include "grid.cpp"			// Included through CheifRobot.cpp, includes structs "cell" and "cellID"
#include "Dynamic2DArray.cpp"	// Creates an array that can be made larger than commonly restricted by a 2D array (not used)
#include "AStarCell.cpp"		// Contains class "AStarCell" used for holding information related to calculating A* Pathfinding
#include "CheifRobot.cpp"		// Contains class "CheifRobot" used for storing data for representing a robot being tracked

using namespace std;

enum { EOF_Char = 27 };			// For Serial Communications
CSerial serial;
LONG lLastError = ERROR_SUCCESS;

IplImage* curImg;				// Store last frame of camera
IplImage* curHSV;				// Store HSV data for last captured frame
IplImage* curLocal;				// Superimpose color localization data for debug display
IplImage* field;				// Image to draw localization grid with data on

CheifRobot robot1(1, CV_RGB(255,50,50) );		// Represent each robot to be tracked
CheifRobot robot2(2, CV_RGB(25,150,25) );
CheifRobot robot3(3, CV_RGB(210,200,20) );

const int XRES = 320;			// Camera X-resolution
const int YRES = 240;			// Camera Y-Resolution

const double PI = 3.141592;

const int X_DIVISIONS = 33;		// Horizontal resolution of localization grid 
const int Y_DIVISIONS = 25;		// Vertical resolution of localization grid
const double FIELD_SCALE = 2;
			
bool setTrackedRobots = false;	// The folllowing 4 booleans will be used to ensure that some tasks are
bool setNewImages = false;		// only performed the first time cameracallback function (current frame
bool setBeacons = false;		// processing) is called
bool setCells = false;		
int alteredCells = 0;		int maxAlteredCells = 2;	// Keep track of number of cells a robot has entered
														// that should have been obstacle cells and how many
														// are tolerable

//vector<point> beacons;						// Used by findPosition function

cell fieldData[X_DIVISIONS][Y_DIVISIONS];		// Contains the characteristics of each grid cell

cellID goalCells[100];							// Store the coordinates of cells containing goal objects

// -------------------
// Function Prototypes
// -------------------
bool hsvThreshold(point, point, point);
bool withinLoopBoundry(int, int, int, int, int);
point findBodyCenter(IplImage*, point, point, int, int, int, int);
vector<point> findAllBodyCenters(IplImage*, point, point);
position findPosition(IplImage*, vector<point>, point, double, double);

int drawLine (IplImage*, int, int, int, int, CvScalar);
int drawRect (IplImage*, int, int, int, int, CvScalar);
int thickRect (IplImage*, int, int, int, int, CvScalar);
int fillRect (IplImage*, int, int, int, int, CvScalar);
int fillCircle (IplImage*, int, int, double, CvScalar);

int AStarSearch(IplImage*, cell[X_DIVISIONS][Y_DIVISIONS], cellID, cellID);
bool within_field(int,int);

int initialize_cells(cell[X_DIVISIONS][Y_DIVISIONS], IplImage*);
int update_cell(cellID, cell[X_DIVISIONS][Y_DIVISIONS],CELL_STATUS,double);
int draw_field(IplImage*);
int draw_cells(IplImage*, cell[X_DIVISIONS][Y_DIVISIONS]);
cellID locate_cell(point position, cell fieldData[X_DIVISIONS][Y_DIVISIONS]);
int atrophy_knowledge(cell[X_DIVISIONS][Y_DIVISIONS]);

int msg_translate(char[]);

//----------------------------------------------------------------------------
// Purpose: Get pixel data from image files, NOT A POINTER TO THE ACTUAL DATA
// ---------------------------------------------------------------------------
int getR(IplImage* image, int x, int y)
{	return (int)((uchar*)(image->imageData + image->widthStep*y))[x*3 + 2]; }		// <-- Would normally be a pointer to the actual data
int getG(IplImage* image, int x, int y)
{	return (int)((uchar*)(image->imageData + image->widthStep*y))[x*3 + 1]; }
int getB(IplImage* image, int x, int y)
{	return (int)((uchar*)(image->imageData + image->widthStep*y))[x*3 + 0]; }

int getH(IplImage* image, int x, int y)								// Must be performed on the curHSV image (otherwise won't give HSV data
{	return (int)(((uchar*)(curHSV->imageData + curHSV->widthStep*y))[x*3 + 0] * 2.0);   }
int getS(IplImage* image, int x, int y)
{	return (int)(((uchar*)(curHSV->imageData + curHSV->widthStep*y))[x*3 + 1] / 256.0 * 100); }
int getV(IplImage* image, int x, int y)
{   return (int)(((uchar*)(curHSV->imageData + curHSV->widthStep*y))[x*3 + 2] / 256.0 * 100); }

// -------------------------------------------------------------------------
// Function:	cameracallback(IplImage* image)
// Purpose:		Callback for each captured frame
// Parameters:	IplImage* image --> the current frame, given by cvcam
// -------------------------------------------------------------------------
bool noredraw;			// also use by setObstacleGoalCallback
point testPoint;
int offset=0;
bool wasHigh=false;
int numGoalCells=0;	
bool robotStatusChanged = true;
void cameracallback(IplImage* image)
{
	if (!setNewImages)
	{
		CvSize feedSize = {image->width, image->height};
		curImg = cvCreateImage( feedSize, image->depth, image->nChannels);
		curHSV = cvCreateImage( feedSize, image->depth, image->nChannels);
		curLocal = cvCreateImage( feedSize, image->depth, image->nChannels);
		setNewImages = true;
	}

	// Render Live Feed Window (current captured frame)
	cvCopy (image, curImg, NULL);		
	cvFlip( curImg, curImg, 0);
	cvShowImage( "Feed", curImg); 

	// Render Live Feed in HSV Color Space
	cvCvtColor( image, curHSV, CV_RGB2HSV);
	cvFlip( curHSV, curHSV, 0);

	if (!setCells)
	{
		initialize_cells(fieldData,curHSV);	// SETUP OBSTACLES AND GRID
		draw_field(field);
		draw_cells(field, fieldData);
		cvShowImage ( "Grid", field);
		setCells = true;
	}

	// Get Robot Positions and output them in localization after storing
	// TODO: Setup following to use a list containing all robots for greater ease of modification
	cvCopy(image, curLocal);
	cvFlip (curLocal, curLocal, 0);

	if (!setTrackedRobots)
	{
		robot1.setColorTarget(241,80,70);	// Orange Color ID Square
		robot1.setColorRange(10,30,30);

		robot2.setColorTarget(110,80,70);	// Green Color ID Square
		robot2.setColorRange(20,30,30);

		robot3.setColorTarget(183, 85, 70);	// Yellow Color ID Square
		robot3.setColorRange(10,30,30);

		setTrackedRobots = true;
	}
	
	point curLocation1 = findBodyCenter(curHSV, robot1.colorTarget, robot1.colorRange, 0, 0, XRES, YRES);
	point curLocation2 = findBodyCenter(curHSV, robot2.colorTarget, robot2.colorRange, 0, 0, XRES, YRES);
	point curLocation3 = findBodyCenter(curHSV, robot3.colorTarget, robot3.colorRange, 0, 0, XRES, YRES);
	cellID curCell1 = locate_cell(curLocation1, fieldData);
	cellID curCell2 = locate_cell(curLocation2, fieldData); 
	cellID curCell3 = locate_cell(curLocation3, fieldData);
	robot1.updateLocation(curCell1);
	robot2.updateLocation(curCell2);
	robot3.updateLocation(curCell3);
   	
	// Handle Field Grid Updates (modify fieldData array) whenever a robot changes position
	if (robot1.changedLocation() || robot2.changedLocation() || robot3.changedLocation() )
	{
		printf("Robot1 in cell: %d, %d\n", (robot1.curLocation).i, (robot1.curLocation).j);
		printf("Robot2 in cell: %d, %d\n", (robot2.curLocation).i, (robot2.curLocation).j);
		printf("Robot3 in cell: %d, %d\n", (robot3.curLocation).i, (robot3.curLocation).j);
		
		if (fieldData[(robot1.curLocation).i][(robot1.curLocation).j].type == CELL_PERMANENT_OBSTACLE)
		{
			printf("Altered cells: %d\n", alteredCells);
			alteredCells++;
		}
		if (fieldData[(robot2.curLocation).i][(robot2.curLocation).j].type == CELL_PERMANENT_OBSTACLE)
		{
			printf("Altered cells: %d\n", alteredCells);
			alteredCells++;
		}
		if (fieldData[(robot3.curLocation).i][(robot3.curLocation).j].type == CELL_PERMANENT_OBSTACLE)
		{
			printf("Altered cells: %d\n", alteredCells);
			alteredCells++;
		}
		if (alteredCells > maxAlteredCells)
		{
			setCells = false;
			alteredCells = 0;
		}

		update_cell(robot1.lastLocation,fieldData,CELL_EMPTY,100);
		update_cell(robot2.lastLocation,fieldData,CELL_EMPTY,100);
		update_cell(robot3.lastLocation,fieldData,CELL_EMPTY,100);
		update_cell(robot1.curLocation,fieldData,CELL_ROBOT,100);
		update_cell(robot2.curLocation,fieldData,CELL_ROBOT,100);
		update_cell(robot3.curLocation,fieldData,CELL_ROBOT,100);

		if (!noredraw)
		{
		draw_field(field);
		draw_cells(field, fieldData);
		cvShowImage ( "Grid", field);
		}
	}

	cvShowImage("Localized", curLocal);	// Show localized image (with robot locations superimposed)

	// Handle "Show Image" Trackbar
	// Will overlay the current frame over the "Grid" viewport
	if ( cvGetTrackbarPos( "Show Image", "Grid") == 1)
	{
		int scaleX, scaleY;
		for (int x = 0; x < XRES*FIELD_SCALE; x++)
		{
			for (int y = 0; y < YRES*FIELD_SCALE; y++)
			{
				((uchar*)(field->imageData + field->widthStep*y))[x*3 + 2] = 100;
				((uchar*)(field->imageData + field->widthStep*y))[x*3 + 1] = 100;
				((uchar*)(field->imageData + field->widthStep*y))[x*3 + 0] = 100;
			}
		}
		for (int x = 0; x < XRES; x++)
		{
			for (int y = 0; y < YRES; y++)
			{
				scaleX = (int)(x * FIELD_SCALE);
				scaleY = (int)(y * FIELD_SCALE);
				((uchar*)(field->imageData + field->widthStep*scaleY))[scaleX*3 + 2] = getR(image,x,YRES-y);
				((uchar*)(field->imageData + field->widthStep*scaleY))[scaleX*3 + 1] = getG(image,x,YRES-y);
				((uchar*)(field->imageData + field->widthStep*scaleY))[scaleX*3 + 0] = getB(image,x,YRES-y);
			}
		}
		cvShowImage("Grid", field);
		wasHigh=true;
	}
	if ( cvGetTrackbarPos( "Show Image", "Grid") == 0 && wasHigh)
	{
		wasHigh = false;
		CvPoint UL = {0,0};		CvPoint LR = {(int)(XRES*FIELD_SCALE), (int)(YRES*FIELD_SCALE)};
		cvRectangle(field, UL, LR, CV_RGB(255,255,255), CV_FILLED, 8, 0);
		draw_field(field);
		draw_cells(field, fieldData);
		cvShowImage ( "Grid", field);
	}


	// Watch serial port for communication
	DWORD bytesRead = 0;
    CHAR  msgBuffer[20];
    do
    {
        // Read data from the COM-port
        serial.Read(msgBuffer,sizeof(msgBuffer),&bytesRead);
        if (bytesRead > 0)
        {
			msgBuffer[bytesRead] = '\0';
            printf("%s\n", msgBuffer);
			char msg[20];
			sprintf(msg, "%s", msgBuffer);
			msg_translate(msg);
        }
    } while (bytesRead == sizeof(msgBuffer));

}


// ---------------------------------------------------------------------------------
// Function: msg_translate(char[])
// Purpose: Translate a message and dump into a message wrapper once decoded
// Parameters: char[] rawMessage -> 4 chars starting with the message start command
// NOT IMPLEMENTED
// ---------------------------------------------------------------------------------
int msg_translate(char rawMessage[])
{
	

	return 0;
}

// --------------------------------------------------------------------------------
// Function: mousecallback(int event, int x, int y, int flags)
// Purpose: Callback for mouse events in "Feed" window
// Parameters: explained by OpenCV HighGUI documentation
// --------------------------------------------------------------------------------
void mousecallback(int event, int x, int y, int flags)
{
	if (event == CV_EVENT_LBUTTONDOWN)
	{
		CvPoint mousePoint = {x,y};
		printf ("Click @ %d, %d ", mousePoint.x, mousePoint.y);
		
		int r = getR(curImg, x, y);
		int g = getG(curImg, x, y);
		int b = getB(curImg, x, y);

		int h = getH(curHSV, x, y);
		int s = getS(curHSV, x, y);
		int v = getV(curHSV, x, y);

		printf ("R: %d, G: %d, B: %d |", r, g, b);
		printf (" H: %d, S: %d, V: %d \n", h, s, v);
	}
}

// --------------------------------------------------------------------------------
// Function setObstacleGoalCallback(int event, int x, int y, int flags)
// Purpose: Callback for mouse event in grid window, mainly for showing A* paths
// Parameters: explained by OpenCV HighGUI documentation
// --------------------------------------------------------------------------------
void setObstacleGoalCallback(int event, int x, int y, int flags)
{
	if (flags & CV_EVENT_LBUTTONDOWN)
	{
		point mousePoint;
		mousePoint.x = (int)(x/FIELD_SCALE);	mousePoint.y = (int)(y/FIELD_SCALE);
		cellID mouseCell = locate_cell(mousePoint, fieldData);

		update_cell(mouseCell, fieldData, CELL_PERMANENT_OBSTACLE, 100);
		draw_cells(field, fieldData);
		cvShowImage("Grid", field);
	}
	if (flags & CV_EVENT_RBUTTONDOWN)
	{
		point mousePoint;
		mousePoint.x = (int)(x/FIELD_SCALE);	mousePoint.y = (int)(y/FIELD_SCALE);
		cellID mouseCell = locate_cell(mousePoint, fieldData);

		update_cell(mouseCell, fieldData, CELL_EMPTY, 100);
		draw_cells(field, fieldData);
		cvShowImage("Grid", field);
	}

	if (flags & CV_EVENT_FLAG_MBUTTON)  // Hold off on redrawing so you can view the path about to be generated
		noredraw = true;
	else
		noredraw = false;

	if (event == CV_EVENT_MBUTTONDOWN) // Do A* Search
	{
		point mousePoint;
		mousePoint.x = (int)(x/FIELD_SCALE);	mousePoint.y = (int)(y/FIELD_SCALE);
		cellID mouseCell = locate_cell(mousePoint, fieldData);

		update_cell(mouseCell, fieldData, CELL_GOAL_OBJECT, 100);
		draw_cells(field, fieldData);
		cvShowImage("Grid", field);

		numGoalCells=0;											// Keep track of number of goal cells (only 1 for mouse clicking)
		for (int i=0; i<X_DIVISIONS; i++)
			for (int j=0; j<Y_DIVISIONS; j++)
			{
				if (fieldData[i][j].type == CELL_GOAL_OBJECT)
				{
					numGoalCells++;
					goalCells[numGoalCells-1].i=i;
					goalCells[numGoalCells-1].j=j;
				}
			}
		if (numGoalCells>0)
		{
			int heading = AStarSearch(field,fieldData,robot1.curLocation,goalCells[0]);
			cvShowImage("Grid",field);	
			printf("Heading Recommmendation: %d\n", heading);
		}
	}
	if (event == CV_EVENT_MBUTTONUP) // Undo A* Search
	{
		point mousePoint;
		mousePoint.x = (int)(x/FIELD_SCALE);	mousePoint.y = (int)(y/FIELD_SCALE);
		cellID mouseCell = locate_cell(mousePoint, fieldData);
		update_cell(mouseCell, fieldData, CELL_EMPTY, 100);
		
		CvPoint UL = {0,0};		CvPoint LR = {(int)(XRES*FIELD_SCALE), (int)(YRES*FIELD_SCALE)};	// Blank field
		cvRectangle(field, UL, LR, CV_RGB(255,255,255), CV_FILLED, 8, 0);
		
		draw_field(field);
		draw_cells(field, fieldData);
		cvShowImage("Grid", field);
	}
}


// *********************************************************************************
// ---------------------------------------------------------------------------------
// MAIN Function -------------------------------------------------------------------
// ---------------------------------------------------------------------------------
// *********************************************************************************
int main()
{
	// Initialize for grid (initialize_cells called first time camera polled)
	CvSize fieldImageSize = {(int)(XRES*FIELD_SCALE), (int)(YRES*FIELD_SCALE) };
	field = cvCreateImage( fieldImageSize, 8, 3);
	
	cvNamedWindow( "Feed", 1);
	cvNamedWindow( "Localized", 1);
	cvNamedWindow( "Grid", 1);

	cvMoveWindow ( "Feed", 1000, 0);
	cvMoveWindow ( "Localized", 1000, 560);
	cvMoveWindow ( "Grid", 200, 560);

	// Draw grid first time
	draw_field(field);
	draw_cells(field, fieldData);
	cvShowImage ( "Grid", field);

	cvSetMouseCallback( "Feed", mousecallback);
	cvSetMouseCallback( "Grid", setObstacleGoalCallback);	// really callback on any mouse event in "Grid"
	cvCreateTrackbar( "Show Image", "Grid", 0, 1, NULL);

	
	// Initialize COMM
		printf("Beginning COM-access...\n");

		// Attempt to open the serial port
		printf("Opening Serial Port...");
		lLastError = serial.Open(_T("COM1"),0,0,false);
		if (lLastError != ERROR_SUCCESS)
			printf("Unable to open COM-port\n");
		else
			printf("DONE\n");

		// Setup the serial port to (9600,N81)
		printf("Setting up serial port...");
		lLastError = serial.Setup(CSerial::EBaud19200, CSerial::EData8, CSerial::EParNone, CSerial::EStop1);
		if (lLastError != ERROR_SUCCESS)
			printf("Unable to set COM-port setting\n");
		else
			printf("DONE\n");

		// Setup the serial port to use hardware handshaking
		printf("Setting up handshaking...");
		lLastError = serial.SetupHandshaking(CSerial::EHandshakeHardware);
		if (lLastError != ERROR_SUCCESS)
			printf("Unable to set COM-port handshaking\n");
		else
			printf("DONE\n");

		// Use non-blocking reads because the number of bytes to be recieved is unknown
		printf("Setting for non-blocking reads...");
		lLastError = serial.SetupReadTimeouts(CSerial::EReadTimeoutNonblocking);
		if (lLastError != ERROR_SUCCESS)
			printf("Set for non-blocking reads failed\n");
		else
			printf("DONE\n");

		printf("Setting COM-port event mask...");
		lLastError = serial.SetMask(CSerial::EEventBreak |
									CSerial::EEventCTS   |
									CSerial::EEventDSR   |
									CSerial::EEventError |
									CSerial::EEventRing  |
									CSerial::EEventRLSD  |
									CSerial::EEventRecv  );
		if (lLastError != ERROR_SUCCESS)
			printf("Unable to set COM-port event mask\n");
		else
			printf("DONE\n");
	// End COMM Setup

	int numCams = cvcamGetCamerasCount();						// cvCam camera setup, explained in cvcam documentation
	int cvcamone = 0;
	cvcamSetProperty(0, CVCAM_PROP_ENABLE, &cvcamone);
	cvcamSetProperty(0, CVCAM_PROP_RENDER, &cvcamone);
	
	cvcamSetProperty(0, CVCAM_PROP_WINDOW, "Test");

	cvcamSetProperty(0, CVCAM_PROP_CALLBACK, cameracallback);	// will head upwards to our callback which does most everything

	cvcamInit();
	cvcamStart();

	cvWaitKey(0);												// blocks AND performs event handling until a keypress in a window

	cvcamStop();
	cvcamExit();


	

	return 0;
}
// *********************************************************************************
// *********************************************************************************

// --------------------------------------------------------------------------------------------------------------------
// Function: AStarSearch(IplImage* image, cell fieldData[X_DIVISIONS][Y_DIVISIONS], cellID startCell, cellID goalCell)
// Returns: int -> angle for robot to head in to follow path
// Purpose: Preform and draw A* Search, return direction of next cell (0-360, 0 is right, CCW increasing)
// Parameters:	IplImage* image ->	image to draw path on
//				cell fieldData[X...][Y...] ->	pointer (I believe) to the array containing all field information
//				cellID startCell ->	cell to start A* from
//				cellID goalCell	 -> cell to finish A* path
// --------------------------------------------------------------------------------------------------------------------
int AStarSearch(IplImage* image, cell fieldData[X_DIVISIONS][Y_DIVISIONS], cellID startCell, cellID goalCell)
{
	list<AStarCell> openList;
	list<AStarCell> closedList;
	AStarCell startAStarCell(startCell.i, startCell.j);
	startAStarCell.setGHF( 0, abs(goalCell.i-startCell.i) + abs(goalCell.j-startCell.j));
	startAStarCell.setParent(-1,-1);
	openList.push_front(startAStarCell);

	list<AStarCell>::iterator listIterator;
	list<AStarCell>::iterator minIterator;
	
	// Create Temporary Data That Will Be Used Later Loop
	AStarCell tempCell(-1,-1);	int curI; int curJ;

	// Sort by increasing F and grab first
	minIterator = openList.begin();
	AStarCell cellMinF = *minIterator;
	while (cellMinF.i != goalCell.i || cellMinF.j != goalCell.j)
	{
		int minF = openList.begin()->F;
		minIterator = openList.begin();
		for (listIterator = openList.begin(); listIterator != openList.end(); listIterator++)
		{
			if (listIterator->F < minF)
			{
				minF = openList.begin()->F;
				minIterator = listIterator;
			}
		}


	//openList.sort();
	//minIterator = openList.begin();
	cellMinF = *minIterator;
	closedList.push_front(cellMinF);
	openList.erase(minIterator);

		// Add surrounding, valid squares to open list
		curI = cellMinF.i;	curJ = cellMinF.j - 1;	tempCell.setIJ(curI,curJ);																									// Cell Above
		if ( fieldData[curI][curJ].type != CELL_PERMANENT_OBSTACLE && !count(closedList.begin(), closedList.end(), tempCell) && within_field(curI, curJ))	
		{	
			if ( !count(openList.begin(), openList.end(), tempCell) )
			{
				AStarCell newCellToAdd( curI, curJ );
				newCellToAdd.setGHF( cellMinF.G + 10, 10*(abs(newCellToAdd.i - goalCell.i) + abs(newCellToAdd.j - goalCell.j)) );
				newCellToAdd.setParent(cellMinF.i,cellMinF.j);
				openList.push_front(newCellToAdd);
			}
			else
			{
				listIterator = find(openList.begin(), openList.end(), tempCell);
				if ( cellMinF.G + 10 < listIterator->G )
					listIterator->setParent(cellMinF.i,cellMinF.j);
			}
		}	

		curI = cellMinF.i + 1;	curJ = cellMinF.j - 1;	tempCell.setIJ(curI,curJ);																								// Cell Above-Right
		if ( fieldData[curI][curJ].type != CELL_PERMANENT_OBSTACLE && !count(closedList.begin(), closedList.end(), tempCell) && within_field(curI, curJ))	
		{	
			if ( !count(openList.begin(), openList.end(), tempCell) )
			{
				AStarCell newCellToAdd (curI, curJ);
				newCellToAdd.setGHF ( cellMinF.G + 14, 10*(abs(newCellToAdd.i - goalCell.i) + abs(newCellToAdd.j - goalCell.j)) );
				newCellToAdd.setParent(cellMinF.i,cellMinF.j);
				openList.push_front(newCellToAdd);
			}
			else
			{
				listIterator = find(openList.begin(), openList.end(), tempCell);
				if ( cellMinF.G + 14 < listIterator->G )
					listIterator->setParent(cellMinF.i,cellMinF.j);
			}
		}	

		curI = cellMinF.i + 1;	curJ = cellMinF.j;	tempCell.setIJ(curI,curJ);																									// Cell Right
		if ( fieldData[curI][curJ].type != CELL_PERMANENT_OBSTACLE && !count(closedList.begin(), closedList.end(), tempCell) && within_field(curI, curJ))	
		{	
			if ( !count(openList.begin(), openList.end(), tempCell) )
			{
				AStarCell newCellToAdd( curI, curJ);
				newCellToAdd.setGHF( cellMinF.G + 10, 10*(abs(newCellToAdd.i - goalCell.i) + abs(newCellToAdd.j - goalCell.j)) );
				newCellToAdd.setParent(cellMinF.i,cellMinF.j);
				openList.push_front(newCellToAdd);
			}
			else
			{
				listIterator = find(openList.begin(), openList.end(), tempCell);
				if ( cellMinF.G + 10 < listIterator->G )
					listIterator->setParent(cellMinF.i,cellMinF.j);
			}
		}	

		curI = cellMinF.i + 1;	curJ = cellMinF.j + 1;	tempCell.setIJ(curI,curJ);																								// Cell Below-Right
		if ( fieldData[curI][curJ].type != CELL_PERMANENT_OBSTACLE && !count(closedList.begin(), closedList.end(), tempCell) && within_field(curI, curJ))	
		{	
			if ( !count(openList.begin(), openList.end(), tempCell) )
			{
				AStarCell newCellToAdd (curI, curJ);
				newCellToAdd.setGHF ( cellMinF.G + 14, 10*(abs(newCellToAdd.i - goalCell.i) + abs(newCellToAdd.j - goalCell.j)) );
				newCellToAdd.setParent(cellMinF.i,cellMinF.j);
				openList.push_front(newCellToAdd);
			}
			else
			{
				listIterator = find(openList.begin(), openList.end(), tempCell);
				if ( cellMinF.G + 14 < listIterator->G )
					listIterator->setParent(cellMinF.i,cellMinF.j);
			}
		}	

		curI = cellMinF.i;	curJ = cellMinF.j + 1;	tempCell.setIJ(curI,curJ);																									// Cell Below
		if ( fieldData[curI][curJ].type != CELL_PERMANENT_OBSTACLE && !count(closedList.begin(), closedList.end(), tempCell) && within_field(curI, curJ))	
		{	
			if ( !count(openList.begin(), openList.end(), tempCell) )
			{
				AStarCell newCellToAdd( curI, curJ);
				newCellToAdd.setGHF( cellMinF.G + 10, 10*(abs(newCellToAdd.i - goalCell.i) + abs(newCellToAdd.j - goalCell.j)) );
				newCellToAdd.setParent(cellMinF.i,cellMinF.j);
				openList.push_front(newCellToAdd);
			}
			else
			{
				listIterator = find(openList.begin(), openList.end(), tempCell);
				if ( cellMinF.G + 10 < listIterator->G )
					listIterator->setParent(cellMinF.i,cellMinF.j);
			}
		}	

		curI = cellMinF.i - 1;	curJ = cellMinF.j + 1;	tempCell.setIJ(curI,curJ);																								// Cell Below-Left
		if ( fieldData[curI][curJ].type != CELL_PERMANENT_OBSTACLE && !count(closedList.begin(), closedList.end(), tempCell) && within_field(curI, curJ))	
		{	
			if ( !count(openList.begin(), openList.end(), tempCell) )
			{
				AStarCell newCellToAdd (curI, curJ);
				newCellToAdd.setGHF ( cellMinF.G + 14, 10*(abs(newCellToAdd.i - goalCell.i) + abs(newCellToAdd.j - goalCell.j)) );
				newCellToAdd.setParent(cellMinF.i,cellMinF.j);
				openList.push_front(newCellToAdd);
			}
			else
			{
				listIterator = find(openList.begin(), openList.end(), tempCell);
				if ( cellMinF.G + 14 < listIterator->G )
					listIterator->setParent(cellMinF.i,cellMinF.j);
			}
		}	

		curI = cellMinF.i - 1;	curJ = cellMinF.j;	tempCell.setIJ(curI,curJ);																									// Cell Left
		if ( fieldData[curI][curJ].type != CELL_PERMANENT_OBSTACLE && !count(closedList.begin(), closedList.end(), tempCell) && within_field(curI, curJ))	
		{	
			if ( !count(openList.begin(), openList.end(), tempCell) )
			{
				AStarCell newCellToAdd( curI, curJ);
				newCellToAdd.setGHF( cellMinF.G + 10, 10*(abs(newCellToAdd.i - goalCell.i) + abs(newCellToAdd.j - goalCell.j)) );
				newCellToAdd.setParent(cellMinF.i,cellMinF.j);
				openList.push_front(newCellToAdd);
			}
			else
			{
				listIterator = find(openList.begin(), openList.end(), tempCell);
				if ( cellMinF.G + 10 < listIterator->G )
					listIterator->setParent(cellMinF.i,cellMinF.j);
			}
		}	

		curI = cellMinF.i - 1;	curJ = cellMinF.j - 1;	tempCell.setIJ(curI,curJ);																								// Cell Above-Left
		if ( fieldData[curI][curJ].type != CELL_PERMANENT_OBSTACLE && !count(closedList.begin(), closedList.end(), tempCell) && within_field(curI, curJ))	
		{	
			if ( !count(openList.begin(), openList.end(), tempCell) )
			{
			AStarCell newCellToAdd (curI, curJ);
			newCellToAdd.setGHF ( cellMinF.G + 14, 10*(abs(newCellToAdd.i - goalCell.i) + abs(newCellToAdd.j - goalCell.j)) );
			newCellToAdd.setParent(cellMinF.i,cellMinF.j);
			openList.push_front(newCellToAdd);
			}
			else
			{
				listIterator = find(openList.begin(), openList.end(), tempCell);
				if ( cellMinF.G + 14 < listIterator->G )
					listIterator->setParent(cellMinF.i,cellMinF.j);
			}
		}	

	}
	
	listIterator = closedList.begin();
	AStarCell curRouteCell = *listIterator; 

	list<AStarCell> path;				// Used for determining heading
	path.push_back(*listIterator);
	int i,j;
	while (curRouteCell.parentI != startCell.i || curRouteCell.parentJ != startCell.j)
	{
		i = curRouteCell.parentI;
		j = curRouteCell.parentJ;
		fillRect(image, (int)fieldData[i][j].xPos+2, (int)fieldData[i][j].yPos+2, (int)fieldData[i][j].xPos + (int)fieldData[i][j].width - 2, (int)fieldData[i][j].yPos + (int)fieldData[i][j].height - 2, CV_RGB(255,255,0));

		listIterator = find( closedList.begin(), closedList.end(), new AStarCell(curRouteCell.parentI, curRouteCell.parentJ) ); 
		if (listIterator != closedList.end() )
			curRouteCell = *listIterator;
		else
			break;

		path.push_back(*listIterator);

	}
	
	// Determine heading of next cell
	list<AStarCell>::iterator pathIterator = path.begin();
	for (unsigned int i = 0; i < path.size()-1; i++)
		pathIterator++;

	AStarCell cellToHeadNext(pathIterator->i, pathIterator->j);
	int nextI = cellToHeadNext.i;
	int nextJ = cellToHeadNext.j;
	fillCircle(image, (int)fieldData[nextI][nextJ].xPos+(int)fieldData[nextI][nextJ].width/2, (int)fieldData[nextI][nextJ].yPos+(int)fieldData[nextI][nextJ].height/2, fieldData[nextI][nextJ].height/3, CV_RGB(0,0,255) );
		
	int startI = startCell.i;
	int startJ = startCell.j;

	double heading;
	heading = atan2((double)(startI-nextI), (double)(startJ-nextJ)) * 360 / 2 / PI + 90;

	return (int)heading;
}

// --------------------------------------------------------------
// Funciton: within_field(int i, int j)
// Returns:  true if i and j location is inside fieldData array
// Purpose:  Make sure a cell is within the grid for navigation
// --------------------------------------------------------------
bool within_field(int i, int j)
{
	if (i >= 0 && i < X_DIVISIONS && j >= 0 && j < Y_DIVISIONS)
		return true;
	else
		return false;
}

// ----------------------------------------------------------------------------------------
//	Function: initialize_cells(cell fieldData[X_DIVISIONS][Y_DIVISIONS], IplImage* image)
//  Pupose:   Initialize all cell information, set white objects as obstacles
//  Parameters: cell fieldData[X...][Y...] -> array with all field information
//				IplImage* image -> image to grab data from for determining obstacles
// ----------------------------------------------------------------------------------------
int initialize_cells(cell fieldData[X_DIVISIONS][Y_DIVISIONS], IplImage* image)
{  
	for (int i=0; i<X_DIVISIONS; i++)		// Hozontal Progression			Set up each cell's attributes
	{ 
		for (int j=0; j<Y_DIVISIONS; j++)	// Vertical Progression
		{ 
		fieldData[i][j].width= XRES * FIELD_SCALE / X_DIVISIONS;
		fieldData[i][j].height=YRES * FIELD_SCALE / Y_DIVISIONS;
		fieldData[i][j].xPos = i * fieldData[i][j].width;
		fieldData[i][j].yPos=  j * fieldData[i][j].height;
		} 
	}

	// Determine obstacles (White boxes)
	int cellX,cellY, numObstaclePixels, obstacleRatio;
	for (int i=0; i<X_DIVISIONS; i++)
	{
		for (int j=0; j<Y_DIVISIONS; j++)
		{
			point curPoint;

			cellX = i * XRES / X_DIVISIONS;
			cellY = j * YRES / Y_DIVISIONS;

			numObstaclePixels=0;
			for (int x=0; x < XRES * FIELD_SCALE / X_DIVISIONS; x++)
			{
				for (int y=0; y < YRES * FIELD_SCALE / Y_DIVISIONS; y++)
				{
					curPoint.h = getH(image, cellX + x, cellY + y);
					curPoint.s = getS(image, cellX + x, cellY + y);
					curPoint.v = getV(image, cellX + x, cellY + y);

					if ( curPoint.s < 35 && curPoint.v > 70)
						numObstaclePixels++;
				}
			}
			obstacleRatio = numObstaclePixels / (XRES * FIELD_SCALE / X_DIVISIONS) / (YRES * FIELD_SCALE / Y_DIVISIONS) * 100;

			if ( obstacleRatio > 50)
			{
				fieldData[i][j].type =		CELL_PERMANENT_OBSTACLE;
				fieldData[i][j].certainty = 101;
			}
			else
			{
				fieldData[i][j].type =		CELL_EMPTY;
				fieldData[i][j].certainty = 101;
			}
		}
	}
	

	return 0;
}

// -------------------------------------------------------------------------------------------------------------------
// Function: update_cell(cellID curCell,cell fieldData[X_DIVISIONS][Y_DIVISIONS], CELL_STATUS type, double certainty)
// Purpose:  Update a particular cell
// Parameters:	cellID curCell -> cell to update
//				cell fieldData[X...][Y...] -> array with all field information
//				CELL_STATUS type -> type to set cell to, types defined in grid.cpp
//				double certainty -> certainty of cell type, important with atrophy function, not really used
// -------------------------------------------------------------------------------------------------------------------
int update_cell(cellID curCell,cell fieldData[X_DIVISIONS][Y_DIVISIONS], CELL_STATUS type, double certainty)
{
	fieldData[curCell.i][curCell.j].type = type;
	fieldData[curCell.i][curCell.j].certainty = certainty;

	return 0;
}

// -------------------------------------------------------------
// Function: draw_field(IplImage* image)
// Purpose: Output gridlines and dimensions
// Parameters: IplImage* image -> image to draw gridlines on
// -------------------------------------------------------------
int draw_field(IplImage* image)
{
	CvScalar WHITE = CV_RGB(255,255,255);
	CvScalar BLACK = CV_RGB(0,0,0);
	CvScalar GRAY =  CV_RGB(50,50,50);
	CvScalar LIGHT_GRAY = CV_RGB(200,200,200);

	fillRect(image, 0, 0, FIELD_SCALE*XRES, FIELD_SCALE*YRES, LIGHT_GRAY);
	
	for (double i=0; i <= XRES; i+=(double)XRES/(double)X_DIVISIONS)	// Draw And Label Vertical Lines
	{
		drawLine(image, i*FIELD_SCALE, 0, i*FIELD_SCALE, FIELD_SCALE*YRES, BLACK);
	}	
	for (double j=0; j <= YRES; j+=(double)YRES/(double)Y_DIVISIONS)	// Draw And Label Horizontal Lines
	{
		drawLine(image, 0, j*FIELD_SCALE, FIELD_SCALE*XRES, j*FIELD_SCALE, BLACK);
	}

	return 0;
}

// --------------------------------------------------------------------------------
// Function: draw_cells(IplImage* image, cell fieldData[X_DIVISIONS][Y_DIVISIONS])
// Purpose: Color cells and label numbers
// Parameters:	IplImage* -> image to draw cells on
//				cell fieldData[X...][Y...] -> array with field information
// --------------------------------------------------------------------------------
int draw_cells(IplImage* image, cell fieldData[X_DIVISIONS][Y_DIVISIONS])
{
	CvScalar WHITE = CV_RGB(255,255,255);
	CvScalar GREEN = CV_RGB(0,255,0);
	CvScalar BLACK = CV_RGB(0,0,0);
	CvScalar RED   = CV_RGB(255,0,0);
	CvScalar BLUE  = CV_RGB(0,0,255);

	for (int i=0; i<X_DIVISIONS; i++)		// Hozontal Progression
	{
		for (int j=0; j<Y_DIVISIONS; j++)	// Vertical Progression
		{
			CvScalar curCellColor;
			if (fieldData[i][j].type == CELL_UNCERTAIN)				curCellColor = WHITE;
			if (fieldData[i][j].type == CELL_EMPTY)					curCellColor = BLACK;
			if (fieldData[i][j].type == CELL_PERMANENT_OBSTACLE)	curCellColor = RED;
			if (fieldData[i][j].type == CELL_GOAL_OBJECT)			curCellColor = GREEN;
			if (fieldData[i][j].type == CELL_ROBOT)					curCellColor = BLUE;	// Usually overdrawn

			thickRect(image, fieldData[i][j].xPos, fieldData[i][j].yPos, fieldData[i][j].xPos + fieldData[i][j].width, fieldData[i][j].yPos + fieldData[i][j].height, curCellColor);	
		}
	}

	// Color Robot 1
	int i = robot1.curLocation.i;
	int j = robot1.curLocation.j;
	fillRect(image, fieldData[i][j].xPos, fieldData[i][j].yPos, fieldData[i][j].xPos + fieldData[i][j].width, fieldData[i][j].yPos + fieldData[i][j].height, robot1.debugColor);

	// Color Robot 2
	i = robot2.curLocation.i;
	j = robot2.curLocation.j;
	fillRect(image, fieldData[i][j].xPos, fieldData[i][j].yPos, fieldData[i][j].xPos + fieldData[i][j].width, fieldData[i][j].yPos + fieldData[i][j].height, robot2.debugColor);	

	// Color Robot 3
	i = robot3.curLocation.i;
	j = robot3.curLocation.j;
	fillRect(image, fieldData[i][j].xPos, fieldData[i][j].yPos, fieldData[i][j].xPos + fieldData[i][j].width, fieldData[i][j].yPos + fieldData[i][j].height, robot3.debugColor);	

	return 0;
}

// -----------------------------------------------------------------------------------------------------------
// Function: locate_cell(point position, cell fieldData[X_DIVISIONS][Y_DIVISIONS]) 
// Returns: cellID containing the location of the position point within the field information array
// Purpose: determine the cell (position in fieldData) a point is within
// Parameters:	point position -> coordinates of a robot location with respect to the camera's field of view
//				cell fieldData[X...][Y...] -> array with field information
// -----------------------------------------------------------------------------------------------------------
cellID locate_cell(point position, cell fieldData[X_DIVISIONS][Y_DIVISIONS])
{
	int i,j;
	cellID inCell;

	if (position.x < XRES && position.x > 0 && position.y < YRES && position.y > 0)
	{
		inCell.i=position.x * X_DIVISIONS / XRES;
		inCell.j=position.y * Y_DIVISIONS / YRES;
		return inCell;
	}
	else
	{
		inCell.i=-1;
		inCell.j=-1;
		return inCell;
	}
}

// -------------------------------------------------------------------------
// Function: atrophy_knowledge(cell fieldData[X_DIVISIONS][Y_DIVISIONS])
// Purpose: Atrophy All Cell Knowledge except those with certainty > 100
// Parameters: (similar)
// -------------------------------------------------------------------------
int atrophy_knowledge( cell fieldData[X_DIVISIONS][Y_DIVISIONS])
{
	for (int i=0; i<X_DIVISIONS; i++)
	{ 
		for (int j=0; j<Y_DIVISIONS; j++)
		{ 
			int type = fieldData[i][j].type;
			double certainty = fieldData[i][j].certainty;	// Change Second Certainty and increase atrophy rate for DANCE PARTY

			if (certainty > 0 && certainty != 101)
				fieldData[i][j].certainty -= 0.01;
			if (certainty <= 0)
			{
				//fieldData[i][j].type = CELL_UNCERTAIN;
				fieldData[i][j].certainty = 0;
			}
		} 
	}
	return 0;
}

// ----------------------------------------------------------------------------------------------------------------------------------
// Function: hsvThreshold(point testPoint, point targetPoint, point rangePoint)
// Returns:	bool dependant on thresholding
// Purpose: Determine if a point's properties within a range (HSV)
// Parameters:	point testPoint ->		characteristics of point to test
//				point targetPoint ->	characteristics of point to compare to (return true if similar to this point)
//				point rangePoint ->		characteristtics of ranges to either side of target point that are acceptable to threshold
// ----------------------------------------------------------------------------------------------------------------------------------
bool hsvThreshold(point testPoint, point targetPoint, point rangePoint)
{
	if ( withinLoopBoundry(testPoint.h, targetPoint.h, rangePoint.h, 0, 360 ) && abs(testPoint.s - targetPoint.s) <= rangePoint.s && abs(testPoint.v - targetPoint.v) <= rangePoint.v )
		return true;
	else
		return false;
}

// ---------------------------------------------------------------------------------------------------
// Function: withinLoopBoundry(int testVal, int target, int range, int boundLow, int boundHigh)
// Returns: bool true/false dependent on whether or not the target is within cyclic range
// Purpose: Check if a value is within a boundary that is cyclic, used because Hue values are radial
// Example: a clock is a cyclic boundary with boundLow = 0 and boundHigh = 12; if testVal = 2,
//				range = 5 and target = 11, it will return true
// Parameters:	int testVal -> value to check if within boundary
//				int target	-> value to return true if close to
//				int range   -> acceptable distance of testVal from target
//				int boundLow/int boundHigh -> upper and lower values of looping boundary
// ---------------------------------------------------------------------------------------------------
bool withinLoopBoundry(int testVal, int target, int range, int boundLow, int boundHigh)
{
	if ( abs(testVal-target) <= range )
			return true;

	if ( target + range > boundHigh && testVal < target)
		if ( testVal <= target + range - 360 )
			return true;

	if ( target - range < boundLow && testVal > target)
		if ( testVal >= target - range + 360)
			return true;

	return false;

}

// --------------------------------------------------------------------------------------------------------------
// Function: findBodyCenter(IplImage* image, point targetPoint, point rangePoint, int x1, int y1, int x2, int y2)
// Returns: point -> pixel based location of the target in a image
// Purpose: Find the center of a body within a region (using HSV comparison) using average location of acceptable points
// Parameters:	IplImage* image -> image to analyze
//				point targetPoint -> ideal HSV characteristics
//				point rangePoint -> range of HSV characteristics acceptable
//				int x1 / int y1 / int x2 / int y2 -> rectangular area to analyze
// ----------------------------------------------------
point findBodyCenter(IplImage* image, point targetPoint, point rangePoint, int x1, int y1, int x2, int y2)
{
	int sumX = 0;
	int sumY = 0;
	int numPoints = 0;

	switch (offset)
		{
			case 0:
				offset = 1;
			case 1:
				offset = 2;
			case 2:
				offset = 3;
			case 3:
				offset = 0;
		}
	int step = 3;
	for (int xPos=x1+offset; xPos<x2; xPos+=step)
	{
		for (int yPos=y1+offset; yPos<y2; yPos+=step)
		{
			testPoint.h = getH(image,xPos,yPos);		testPoint.s = getS(image,xPos,yPos);		testPoint.v = getV(image,xPos,yPos);
			if ( hsvThreshold(testPoint, targetPoint, rangePoint) )
			{
				((uchar*)(curLocal->imageData + curImg->widthStep*yPos))[xPos*3 + 2] = 0;
				((uchar*)(curLocal->imageData + curImg->widthStep*yPos))[xPos*3 + 1] = 0;
				((uchar*)(curLocal->imageData + curImg->widthStep*yPos))[xPos*3 + 0] = 255;
				sumX = sumX + xPos; 
				sumY = sumY + yPos;
				numPoints++;
			}
		}
	}

	int aveX = 0;
	int aveY = 0;

	if (numPoints > 10)
	{
		aveX = sumX/numPoints;
		aveY = sumY/numPoints;

		CvPoint UL = {aveX,aveY};		CvPoint BR = {aveX+2,aveY+2};
		cvRectangle (curLocal, UL, BR, CV_RGB(255,255,255), CV_FILLED);
		cvShowImage( "Current Color", curLocal);
	}

	point returnPoint;
	returnPoint.x = aveX;	returnPoint.y = aveY;

	return returnPoint;
}


// ---------------------------------------------------------------------------------------------------------------
// Function: findAllBodyCenters(IplImage* image, point targetPoint, point rangePoint)
// Returns: vector<point> containing all bodies found
// Purpose: Find, segment, draw, and locate all bodies of a target color within a threshold (HSV)
// Parameters:	IplImage* image -> image to analyze
//				point targetPoint -> ideal HSV characteristics
//				point rangePoint -> range of HSV characteristics acceptable
// NOTE:	TARGET-O-GRAMS ARE NO LONGER USED IN THE OPENCV LIBRARY, THEY WERE FOR DEBUG PURPOSES ONLY
//			Algorithm based on the work by Cormac Herley (ftp://ftp.research.microsoft.com/pub/tr/tr-2004-01.pdf)
// ----------------------------------------------------------------------------------------------------------------
vector<point> findAllBodyCenters(IplImage* image, point targetPoint, point rangePoint)
{
	// VERTICAL TARGET-O-GRAM AND SEPERATION
	int yLineBase = XRES+10;
	//vline(buffer, yLineBase, 0, Y_RESOLUTION, makecol(0,0,255) );
	int curSum;
	vector<int> vertDivisions;	vertDivisions.push_back(0);
	vector<int> vertSums;		vertSums.push_back(0);
	//for (int yPos=0; yPos<image->height; yPos++)
	int step = 1;
	for (int yPos=0; yPos<image->height; yPos+=step)
	{
		curSum=0;
		//for (int xPos=0; xPos<image->width; xPos++)
		for (int xPos=0; xPos<image->width; xPos+=step)
		{
			testPoint.h = getH(image,xPos,yPos);		testPoint.s = getS(image,xPos,yPos);		testPoint.v = getV(image,xPos,yPos);
			if ( hsvThreshold(testPoint, targetPoint, rangePoint) )
				curSum++;
		}

		vertSums.push_back(curSum);
		//putpixel(buffer, yLineBase + curSum, yPos, makecol(targetPoint.r,targetPoint.g,targetPoint.b) );
	}

	bool wasPositive = (vertSums[0]>5) ? true : false;

	for (int yPos=0; yPos<image->height; yPos++)
	{
		if (vertSums[yPos] > 5)
		{
			wasPositive=true;
		}
		if (wasPositive && vertSums[yPos] < 2)
		{
			vertDivisions.push_back(yPos);
			//hline(buffer, 0, yPos, X_RESOLUTION, makecol(targetPoint.r,targetPoint.g,targetPoint.b));
			drawLine(curLocal, 0, yPos, XRES, yPos, CV_RGB(0,0,255));
			wasPositive = false;
		}
	}

	// HORIZONTAL TARGET-O-GRAM AND SEPERATION
	//int xLineBase = Y_RESOLUTION+10;
	//hline(buffer, 0, xLineBase, X_RESOLUTION, makecol(0,0,255) );
	vector<int> horDivisions;	horDivisions.push_back(0);
	vector<int> horSums;		horSums.push_back(0);
	//for (int xPos = 0; xPos<image->width; xPos++)
	for (int xPos = 0; xPos<image->width; xPos+=step)
	{
		curSum = 0;
		//for (int yPos=0; yPos<image->height; yPos++)
		for (int yPos=0; yPos<image->height; yPos+=step)
		{
			testPoint.h = getH(image,xPos,yPos);		testPoint.s = getS(image,xPos,yPos);		testPoint.v = getV(image,xPos,yPos);
			if ( hsvThreshold(testPoint, targetPoint, rangePoint) )
			{
				curSum++;
				((uchar*)(curLocal->imageData + curLocal->widthStep*yPos))[xPos*3 + 2] = 0;
				((uchar*)(curLocal->imageData + curLocal->widthStep*yPos))[xPos*3 + 1] = 0;
				((uchar*)(curLocal->imageData + curLocal->widthStep*yPos))[xPos*3 + 0] = 255;


			}
		}

		horSums.push_back(curSum);
		//putpixel(buffer, xPos, xLineBase + curSum, makecol(targetPoint.r,targetPoint.g,targetPoint.b) );
	}

	wasPositive = (horSums[0]>5) ? true : false;

	for (int xPos=0; xPos<image->width; xPos++)
	{
		if (horSums[xPos] > 5)
		{
			wasPositive = true;
		}
		if (wasPositive && horSums[xPos] < 2)
		{
			horDivisions.push_back(xPos);
			//vline(buffer, xPos, 0, Y_RESOLUTION, makecol(targetPoint.r,targetPoint.g,targetPoint.b));
			drawLine(curLocal, xPos, 0, xPos, YRES, CV_RGB(0,0,255));
			wasPositive = false;
		}
	}

	// Go through and highlight centers of bodies
	vector<point> bodyCenters;		int numBodies = 0;

	for (int i=0; i<horDivisions.size()-1; i++)
	{
		for (int j=0; j<vertDivisions.size()-1; j++)
		{
			int sumX = 0;
			int sumY = 0;
			int numPoints = 0;
			for (int xPos=horDivisions[i]; xPos<horDivisions[i+1]; xPos++)
			{
				for (int yPos=vertDivisions[j]; yPos<vertDivisions[j+1]; yPos++)
				{
					testPoint.h = getH(image,xPos,yPos);		testPoint.s = getS(image,xPos,yPos);		testPoint.v = getV(image,xPos,yPos);
					if ( hsvThreshold(testPoint, targetPoint, rangePoint) )
					{
						sumX = sumX + xPos; 
						sumY = sumY + yPos;
						numPoints++;
					}
				}
			}

			int aveX;	int aveY;

			if (numPoints > 20)
			{
				aveX = sumX/numPoints;	aveY = sumY/numPoints;

				CvPoint UL = {aveX,aveY};		CvPoint BR = {aveX+2,aveY+2};
				cvRectangle (curLocal, UL, BR, CV_RGB(255,255,255), CV_FILLED);
				cvShowImage( "Current Color", curLocal);

				point newBodyCenter;	newBodyCenter.x = aveX;		newBodyCenter.y = aveY;
				bodyCenters.push_back(newBodyCenter);
				numBodies++;
			}
		}
	}

	return bodyCenters;
}

// --------------------------------------------------------------------------------------------------------------------------------
// Function: findPosition(IplImage* image, vector<point> beaconPoints, point objectPoint, double actualWidth, double actualHeight) 
// Returns: position -> in the same units as actualWidth/Height using the 4 beacons to define coordinate axis
// Purpose: find the quantatative location of a body with respect to 4 beacons
// Parameters:	IplImage* image -> image to analyze
//				vector<point> beaconPoints -> points used to create coordinate axis of position returned
//				point objectPoint -> point to localize
//				double actualWidth / actualHeight -> horizontal and vertical spacing between beacons
//
//		x -> beacon;  o -> object
//
//		x-----------x----->  increasing x component
//      |     | < Vertical Displacement
//      |-----o
//      |  ^ Horizontal Displacement
//		x           x
//      |
//      v increasing y component
// ---------------------------------------------------------------------------------------------------------------------------------
position findPosition(IplImage* image, vector<point> beaconPoints, point objectPoint, double actualWidth, double actualHeight) 
{
	
	// Find upper-left point
	sort(beaconPoints.begin(),beaconPoints.end());
	int upperLeft = 0;

	// Find lower-left point
	int lowerLeft;
	if (beaconPoints[1].x < beaconPoints[2].x)
		lowerLeft = 1;
	else
		lowerLeft = 2;

	// Find upper-right point
	int upperRight;
	if (beaconPoints[1].y > beaconPoints[2].y)
		upperRight = 2;
	else
		upperRight = 1;

	// Find lower-right point
	int lowerRight = 3;

	int upperLeftX = beaconPoints[upperLeft].x;
	int upperLeftY = beaconPoints[upperLeft].y;
	int upperRightX = beaconPoints[upperRight].x;
	int upperRightY = beaconPoints[upperRight].y;
	int lowerLeftX = beaconPoints[lowerLeft].x;
	int lowerLeftY = beaconPoints[lowerLeft].y;
	int lowerRightX = beaconPoints[lowerRight].x;
	int lowerRightY = beaconPoints[lowerRight].y;

	drawRect(curHSV, (upperLeftX+lowerLeftX)/2, (upperLeftY+upperRightY)/2, (upperRightX+lowerRightX)/2, (lowerLeftY+lowerRightY)/2, CV_RGB(0,0,255) );
	
	int rectLeft = (upperLeftX+lowerLeftX)/2; 
	int rectTop = (upperLeftY+upperRightY)/2;

	int rectWidth = (upperRightX+lowerRightX)/2 - rectLeft;
	int rectHeight = (lowerLeftY+lowerRightY)/2 - rectTop;

	double scaledX = (objectPoint.x - rectLeft) * actualWidth  / rectWidth;
	double scaledY = (objectPoint.y - rectTop)  * actualHeight / rectHeight;

	position scaledPos;
	scaledPos.x = scaledX;
	scaledPos.y = scaledY;

	

	
	printf("%.2f,%.2f\n", scaledPos.x, scaledPos.y);

	return  scaledPos;

}


// The following are wrappers for the built in openCV drawing functions
// More information in openCV documentation
int drawLine (IplImage* image, int x1, int y1, int x2, int y2, CvScalar color)
{
	CvPoint point1 = {x1,y1};
	CvPoint point2 = {x2,y2};
	cvLine( image, point1, point2, color);
	return 0;
}

int drawRect (IplImage* image, int x1, int y1, int x2, int y2, CvScalar color)
{
	CvPoint UL = {x1,y1};
	CvPoint LR = {x2,y2};
	cvRectangle( image, UL, LR, color);
	return 0;
}

int thickRect (IplImage* image, int x1, int y1, int x2, int y2, CvScalar color)
{
	CvPoint UL = {x1+5,y1+5};
	CvPoint LR = {x2-5,y2-5};
	cvRectangle( image, UL, LR, color, 2, 8, 0);
	return 0;
}

int fillRect (IplImage* image, int x1, int y1, int x2, int y2, CvScalar color)
{
	CvPoint UL = {x1,y1};
	CvPoint LR = {x2,y2};
	cvRectangle( image, UL, LR, color, CV_FILLED, 8, 0);
	return 0;
}

int fillCircle (IplImage* image, int x, int y, double radius, CvScalar color)
{
	CvPoint center = {x,y};
	cvCircle(image, center, radius, color, CV_FILLED);
	return 0;
}