// VideoTool.cpp : Defines the entry point for the console application.

#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <string>
#include <tchar.h>
#include <math.h>
#include <queue>
// TODO: reference additional headers your program requires here

#include "cv.h"
#include "highgui.h"

// ARGH! why is this necessary ??
#define _NO_HMC_WINSOCK_
#include "SmallSocket.h"
#undef _NO_HMC_WINSOCK_

using namespace std;
int colorMode = 0;

// Our Reds!
double RedHueMin = -.1;
double RedHueMax = .5;
double RedSatMin = .12;
double RedSatMax = .99;
double RedValMin = .04;
double RedValMax = .99;
double RedValFactorMax = 2;
double RedValFactorMin = .5;

int RedRMin = 20;

// Our Greens!
double GreenHueMin = -.3;
double GreenHueMax = 1.0;
double GreenSatMin = .16;
double GreenSatMax = .40;
double GreenValMin = .2;
double GreenValMax = .65;
double GreenValFactorMax = 2;
double GreenValFactorMin = .5;

int GreenGMin = 20;
int GreenGMax = 170;
int GreenGMaxBy = 5;

bool foundRed = false;
bool toggle = false; // used to display rectangle results only once per static loaded image


// Our Purples!
double PurpleHueMin = -.4;
double PurpleHueMax = 1.2;
double PurpleSatMin = .00;
double PurpleSatMax = .21;
double PurpleValMin = .01;
double PurpleValMax = .99;
double PurpleValFactorMax = 2;
double PurpleValFactorMin = .5;

int PurpleColorMax = 180;
int PurpleRMin = 20;


// these are the two functions you'll need to write
void RGBtoHSV( double r, double g, double b, double *h, double *s, double *v );
void processRed( IplImage* frame, IplImage* f_p );
void processGreen( IplImage* frame, IplImage* f_p );
void processPurple( IplImage* frame, IplImage* f_p );

void erode(int** dst, int** src, int rows, int cols);
void erode3(int** dst, int** src, int rows, int cols);
void dilate(int** dst, int** src, int rows, int cols);

// set these for calibration of Rectangle position estimation
int knownPixelHeight = 70; // (dummy value)
double knownDistance = 121; // cm (dummy value)
double knownRectWidth = 30.48; // width of the rectangle (real world) in cm

double distCalibration = 9125.0;
double systematicAngleError = -6;

double cameraAngleWidth = 40.0;

bool seeLandmark = false;
double landmarkDist = 0;
double landmarkAngle = 0;

// given the height of a vertical line in pixels, return its distance from the camera
double pixelHeightToDistance(int pixelHeight);

// given the distance to the left edge and the right edge of a known rectangle, determines the distance to the center of the rectangle
double calculateDist(double leftDist, double rightDist);

// given the width of the window in pixels and the horizontal location of the center of the rectangle in pixels returns its angle from the camera
double calculateAngle(int rectLocation);

bool isRectColor(int r, int g, int b, double hue, double sat, double val);

// process the image to find a rectangle landmark and determine its location relative to the camera
void processRectangle(IplImage* frame, IplImage* f_p);

POINT centroid(int** matrix, int value, int rows, int cols);

int connectedComponents(int** matrix, int r, int c, int oldVal, int newVal, int rows, int cols);



void setGlobals();  // the code appears at the very bottom of this file

char wndname[] = "VideoTool";
char wndname2[] = "Undistorted image";
char wndname3[] = "Painted image";

// here, static refers to the fact that these variables
// are local to this file (they're not available in other files,
// which is safer because it allows reuse of their names)
static CvSize imageSize;
static CvPoint old_click_pt;
static CvPoint new_click_pt;
static double framenum;

// global points for drawing boxes, defining loops, etc.
static CvPoint pt1; 
static CvPoint pt2; 
static CvPoint pt3;
static CvPoint pt4;
static CvPoint pt5;
static CvPoint pt6;
static CvPoint pt7;

// global calibration data
static float* M; 
static float* D;

// more global variables
// these are simply recording all the different states you
// can put the program in via the keyboard
static bool bmpRecording = false;
static bool capturing = true;
static bool getImageFromFile = false;
static bool getImageAgainFromFile = false;
static bool getImage0FromVideoBMPs = false;
static bool saveNextImage = false;
static bool quittingTime = false;
static bool detectRed = false;
static bool detectGreen = false;
static bool detectPurple = false;
static bool showing_savedImage = false;

static char* filename = new char[150];
static char* filename2 = new char[150];
static int frameNumber = 0;
static int keyCode = -1;

// pointers to images in memory
static IplImage *frame;
static IplImage* undis_frame;
static IplImage* undis_frame_painted;
static CvArr* undisMap;

// globals for tuning parameters by keyboard...
static int blueValue = 255;

// handle mouse clicks here
void mouse_callback(int event, int x, int y, int flags)
{
	if (event == CV_EVENT_LBUTTONDOWN)
	{
		// cout << "(x,y) = (" << x << "," << y << ")" << endl;

		// reset old_click_pt
		old_click_pt.x = new_click_pt.x;
		old_click_pt.y = new_click_pt.y;

		// get new click point -- note the coordinate change in y
		new_click_pt.x = x;  // coming in from the window system
		new_click_pt.y = imageSize.height-y;  // window system and images have different y axes
	}
}

// print pixel information
void pixelInformation(IplImage* f)
{
	if (f == NULL) return;

	// see if things have changed...
	if (new_click_pt.x != old_click_pt.x || new_click_pt.y != old_click_pt.y) // has it changed?
	{
		// draw a line from old to new
		// cvLine(frame,old_click_pt,new_click_pt,CV_RGB(255,0,0),/*thickness*/1,/*connectivity*/8);
		// set the old = the new
		old_click_pt.x = new_click_pt.x;
		old_click_pt.y = new_click_pt.y;
		// print the RGB values and coordinates of the newly clicked point:
		// note that blue is #0, green is #1, and red is #2 !!

		/* note that this uses the undistorted frame right here... */

		// this is insane!!!
		// see the processRed function for an explanation
		// of why we need "realy"
		int realy = new_click_pt.y;
		if (getImageAgainFromFile || getImageFromFile) 
		{
			realy = imageSize.height - realy;
		}
		uchar* pixel = cvPtr2D(f,realy,new_click_pt.x);

		cout << "(x,y) = (" << new_click_pt.x << "," << new_click_pt.y << ")\n with (r,g,b) = ("
			 << (int)(pixel[2]) << "," << (int)(pixel[1]) << "," << (int)(pixel[0]) << ")";

		double hue,sat,val;
		RGBtoHSV((double)pixel[2],(double)pixel[1],(double)pixel[0],&hue,&sat,&val);

		cout << " and (h,s,v) = (" << hue << "," << sat << "," << val << ")" << endl;

		if(isRectColor(pixel[2], pixel[1], pixel[0], hue, sat, val))
			cout << "\tMe thinks its Green\n";
		else
			cout << "\tMe thinks it ain't Green\n";
	}
}

// the thread for the server
DWORD WINAPI ThreadFunc( LPVOID lpParam ) 
{ 
   	char recvbuf[80];
	char sendbuf[80];

	string t("HI");
	SmallSocket SS(SmallSocket::SERVER);

    sprintf( recvbuf, "Server Started", *(DWORD*)lpParam ); 
    //MessageBox( NULL, szMsg, "ThreadFunc", MB_OK );
	
	while (true)
	{
		SS.receive(recvbuf,80);
		cout << "Received: " << recvbuf << endl;
		
		if (recvbuf[0] == 'q') {
			SS.sendout("q",2);
			// set a variable that tells it to quit...
			quittingTime = true;
		    // cvReleaseCapture( &capture );
			//	cvDestroyWindow("result");
			break;
		}

		if (recvbuf[0] == 'e') {
			// just an example of passing in more information
			// and using sscanf to extract it
			// sscanf(recvbuf,"q %lf %lf %lf ",&savedXforLog,&savedYforLog,&savedTforLog);
			//
			//the connection requires that something come back...
			// note that we're sending two zeros every time here...
			sprintf(sendbuf, "%d %d", 0, 0);
			SS.sendout(sendbuf,80);
		}
		if (recvbuf[0] == 'r') {
			detectRed = !detectRed;
			sprintf(sendbuf, "HI!");
			SS.sendout(sendbuf, 80);
		}
		if (recvbuf[0] == 's') {
			int red = 0;
			if(foundRed)
				red = 1;
			sprintf(sendbuf, "%d\n", red);
			SS.sendout(sendbuf, 80);

			if(red == 1)
				cout << "Sent sees red\n";
			else
				cout << "Sent does not see red\n";
		}
		if(recvbuf[0] == 'l') {
			char yorn;
			if(seeLandmark)
			{
				yorn = 'y';
				//sprintf(sendbuf, "%f %f\n", landmarkDist, landmarkAngle);
				cout << "Sent sees landmark(y)" << endl
					<< "Dist: " << landmarkDist << "\tAngle: " << landmarkAngle << endl << endl;
			}
			else
			{
				yorn = 'n';
				//sprintf(sendbuf, "%f %f\n", -1, 0);
				cout << "Sent no landmark seen (n)" << endl << endl;
				cout << "For the record\t" << "Dist: " << landmarkDist << "\tAngle: " << landmarkAngle << endl << endl;
			}
			sprintf(sendbuf, "%c %f %f\n", yorn, landmarkDist, landmarkAngle);
			SS.sendout(sendbuf, 80);
		}
		// pause for a bit...
		Sleep(10);
	}

    return 0; 

} 


int main( int argc, char** argv )
{
	// in case you want to log things...
	//ofstream logfile("logfile_out.txt");
	//ifstream loginfile("logfile_in.txt");

	double* currentHueMin = &GreenHueMin;
	double* currentHueMax = &GreenHueMax;
	double* currentSatMin = &GreenSatMin;
	double* currentSatMax = &GreenSatMax;
	double* currentValMin = &GreenValMin;
	double* currentValMax = &GreenValMax;
	double* currentValFactorMax = &GreenValFactorMax;
	double* currentValFactorMin = &GreenValFactorMin;

	// set up the globals...
	setGlobals();

	// start the server's thread 
	// make sure that all imageprocessing is in main's thread
	DWORD dwThreadId, dwThrdParam = 1; 
    HANDLE hThread; 
    char szMsg[80];

    hThread = CreateThread( 
        NULL,                        // default security attributes 
        0,                           // use default stack size  
        ThreadFunc,                  // thread function 
        &dwThrdParam,                // argument to thread function 
        0,                           // use default creation flags 
        &dwThreadId);                // returns the thread identifier 
 
    // Check the return value for success. 
    if (hThread == NULL) 
    {
        sprintf( szMsg, "CreateThread failed." ); 
        MessageBox( NULL, szMsg, "main", MB_OK );
    }

	/* 
	 * set up the video windows
	 */
	cvNamedWindow( wndname, CV_WINDOW_AUTOSIZE );
	cvNamedWindow( wndname2, CV_WINDOW_AUTOSIZE );
	cvNamedWindow( wndname3, CV_WINDOW_AUTOSIZE );

	/*
	 * set up the mouse input
	 */
	//cvSetMouseCallback( wndname, mouse_callback );
	cvSetMouseCallback( wndname2, mouse_callback );

	/*
	 * more camera-specific stuff
	 */
	CvCapture* capture = 0;
	capture = cvCaptureFromCAM(0);

	// this is allocating the undis_frame once (it will get written each time)
	// get one frame and then copy it...
	if (cvGrabFrame(capture)) 
	{
		frame = cvRetrieveFrame( capture );
		undis_frame = cvCloneImage(frame);
		undis_frame_painted = cvCloneImage(frame);
		// now let's set up the undistortion parameters
		// note that the docs say 3*wider, but that did not work
		// 3*wider _and_ 3*higher worked...
		undisMap = cvCreateMat( imageSize.width*3, imageSize.height*3, CV_32SC3 );
		// need to release this at the end of the program...
		cvUnDistortInit( frame, undisMap, M, D, 1 );
	}
	else
	{
		cout << "No camera??" << endl;
		capturing = false;
	}

//	getImageFromFile = true;  // added this to work on files when no camera present

	// main loop
	while (!quittingTime)
    {
		if (capturing)
		{
			if( !cvGrabFrame( capture ))   break;
			frame = cvRetrieveFrame( capture );
			if( !frame ) break;

			// cvUnDistortOnce( frame, undis_frame, M, D, 1 );  // might take some extra time...
			cvUnDistort( frame, undis_frame, undisMap, 1 );
			cvReleaseImage(&undis_frame_painted);
			undis_frame_painted = cvCloneImage(undis_frame);
		}
		else if (getImageFromFile || getImageAgainFromFile)
		{
			if (getImageFromFile) {
				// ask for a frame filename
				int i;
				// should factor this path out!!
				if (getImage0FromVideoBMPs) { 
					i = 0;
					getImage0FromVideoBMPs = false;
				}
				else {
					cout << "What image number (in C:\\Program Files\\ERSP\\sample_code\\driver\\VideoTool\\VideoBMPs): ";
					cin >> i;
				}
				sprintf(filename2,"C:\\Program Files\\ERSP\\sample_code\\driver\\VideoTool\\VideoBMPs\\image%05d.bmp",i);
				// we probably need to release the images returned by cvLoadImage somewhere...
				//frame = cvLoadImage(filename2);
				//undis_frame = cvCloneImage(frame);
				cvReleaseImage(&undis_frame);
				undis_frame = cvLoadImage(filename2);
				cvReleaseImage(&undis_frame_painted);
				undis_frame_painted = cvLoadImage(filename2);
				if (undis_frame == NULL) {
					cout << "Could not find that image.\n";
					continue;
				}
				getImageFromFile = false;
				getImageAgainFromFile = true;
				
			}
			else if (getImageAgainFromFile) {
				cvReleaseImage(&undis_frame_painted);
				undis_frame_painted = cvCloneImage(undis_frame);
			}
		}
		else
		{
			;
		}

		// click only on undis_image
		pixelInformation(undis_frame_painted);

		// save the images to a file, if desired
		if (bmpRecording == true && capturing == true)
		{
			sprintf(filename,"C:\\Program Files\\ERSP\\sample_code\\driver\\VideoTool\\BMPs\\image%05d.bmp",frameNumber);
			cout << "Captured single image (dist): " << filename <<  endl;
			cvSaveImage(filename,frame);
			sprintf(filename2,"C:\\Program Files\\ERSP\\sample_code\\driver\\VideoTool\\BMPsUnd\\image%05d.bmp",frameNumber);
			cout << "Captured single image (undist): " << filename2 <<  endl;
			cvSaveImage(filename2,undis_frame);
			++frameNumber;
			//saveNextImage = false;
		    //if (getImageFromFile) {
			//	bmpRecording = false; 
			//}
		}

		// here we can add graphics to each frame
		// in order that they show up on the screen
		// we also decide what to display in this routine
		processRectangle(undis_frame,undis_frame_painted);

/*		if (detectRed == true)
		{
			processRed(undis_frame,undis_frame_painted);
		}
		if (detectGreen == true)
		{
			processGreen(undis_frame,undis_frame_painted);
		}
		if (detectPurple == true)
		{
			processPurple(undis_frame,undis_frame_painted);
		}
*/


		/*
		 * display everything in the windows
		 */
		cvShowImage(wndname, frame);
		cvShowImage(wndname2, undis_frame);
		cvShowImage(wndname3, undis_frame_painted);

		//handle key presses
		/*
		 * quits
		 */
		if( (keyCode = cvWaitKey( 10 )) == ((int)'q') ) {          
			break;                       // quit everything
		}
		/*
		 * we use = and - to increase and decrease blueValue
		 */
		else if (keyCode == (int)'=') {
//			if (blueValue < 251) blueValue += 5;
			GreenGMaxBy += 1;
		}
		else if (keyCode == (int)'-') {
//			if (blueValue > 4) blueValue -= 5;
			GreenGMaxBy -= 1;
		}
		else if (keyCode == (int)'+') {
			GreenGMax += 5;
		}
		else if (keyCode == (int)'_') {
			GreenGMax -= 5;
		}

		else if (keyCode == (int)'1') {
			if(*currentHueMax - .01 >= -.5) *currentHueMax -= .01;
		}
		else if (keyCode == (int)'2') {
			if(*currentHueMax + .01 <= 1.5) *currentHueMax += .01;
		}
		else if (keyCode == (int)'3') {
			if(*currentSatMax - .01 >= 0) *currentSatMax -= .01;
		}
		else if (keyCode == (int)'4') {
			if(*currentSatMax + .01 <= 1.0) *currentSatMax += .01;
		}
		else if (keyCode == (int)'5') {
			if(*currentValMax - .01 >= 0) *currentValMax -= .01;
		}
		else if (keyCode == (int)'6') {
			if(*currentValMax + .01 <= 1.0) *currentValMax += .01;
		}
		else if (keyCode == (int)'!') {
			if(*currentHueMin - .01 >= -.5) *currentHueMin -= .01;
		}
		else if (keyCode == (int)'@') {
			if(*currentHueMin + .01 <= 1.5) *currentHueMin += .01;
		}
		else if (keyCode == (int)'#') {
			if(*currentSatMin - .01 >= 0) *currentSatMin -= .01;
		}
		else if (keyCode == (int)'$') {
			if(*currentSatMin + .01 <= 1.0) *currentSatMin += .01;
		}
		else if (keyCode == (int)'%') {
			if(*currentValMin - .01 >= 0) *currentValMin -= .01;
		}
		else if (keyCode == (int)'^') {
			if(*currentValMin + .01 <= 1.0) *currentValMin += .01;
		}
		else if (keyCode == (int)'0'){
			if(*currentValFactorMax +.1 <=3.0) *currentValFactorMax +=.1;
		}
		else if (keyCode == (int)'9'){
			if(*currentValFactorMax -.1 >=0.0) *currentValFactorMax -=.1;
		}
		else if (keyCode == (int)')'){
			if(*currentValFactorMin +.1 <=1.0) *currentValFactorMin +=.1;
		}
		else if (keyCode == (int)'('){
			if(*currentValFactorMin -.1 >=0.0) *currentValFactorMin -=.1;
		}
		else if (keyCode == (int)'7'){
			if(RedRMin - 5 >= 0) RedRMin -=5;
		}
		else if (keyCode == (int)'8'){
			if(RedRMin + 5 <= 255) RedRMin +=5;
		}
		else if (keyCode == (int)'p') {

			cout << "*currentHueMax: " << *currentHueMax << " *currentHueMin: " << *currentHueMin << endl;
			cout << "*currentSatMax: " << *currentSatMax << " *currentSatMin: " << *currentSatMin << endl;
			cout << "*currentValMax: " << *currentValMax << " *currentValMin: " << *currentValMin << endl;
			cout << "*currentValFactorMax: " << *currentValFactorMax << "*currentValFactorMin" << *currentValFactorMin << endl;
			cout << "GreenGMaxBy: " << GreenGMaxBy << endl;
			cout << "GreenGMax: " << GreenGMax << endl;
		}
		else if (keyCode == (int)'r') { // print out the rectangle stats once inside of the processRect func
			toggle = true;
		}

		/*
		 * records video to bitmaps (files)
		 */
		else if (keyCode == (int)'f') {                           
			if (bmpRecording) {
				cout << "Stopping the writing to bitmaps\n";
				bmpRecording = false;
			} else {
				cout << "Starting to write to bitmaps\n";
				bmpRecording = true;
			}
			showing_savedImage = false;
		} 
		/*
		 * gets image from a file # in ./VideoBMPs
		 */
		else if (keyCode == (int)'g') {
			cout << "Getting image from file.\n";
			capturing = false;
			getImageFromFile = true;
			getImageAgainFromFile = false;
			bmpRecording = false;
			saveNextImage = false;
			showing_savedImage = true;
		} 
		/*
		 * toggle continuous video
		 */
		else if (keyCode == (int)'s') {
			if (capturing) {
				cout << "Stopping continuous capturing.\n";
				capturing = false;
				getImageFromFile = false;
			}
			else {
				cout << "Starting continuous capturing.\n";
				capturing = true;
				getImageFromFile = false;
				getImageAgainFromFile = false;
				/* reset undis_image */
				// note this assumes that everything went well
				// initially with the first captured frame... !!
				cvReleaseImage(&undis_frame);
				undis_frame = cvCloneImage(frame);
				cvReleaseImage(&undis_frame_painted);
				undis_frame_painted = cvCloneImage(frame);
				// not to mention that these transition images have to be released!
			}
			showing_savedImage = false;
		} 
		else if (keyCode == (int)'e') {
			// emulate server behavior
			saveNextImage = true;
			showing_savedImage = false;
		} 
		/*
		 * toggle finding red (and displaying)
		 */
		else if (keyCode == (int)'z') {
			detectRed = !detectRed;
			currentHueMin = &RedHueMin;
			currentHueMax = &RedHueMax;
			currentSatMin = &RedSatMin;
			currentSatMax = &RedSatMax;
			currentValMin = &RedValMin;
			currentValMax = &RedValMax;
			currentValFactorMax = &RedValFactorMax;
			currentValFactorMin = &RedValFactorMin;

			//cout << "red mode" << endl;
		}
		else if (keyCode == (int)'x') {
			detectGreen = !detectGreen;
			currentHueMin = &GreenHueMin;
			currentHueMax = &GreenHueMax;
			currentSatMin = &GreenSatMin;
			currentSatMax = &GreenSatMax;
			currentValMin = &GreenValMin;
			currentValMax = &GreenValMax;
			currentValFactorMax = &GreenValFactorMax;
			currentValFactorMin = &GreenValFactorMin;

			//cout << "green mode" << endl;

		}
		else if (keyCode == (int)'c') {
			detectPurple = !detectPurple;
			currentHueMin = &PurpleHueMin;
			currentHueMax = &PurpleHueMax;
			currentSatMin = &PurpleSatMin;
			currentSatMax = &PurpleSatMax;
			currentValMin = &PurpleValMin;
			currentValMax = &PurpleValMax;
			currentValFactorMax = &PurpleValFactorMax;
			currentValFactorMin = &PurpleValFactorMin;

			//cout << "purple/blue mode" << endl;
		}
		/*
		 * the spacebar advances the image in the VideoBMPs directory
		 */
		else if (keyCode == (int)' ') {
			if (showing_savedImage == true)
			{
				toggle = true;
				int i;
				sscanf(filename2,"C:\\Program Files\\ERSP\\sample_code\\driver\\VideoTool\\VideoBMPs\\image%05d.bmp",&i);
				sprintf(filename2,"C:\\Program Files\\ERSP\\sample_code\\driver\\VideoTool\\VideoBMPs\\image%05d.bmp",++i);
				cvReleaseImage(&undis_frame);
				undis_frame = cvLoadImage(filename2);
				cvReleaseImage(&undis_frame_painted);
				undis_frame_painted = cvLoadImage(filename2);

				cout << "image " << i << endl;

				// wrap around!
				if (undis_frame == NULL) 
				{
					i = 0;
					sprintf(filename2,"C:\\Program Files\\ERSP\\sample_code\\driver\\VideoTool\\VideoBMPs\\image%05d.bmp",i);
					cvReleaseImage(&undis_frame);
					undis_frame = cvLoadImage(filename2);
					cvReleaseImage(&undis_frame_painted);
					undis_frame_painted = cvLoadImage(filename2);
					
					continue;
				}
				getImageAgainFromFile = true;
				getImageFromFile = false;
			}
			/*
			 * initial hit of the space bar...
			 */
			else {
				cout << "No image from file is currently showing..." << endl;
				cout << "Getting image 0 from VideoBMPs.\n";
				capturing = false;
				getImageFromFile = true;
				getImageAgainFromFile = false;
				getImage0FromVideoBMPs = true;
				bmpRecording = false;
				saveNextImage = false;
				showing_savedImage = true;
			}
		}
		
	}

	// release things before quitting
    cvReleaseCapture( &capture );
	cvDestroyWindow("result");

} // end of main

// just in case you'd like to use these...
inline double mymin(double a, double b) { return a<b?a:b; }

inline double mymax(double a, double b) { return a>b?a:b; }

// read about HSV at http://en.wikipedia.org/wiki/HSV_color_space
// and http://www.cs.rit.edu/~ncs/color/t_convert.html#RGB%20to%20HSV%20&%20HSV%20to%20RGB
//
// feel free to grab code from above or elsewhere online, if you'd like
// be sure you understand what RANGE _your_ code for HSV is "returning"
// in the h, s, and v pointers...
void RGBtoHSV( double r, double g, double b, double *h, double *s, double *v )
{
	double maxRGB;
	double minRGB;
	double delta;

	// get max and min
	if(r >= g && r >= b)
	{
		maxRGB = r;
		if(g >= b)
			minRGB = b;
		else
			minRGB = g;
	}
	else if(g >= b)
	{
		maxRGB = g;
		if(r >= b)
			minRGB = b;
		else
			minRGB = r;
	}
	else
	{
		maxRGB = b;
		if(r >= g)
			minRGB = g;
		else
			minRGB = r;
	}

	delta = maxRGB - minRGB;
	*v = maxRGB/255.0;
	*s = delta/255.0;
	
	if(delta == 0)
	{
		*h = -1;
		return;
	}

	if(r == maxRGB)
		*h = (g - b)/delta;
	else if (g == maxRGB)
		*h = (1.0/3.0) + (b-r)/delta;
	else
		*h = (2.0/3.0) + (r-g)/delta;
}

// here is where we seek out the red molding and indicate what we've found
// the input named "frame" is the source location of our pixels
// the "output" named f_p is the destination for drawing
void processGreen(IplImage* frame, IplImage* f_p)
{
    // some useful local variables...
	int r,g,b;
	double hue,sat,val;

	// pixels are handled as uchar*'s
	uchar* color;     // a pixel from the input, "frame"
	uchar* painted;   // a pixel from the output, "painted"
	uchar* painted2;   // a pixel from the output, "painted"

	// this draws a rectangle from pt4 to pt5 with the color
	// specified by the three RGB values in the CV_RGB macro
	// the R, G, and B channels are all 0 to 255...

	// see the "this is insane!" caution below in order to
	// use graphics on the live video stream...
	cvRectangle(f_p, pt4, pt5, CV_RGB(0,0,255), 1); // 1 == thickness
	cvCircle( f_p, pt4, 10,  CV_RGB(255,255,0), /*thickness*/ 1 );

	// This loops through the pixels in the rectangle specified
	// by pt4 and pt5 (see setGlobals for their corners)

	int xlow = (int)mymin(pt4.x,pt5.x);
	int xhi = (int)mymax(pt4.x,pt5.x);
	int ylow = (int)mymin(pt4.y,pt5.y);
	int yhi = (int)mymax(pt4.y,pt5.y);

	int matrix[250][250];
	assert(xhi - xlow < 250 && yhi - ylow < 250);

	/*for(int i = 0; i < 25; i++)
		redTop[i] = redBottom[i] = -2;*/


	int y, realy;

	/*int ** matrix;
	matrix= new int*[yhi-ylow];//[xhi-xlow];
	for(int j = 0; j < yhi-ylow; j++)
		matrix[j] = new int[xhi-xlow];*/


	for (int x = xlow; x<xhi; x++)		//the x direction goes left to right       
	{
		for (int y = ylow; y<yhi; y++)  //the y direction goes top to bottom
		{
			// this is insane!!!
			// unfortunately, access to the pixels is different depending on
			// the source of the pixels: static images and video reverse their
			// y orientations

			// we handle this here...
			realy = y;
			if (getImageAgainFromFile || getImageFromFile) 
			{
				realy = imageSize.height - realy;  // reverse if static image
			}

			// note that we need to use "realy" in each of these cases
			color = cvPtr2D(frame,realy,x);    // get the pixel we care about
			painted = cvPtr2D(f_p,realy,x);    // get the pixel we (might) change

			r = (int)(color[2]);  // the red component of the pixel we care about
			g = (int)(color[1]);  // the green component of the pixel we care about
			b = (int)(color[0]);  // the blue component of the pixel we care about

			// you might want to call this here!
			RGBtoHSV(r,g,b,&hue,&sat,&val);

			// draw on each pixel that we think is "red"
			//if(hue < RedHueMax && hue > RedHueMin && sat < RedSatMax && sat > RedSatMin && val < RedValMax && val > RedValMin)
			// set hue, and make saturation allowable within a range of the value.
			if(g>r && g>b && g > GreenGMin
			&& hue < GreenHueMax && hue < GreenHueMax
			&& sat > GreenSatMin && sat < GreenSatMax
			&& val > GreenValMin && val < GreenValMax /*&& sat < val*(GreenValFactorMax) && sat > val*(GreenValFactorMin)*/ )
			{
				painted[0] = 0;  // sets blue to 0
				painted[1] = 255;    // sets green to 0
				painted[2] = 0;    // sets red to 255
				//cout << "y = " << y << "\t x = " << x << endl;
				//cout << "r = " << y-ylow << "\t x-xlow" << x-xlow << endl;
				matrix[y-ylow][x-xlow]=1;
				//cout << "matrix has: " << matrix[y-ylow][x-xlow] << endl;
			}
			else
			{
				matrix[y-ylow][x-xlow]=0;
			}
		}
	}
}

void processPurple(IplImage* frame, IplImage* f_p)
{
    // some useful local variables...
	int r,g,b;
	double hue,sat,val;

	// pixels are handled as uchar*'s
	uchar* color;     // a pixel from the input, "frame"
	uchar* painted;   // a pixel from the output, "painted"
	uchar* painted2;   // a pixel from the output, "painted"

	// this draws a rectangle from pt4 to pt5 with the color
	// specified by the three RGB values in the CV_RGB macro
	// the R, G, and B channels are all 0 to 255...

	// see the "this is insane!" caution below in order to
	// use graphics on the live video stream...
	cvRectangle(f_p, pt4, pt5, CV_RGB(0,0,255), 1); // 1 == thickness
	cvCircle( f_p, pt4, 10,  CV_RGB(255,255,0), /*thickness*/ 1 );

	// This loops through the pixels in the rectangle specified
	// by pt4 and pt5 (see setGlobals for their corners)

	int xlow = (int)mymin(pt4.x,pt5.x);
	int xhi = (int)mymax(pt4.x,pt5.x);
	int ylow = (int)mymin(pt4.y,pt5.y);
	int yhi = (int)mymax(pt4.y,pt5.y);

	int matrix[250][250];
	assert(xhi - xlow < 250 && yhi - ylow < 250);

	/*for(int i = 0; i < 25; i++)
		redTop[i] = redBottom[i] = -2;*/


	int y, realy;

	/*int ** matrix;
	matrix= new int*[yhi-ylow];//[xhi-xlow];
	for(int j = 0; j < yhi-ylow; j++)
		matrix[j] = new int[xhi-xlow];*/


	for (int x = xlow; x<xhi; x++)		//the x direction goes left to right       
	{
		for (int y = ylow; y<yhi; y++)  //the y direction goes top to bottom
		{
			// this is insane!!!
			// unfortunately, access to the pixels is different depending on
			// the source of the pixels: static images and video reverse their
			// y orientations

			// we handle this here...
			realy = y;
			if (getImageAgainFromFile || getImageFromFile) 
			{
				realy = imageSize.height - realy;  // reverse if static image
			}

			// note that we need to use "realy" in each of these cases
			color = cvPtr2D(frame,realy,x);    // get the pixel we care about
			painted = cvPtr2D(f_p,realy,x);    // get the pixel we (might) change

			r = (int)(color[2]);  // the red component of the pixel we care about
			g = (int)(color[1]);  // the green component of the pixel we care about
			b = (int)(color[0]);  // the blue component of the pixel we care about

			// you might want to call this here!
			RGBtoHSV(r,g,b,&hue,&sat,&val);

			// draw on each pixel that we think is "red"
			//if(hue < RedHueMax && hue > RedHueMin && sat < RedSatMax && sat > RedSatMin && val < RedValMax && val > RedValMin)
			// set hue, and make saturation allowable within a range of the value.
			if((b<PurpleColorMax && r<PurpleColorMax && g<PurpleColorMax) && b+10>r && b+10>g && hue < PurpleHueMax && hue > PurpleHueMin && sat > PurpleSatMin && /*sat < val*(PurpleValFactorMax) && sat > val*(PurpleValFactorMin) && */val < PurpleValMax && val > PurpleValMin)
			{
				painted[0] = 255;  // sets blue to 0
				painted[1] = 0;    // sets green to 0
				painted[2] = 0;    // sets red to 255
				//cout << "y = " << y << "\t x = " << x << endl;
				//cout << "r = " << y-ylow << "\t x-xlow" << x-xlow << endl;
				matrix[y-ylow][x-xlow]=1;
				//cout << "matrix has: " << matrix[y-ylow][x-xlow] << endl;
			}
			else
			{
				matrix[y-ylow][x-xlow]=0;
			}
		}
	}
}

// here is where we seek out the red molding and indicate what we've found
// the input named "frame" is the source location of our pixels
// the "output" named f_p is the destination for drawing
void processRed(IplImage* frame, IplImage* f_p)
{
    // some useful local variables...
	int r,g,b;
	double hue,sat,val;

	// pixels are handled as uchar*'s
	uchar* color;     // a pixel from the input, "frame"
	uchar* painted;   // a pixel from the output, "painted"
	uchar* painted2;   // a pixel from the output, "painted"

	// this draws a rectangle from pt4 to pt5 with the color
	// specified by the three RGB values in the CV_RGB macro
	// the R, G, and B channels are all 0 to 255...

	// see the "this is insane!" caution below in order to
	// use graphics on the live video stream...
	cvRectangle(f_p, pt4, pt5, CV_RGB(0,0,255), 1); // 1 == thickness
	cvCircle( f_p, pt4, 10,  CV_RGB(255,255,0), /*thickness*/ 1 );

	// This loops through the pixels in the rectangle specified
	// by pt4 and pt5 (see setGlobals for their corners)

	int xlow = (int)mymin(pt4.x,pt5.x);
	int xhi = (int)mymax(pt4.x,pt5.x);
	int ylow = (int)mymin(pt4.y,pt5.y);
	int yhi = (int)mymax(pt4.y,pt5.y);

	int matrix[250][250];
	assert(xhi - xlow < 250 && yhi - ylow < 250);
	int colWidth = 10;
	int sampleCols = (xhi-xlow)/colWidth;
	int redTop[25];
	int redBottom[25];
	int redMiddle[25];

	/*for(int i = 0; i < 25; i++)
		redTop[i] = redBottom[i] = -2;*/

	bool foundTop;
	bool foundBottom;
	int temp;
	int temp2;
	int count;
	int i;
	int j;
	int z;
	int y, realy;

	/*int ** matrix;
	matrix= new int*[yhi-ylow];//[xhi-xlow];
	for(int j = 0; j < yhi-ylow; j++)
		matrix[j] = new int[xhi-xlow];*/


	for (int x = xlow; x<xhi; x++)		//the x direction goes left to right       
	{
		for (int y = ylow; y<yhi; y++)  //the y direction goes top to bottom
		{
			// this is insane!!!
			// unfortunately, access to the pixels is different depending on
			// the source of the pixels: static images and video reverse their
			// y orientations

			// we handle this here...
			realy = y;
			if (getImageAgainFromFile || getImageFromFile) 
			{
				realy = imageSize.height - realy;  // reverse if static image
			}

			// note that we need to use "realy" in each of these cases
			color = cvPtr2D(frame,realy,x);    // get the pixel we care about
			painted = cvPtr2D(f_p,realy,x);    // get the pixel we (might) change

			r = (int)(color[2]);  // the red component of the pixel we care about
			g = (int)(color[1]);  // the green component of the pixel we care about
			b = (int)(color[0]);  // the blue component of the pixel we care about

			// you might want to call this here!
			RGBtoHSV(r,g,b,&hue,&sat,&val);

			// draw on each pixel that we think is "red"
			//if(hue < RedHueMax && hue > RedHueMin && sat < RedSatMax && sat > RedSatMin && val < RedValMax && val > RedValMin)
			// set hue, and make saturation allowable within a range of the value.
			if(/*r > RedRMin*/r>g && r>b && hue < RedHueMax && hue > RedHueMin && sat > RedSatMin && sat < val*(RedValFactorMax) && sat > val*(RedValFactorMin) && val < RedValMax && val > RedValMin)
			{
				painted[0] = 0;  // sets blue to 0
				painted[1] = 0;    // sets green to 0
				painted[2] = blueValue;    // sets red to 255
				//cout << "y = " << y << "\t x = " << x << endl;
				//cout << "r = " << y-ylow << "\t x-xlow" << x-xlow << endl;
				matrix[y-ylow][x-xlow]=1;
				//cout << "matrix has: " << matrix[y-ylow][x-xlow] << endl;
			}
			else
			{
				matrix[y-ylow][x-xlow]=0;
			}
		}
	}


	// look through sample columns, trying to find the top yValue of a dense red region, and
	// then yValue for the end of that dense region.
	for(i = 0; i < xhi-xlow; i+=colWidth) // col (x's)
	{
		foundTop = false;
		foundBottom = false;
		for(j = 0; j < yhi-ylow; j++) // row (y's)
		{
			if(!foundTop && matrix[j][i] != 0) // haven't found a top yet and this is red
			{
				
				count = 0; // a count of how many of the next 5 pixels are red
				for(z = j+1; z <= j+5 && z<yhi-ylow; z++)
				{
					if(matrix[z][i] != 0) // is red
						count++; 
				}
				if(count>=4)
				{
					foundTop = true;
					redTop[i/colWidth] = j; // stores the y value of the "top" of the red
				}
			}
			// looking for a bottom and this is red
			else if(foundTop && !foundBottom && matrix[j][i]!=0) 
			{
				count = 0; // a count of how many of the next 5 pixels are NOT red
				for(z = j+1; z <= j+5 && z<yhi-ylow; z++)
				{
					if(matrix[z][i]==0) // these are NOT red
						count++;
				}
				if(count==5)
				{
					foundBottom = true;
					redBottom[i/colWidth] = j; // store the y value of the "bottom" of the red
				}
			}
		}
		// no top found, so store a signal
		if(!foundTop)
		{
			redTop[i/colWidth] = -1;
		}
		// no bottom found, so store a signal
		if(!foundBottom)
		{
			redBottom[i/colWidth] = -1;
		}
	}

	// now if we found both a top and bottom, store the avg of the two
	for(int e = 0; e < sampleCols; e++)
	{
		if(redTop[e] == -1 || redBottom[e] == -1) // not a valid column, so signal
			redMiddle[e] = -1;
		else
			redMiddle[e] = (redTop[e] + redBottom[e])/2; // this is the avg of the top and bottom
	}

	// now calculate a linear regression of the midpoints
	int xVal;
	int yVal;
	int yValOld;
	int sumX = 0;
	int sumY = 0;
	int sumXY = 0;
	int sumXSquared = 0;
	int n = 0;

	// trying to calculate best a,b for y = a + bx
	double aVal;
	double bVal;
	
	// calculating the needed sums for a linear regression
	for(int e = 0; e < sampleCols; e++)
	{
		if(redMiddle[e] != -1)
		{
			xVal = e*colWidth + xlow;
			yVal = redMiddle[e] + ylow;
			if(n>=1 && (yVal - yValOld > 25 || yVal - yValOld < -25)) // this is an outlier
				break;												// ignore it in the stats
			
			sumX		+= xVal;
			sumY		+= yVal;
			sumXY		+= xVal*yVal;
			sumXSquared += xVal*xVal;

			yValOld = yVal;
			n++;
		}
	}
	// using the equation listed on (http://cne.gmu.edu/modules/dau/stat/regression/linregsn/nreg_4_frm.html)
	bVal = (sumXY - (sumX * sumY)/double(n)) / double(sumXSquared - (sumX * sumX)/double(n));
	aVal = sumY/double(n) - bVal * sumX/double(n);

	CvPoint linePt1, linePt2;

	// don't fit a line if there are less than 3 valid points
	if(n >= 3)
	{
		foundRed = true;
		linePt1.x = xlow;
		linePt1.y = int(aVal + bVal * xlow);

		linePt2.x = xhi;
		linePt2.y = int(aVal + bVal * xhi);

		// fix video insanity
		if (getImageAgainFromFile || getImageFromFile) 
		{
			linePt1.y = imageSize.height - linePt1.y;  // reverse if static image
			linePt2.y = imageSize.height - linePt2.y;  // reverse if static image
		}

		cvLine(f_p, linePt1, linePt2,CV_RGB(0,255,0),1,8);      // 1 == thickness, 8 = connectedness

	}
	else
		foundRed = false;


	// this is just to see the process (top, middle, and bottom pixels turned green)
	for(int e = 0; e<sampleCols; e++)
	{
		// draw the detected tops
		y = redTop[e]+ylow;
		realy = y;
		if (getImageAgainFromFile || getImageFromFile) 
		{
			realy = imageSize.height - realy;  // reverse if static image
			//cout << "realy: " << realy << endl;
		}
		if(redTop[e] != -1)
		{
			painted = cvPtr2D(f_p,realy,e*colWidth+xlow);    // get the pixel we (might) change
			painted[0] = 0;  // sets blue to 0
			painted[1] = blueValue;    // sets green to 255
			painted[2] = 0;    // sets red to 0
		}

		// draw the detected bottoms
		y = redBottom[e]+ylow;
		realy = y;
		if (getImageAgainFromFile || getImageFromFile) 
		{
			realy = imageSize.height - realy;  // reverse if static image
		}
		if(redBottom[e] != -1)
		{
			painted2 = cvPtr2D(f_p,realy,e*colWidth+xlow);    // get the pixel we (might) change
			painted2[0] = 0;  // sets blue to 0
			painted2[1] = blueValue;    // sets green to 255
			painted2[2] = 0;    // sets red to 0
		}

		// draw the calculated middles
		y = redMiddle[e]+ylow;
		realy = y;
		if (getImageAgainFromFile || getImageFromFile) 
		{
			realy = imageSize.height - realy;  // reverse if static image
		}
		if(redMiddle[e] != -1)
		{
			painted2 = cvPtr2D(f_p,realy,e*colWidth+xlow);    // get the pixel we (might) change
			painted2[0] = 0;  // sets blue to 0
			painted2[1] = blueValue;    // sets green to 255
			painted2[2] = 0;    // sets red to 0
		}
	}
	
//	delete[][] matrix;
//	delete[] redTop;
//	delete[] redBottom;

//  	cout << "done\n";
}

// given the height of a vertical line in pixels, return its distance from the camera
double pixelHeightToDistance(int pixelHeight)
{
	//return (double(knownPixelHeight) / pixelHeight) * knownDistance;

	// empiraclly determined
	return double(distCalibration) / pixelHeight; 
}

// given the distance to the left edge and the right edge of a known rectangle, determines the distance to the center of the rectangle
double calculateDist(double leftDist, double rightDist)
{
	return sqrt( 0.5 * (leftDist * leftDist + rightDist*rightDist - (0.5*knownRectWidth*knownRectWidth))) ;
}

// given the width of the window in pixels and the horizontal location of the center of the rectangle in pixels returns its angle from the camera
double calculateAngle(int rectLocation)
{
	return -1*(cameraAngleWidth/2.0) * (1 - (2.0 * rectLocation)/imageSize.width) + systematicAngleError;	
}

bool isRectColor(int r, int g, int b, double hue, double sat, double val)
{
	return (g>r + GreenGMaxBy && g>b + GreenGMaxBy && g > GreenGMin && g < GreenGMax
			&& hue > GreenHueMin && hue < GreenHueMax
			&& sat > GreenSatMin && sat < GreenSatMax
			&& val > GreenValMin && val < GreenValMax  /*&& sat < val*(GreenValFactorMax) && sat > val*(GreenValFactorMin)*/ );
}

// process the image to find a rectangle landmark and determine its location relative to the camera
void processRectangle(IplImage* frame, IplImage* f_p)
{
	// some useful local variables...
	int r,g,b;
	double hue,sat,val;

	// pixels are handled as uchar*'s
	uchar* color;     // a pixel from the input, "frame"
	uchar* painted;   // a pixel from the output, "painted"
	uchar* painted2;   // a pixel from the output, "painted"

	// this draws a rectangle from pt4 to pt5 with the color
	// specified by the three RGB values in the CV_RGB macro
	// the R, G, and B channels are all 0 to 255...

	// see the "this is insane!" caution below in order to
	// use graphics on the live video stream...

	// This loops through the pixels in the rectangle specified
	// by pt4 and pt5 (see setGlobals for their corners)

	// 320x240

	int xlow = 1;
	int xhi = imageSize.width -1;
	int ylow = 1;
	int yhi = imageSize.height-1;
/*
	int xlow = 50;
	int xhi = 100;
	int ylow = 50;
	int yhi = 100;


	int xlow = (int)mymin(pt4.x,pt5.x);
	int xhi = (int)mymax(pt4.x,pt5.x);
	int ylow = (int)mymin(pt4.y,pt5.y);
	int yhi = (int)mymax(pt4.y,pt5.y);
*/
/*	if(detectGreen)
	{
		cout << "xlow: " << xlow << endl;
		cout << "ylow: " << ylow << endl;
		cout << "xhi: " << xhi << endl;
		cout << "yhi: " << yhi << endl;

	}
*/

	int** matrix = new int*[yhi-ylow];
	for(int y = 0; y < yhi-ylow; ++y)
	{
		matrix[y] = new int[xhi-xlow];
	}
	

	int realy;

	for (int x = xlow; x<xhi; x++)		//the x direction goes left to right       
	{
		for (int y = ylow; y<yhi; y++)  //the y direction goes top to bottom
		{
			// this is insane!!!
			// unfortunately, access to the pixels is different depending on
			// the source of the pixels: static images and video reverse their
			// y orientations

			// we handle this here...
			realy = y;
			if (getImageAgainFromFile || getImageFromFile) 
			{
				realy = imageSize.height - realy;  // reverse if static image
			}

			// note that we need to use "realy" in each of these cases
			color = cvPtr2D(frame,realy,x);    // get the pixel we care about
			painted = cvPtr2D(f_p,realy,x);    // get the pixel we (might) change

			r = (int)(color[2]);  // the red component of the pixel we care about
			g = (int)(color[1]);  // the green component of the pixel we care about
			b = (int)(color[0]);  // the blue component of the pixel we care about

			// you might want to call this here!
			RGBtoHSV(r,g,b,&hue,&sat,&val);

			// draw on each pixel that we think is "red"
			//if(hue < RedHueMax && hue > RedHueMin && sat < RedSatMax && sat > RedSatMin && val < RedValMax && val > RedValMin)
			// set hue, and make saturation allowable within a range of the value.
			if(isRectColor(r,g,b,hue,sat,val))
			{
/*				painted[0] = 0;  // sets blue to 0
				painted[1] = 255;    // sets green to 0
				painted[2] = 0;    // sets red to 255
*/
				//cout << "y = " << y << "\t x = " << x << endl;
				//cout << "r = " << y-ylow << "\t x-xlow" << x-xlow << endl;
				matrix[y-ylow][x-xlow]=1;
				//cout << "matrix has: " << matrix[y-ylow][x-xlow] << endl;
			}
			else
			{
				matrix[y-ylow][x-xlow]=0;
			}
		}
	}

/*	if(detectGreen)
	{
	for(int r = 0; r < yhi - ylow; r++)
	{
		cout << "r: " << r <<endl;
		for(int c = 0; c < xhi - xlow; c++)
		{
			cout << "c: " << c << "\t matrix: \t" << matrix[r][c] << endl;
		}

	}
	}
*/
	//  now we process the binary matrix
/*
	// create 2 temp matrices for eroding and dilating
	int** temp1 = new int*[yhi-ylow]; 
	for(int y = 0; y < yhi; ++y)
	{
		temp1[y] = new int[xhi-xlow]; 
	}

	int** temp2 = new int*[yhi-ylow]; 
	for(int y = 0; y < yhi; ++y)
	{
		temp2[y] = new int[xhi-xlow]; 
	}

	// erode twice
	erode(temp1, matrix, yhi-ylow, xhi-xlow);
	erode(temp2, temp1, yhi-ylow, xhi-xlow);

    // dilate twice
	dilate(temp1, temp2, yhi-ylow, xhi-xlow);
	dilate(temp2, temp1, yhi-ylow, xhi-xlow);

	//int** holder = temp2;

	//temp2 = matrix;

	erode3(temp1, matrix, yhi-ylow, xhi-xlow);

	// calculate the centroid (assumption is that there is low enough noise that the centroid will be in our rectangle)
	POINT p = centroid(temp1, 1, yhi-ylow, xhi-xlow);

	CvPoint preal;
	preal.x = p.x + xlow;
	preal.y = p.y + ylow;

	if (getImageAgainFromFile || getImageFromFile) 
	{
		preal.y = imageSize.height - preal.y;  // reverse if static image
	}

	cvCircle( f_p, preal, 4,  CV_RGB(0,0,255), 3 );

	if(matrix[p.y][p.x] != 1 || (p.x == -1 && p.y == -1) ) // centroid is not on a green pixel, so either too noisy, or multiple green blobs (or no green pixels)
	{
		seeLandmark = false;
	}
	else
	{
*/

	int currentLabel = 2;
	int currentSize;
	int bestLabel = 1;
	int bestSize = 0;

	// label all connected components, and figure out which is the largest connected component
	for(int r = 0; r < yhi-ylow; ++r)
	{
		for(int c = 0; c < xhi-xlow; ++c)
		{

			if(matrix[r][c] == 1) // ie is unlabeled
			{
				// do a connected components analysis, turning all connected ones into the current label
				currentSize = connectedComponents(matrix, r, c, 1, currentLabel, yhi-ylow, xhi-xlow);

				if(currentSize > bestSize)
				{
					bestSize = currentSize;
					bestLabel = currentLabel;
				}

				++currentLabel;
			}

		}
	}

	// now we can focus on just the label for the rectangle and get the height of it

	// we'll count the number of rows which have at least 10 pixels of green in a row
	int rowCount = 0;

//int** temp2 = matrix;

	for(int r = 0; r < yhi-ylow; ++r)
	{
		int greenCount = 0;
		for(int c = 0; c < xhi-xlow; ++c)
		{
/*				if(detectGreen)
			{
				cout << "c: " << c << "\t" << matrix[r][c] << endl;
			}
			*/
			bool counted = false;
			
			if(matrix[r][c] == bestLabel)
			{
//					cout << "up green" << endl;
				++greenCount;
			}
			else
			{
				greenCount = 0;
			}
			
/*				if(detectGreen)
			{
				cout << "greenCount: " << greenCount;
			}
*/
			if(greenCount == 20 && !counted)
			{
				++rowCount;
				counted = true;
			}
		}
	}

	POINT center = centroid(matrix, bestLabel, yhi-ylow, xhi-xlow);

	CvPoint preal;
	preal.x = center.x + xlow;
	preal.y = center.y + ylow;

	if (getImageAgainFromFile || getImageFromFile) 
	{
		preal.y = imageSize.height - preal.y;  // reverse if static image
	}

	cvCircle( f_p, preal, 4,  CV_RGB(255,0,0), 3 );


	if(rowCount < 20) // too small to count (most likely not a rectangle
	{
		seeLandmark = false;
	}
	else
	{
		seeLandmark = true;

		// visual cue of see landmark
		cvCircle( f_p, pt4, 10,  CV_RGB(255,0,0), /*thickness*/ 1 );

		// later try to get 2 rect edges
		landmarkDist = pixelHeightToDistance(rowCount);		
		landmarkAngle = calculateAngle(center.x);

		if(toggle) // only display this once per static image
		{
		cout << "rowcount: " << rowCount << endl;
		cout << "dist: " << landmarkDist << endl;
		cout << "angle: " << landmarkAngle << endl;

		toggle = false;

		}
	}
//else	}

	for (int x = xlow; x<xhi; x++)		//the x direction goes left to right       
	{
		for (int y = ylow; y<yhi; y++)  //the y direction goes top to bottom
		{
			// we handle this here...
			realy = y;
			if (getImageAgainFromFile || getImageFromFile) 
			{
				realy = imageSize.height - realy;  // reverse if static image
			}

			// note that we need to use "realy" in each of these cases
			color = cvPtr2D(frame,realy,x);    // get the pixel we care about
			painted = cvPtr2D(f_p,realy,x);    // get the pixel we (might) change

			if(matrix[y-ylow][x-xlow] == bestLabel)  // rect shows up blue
			{
				painted[0] = 255;  // sets blue to 0
				painted[1] = 0;    // sets green to 0
				painted[2] = 0;    // sets red to 255
			}
			else if(matrix[y-ylow][x-xlow] != 0) // other non rect green stuff shows up green
			{
				painted[0] = 0;  // sets blue to 0
				painted[1] = 255;    // sets green to 0
				painted[2] = 0;    // sets red to 255
			}

		}

	}

	// release the memory
	for(int y = 0; y < yhi-ylow; ++y)
	{
		delete[] matrix[y];
//		delete[] temp1[y];
//		delete[] temp2[y];
	}
	delete[] matrix;
//	delete[] temp1;
//	delete[] temp2;
}

void erode(int** dst, int** src, int rows, int cols)
{
	assert(dst != src);

	for(int r = 0; r < rows; ++r)
		for(int c = 0; c < cols; ++c)
			dst[r][c] = 1;

	for(int r = 0; r < rows; ++r)
	{
		for(int c = 0; c < cols; ++c)
		{
			if(src[r][c] == 0)
			{
				for(int i = -1; i <= 1; ++i)
					for(int j = -1; j <= 1; ++j)
						if(r+i >0 && r+i < rows && c+j > 0 && c+j < cols)
                            dst[r+i][c+j] = 0;
			}
		}
	}
}

void erode3(int** dst, int** src, int rows, int cols)
{
	assert(dst != src);

	for(int r = 0; r < rows; ++r)
		for(int c = 0; c < cols; ++c)
			dst[r][c] = 1;

	for(int r = 0; r < rows; ++r)
	{
		for(int c = 0; c < cols; ++c)
		{
			if(src[r][c] == 0)
			{
				for(int i = -3; i <= 3; ++i)
					for(int j = -3; j <= 3; ++j)
						if(r+i >0 && r+i < rows && c+j > 0 && c+j < cols)
                            dst[r+i][c+j] = 0;
			}
		}
	}
}


void dilate(int** dst, int** src, int rows, int cols)
{
	assert(dst != src);

	for(int r = 0; r < rows; ++r)
		for(int c = 0; c < cols; ++c)
			dst[r][c] = 0;

	for(int r = 0; r < rows; ++r)
	{
		for(int c = 0; c < cols; ++c)
		{
			if(src[r][c] == 1)
			{
				for(int i = -1; i <= 1; ++i)
					for(int j = -1; j <= 1; ++j)
						if(r+i >0 && r+i < rows && c+j > 0 && c+j < cols)
                            dst[r+i][c+j] = 1;
			}
		}
	}
}

POINT centroid(int** matrix, int value, int rows, int cols)
{
	double rsum = 0.0;
	double csum = 0.0;
	int num = 0;
	POINT result;

	for(int r = 0; r < rows; ++r)
		for(int c = 0; c < cols; ++c)
			if(matrix[r][c] == value)
			{
				rsum += r;
				csum += c;
				++num;
			}

	if(num != 0)
	{
		result.y = int(rsum / num);
		result.x = int(csum / num);
	}
	else
	{
		result.y = result.x = -1;
	}
	
	return result;
}

int connectedComponents(int** matrix, int startR, int startC, int oldVal, int newVal, int rows, int cols)
{
	// using x,y as r,c
	POINT p;
	POINT temp;
	p.x = startR;
	p.y = startC;

	int count = 0;

	queue<POINT> q;

	// if the start point doesn't have oldVal, then the connected component is trivial (ie nothing)
	if(matrix[p.x][p.y] == oldVal)
		q.push(p);

	while(!q.empty())
	{
		p = q.front();
		q.pop();

		for(int i = -1; i <= 1; ++i)
			for(int j = -1; j <= 1; ++j)
			{
				temp.x = p.x + i;
				temp.y = p.y + j;
				if(temp.x > 0 && temp.x < rows && temp.y > 0 && temp.y < cols)
					if(matrix[temp.x][temp.y] == oldVal)
					{
						matrix[temp.x][temp.y] = newVal;
						q.push(temp);
						++count;
					}
			}
	}

	return count;

}


void setGlobals()
{
	// mouse click locations (old and new, to tell if we've
	// clicked on a new point
	old_click_pt.x = 0;
	old_click_pt.y = 0;
	new_click_pt.x = 0;
	new_click_pt.y = 0;

	// in theory these should get deleted at the end of the program, too...

	// this is the calibration matrix of the camera
	M = new float[9];
	M[0]=    393.1788940f; M[1]=      0.0000000f; M[2]=    162.8373871f;
	M[3]=      0.0000000f; M[4]=    395.7017517f; M[5]=    121.8078156f;
	M[6]=      0.0000000f; M[7]=      0.0000000f; M[8]=      1.0000000f;

	// these are the distortion parameters of the camera
    D = new float[4];
	D[0]=-0.358375f;
	D[1]=0.168058f;
	D[2]=-0.006497f;
	D[3]=0.001283f;

    // theseimage sizes are hard-coded, but should probably be obtained for each
    // image individually -- however, with our cameras and images, this is safe.
	imageSize.width = 320; imageSize.height = 240;

	// set up some global points for drawing, etc.
    int margin = 12;
	pt1.x = margin; pt1.y = margin; 
    pt2.x = imageSize.width-margin; pt2.y = imageSize.height-margin; 

	pt3.x = imageSize.width/2;
	pt3.y = imageSize.height/2;

	pt4.x = 80; 
	pt4.y = 12;

	pt5.x = 240;
	pt5.y = 228;

	pt6.x = 92; 
	pt6.y = 24;

	pt7.x = 228;
	pt7.y = 216;

}