// 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>
// 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;


// 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 openImage(IplImage *f_p, int upthresh, int lowthresh);
void closeImage(IplImage *f_p, int thresh);
bool inBounds(int x, int xlow, int xhi, int y, int ylow, int yhi);
bool isArrowColor(int r, int g, int b, double hue, double sat, double val);
void findArrow( IplImage* frame, IplImage* f_p );
bool isRed(double h, double s, double v, double r, double g, double b);
double calculateDistance(int ypixel);
double pixelsPerInch(int ypixel);

// global variables updated by wall detection and arrow detection routines
bool isWall = false;
double wallDist = 1000;

double pixcount;

CvPoint arrowCentroid;
double arrowTheta = 90.0;

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;

static CvPoint mypt1;
static CvPoint mypt2;

// 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 detectArrow = 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;


// 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; }


// convert from pixel to real world distance
double calculateDistance(int ypixel)
{
	return 15.857*exp(0.006*ypixel);
}

// convert from pixel height to width measurement
double pixelsPerInch(int ypixel)
{
	return (-0.4247 * ypixel + 169.3)/12.0;         
}

// 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 << ") 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(isRed(hue,sat,val,pixel[2],pixel[1],pixel[0]))
			cout << "I think it's red" << endl;
		else
			cout << "Not Red\n";
		if(isArrowColor(pixel[2],pixel[1],pixel[0],hue,sat,val))
			cout << "Black" << endl;
		else
			cout << "Not black." << endl;
	}
}

// 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, "%d %d", 0, 0);
			SS.sendout(sendbuf,80);
		}

		if (recvbuf[0] == 'w')	// Am I facing a wall?
		{
			// wallDist has a range of about 20-220
			char *buf;
			if (isWall)
				SS.sendout("1",2);
			else
				SS.sendout("0",2);
			
		}

		if (recvbuf[0] == 'A') // turn on arrow detection
		{
			detectArrow = true;
			SS.sendout("1",2);
		}

		if (recvbuf[0] == 'J') // get arrow pixel count
		{
			sprintf(sendbuf, "%d", (int)pixcount);
			SS.sendout(sendbuf);
		}

        if (recvbuf[0] == 'b') // get arrow distance
		{
			sprintf(sendbuf, "%f", calculateDistance(arrowCentroid.y));
			SS.sendout(sendbuf);
		}

		if (recvbuf[0] == 'C') // return offset of arrow to center the arrow
		{
			int center_pixel = (pt4.x + pt5.x) / 2;
			//cout << (arrowCentroid.x - center_pixel) << endl;
			//int i = 42;
			//double x = 42.42;
			//cout << "Sending " << i << " and " << x << endl;
			sprintf(sendbuf, "%d 0", (arrowCentroid.x - center_pixel));
			SS.sendout(sendbuf);
		}

		if (recvbuf[0] == 'B') // return 1 if arrow touching yhi
		{
			int yhi = (int)mymax(pt4.y, pt5.y);
			int xlow = mymin(pt4.x, pt5.x);
			int xhi = mymax(pt4.x, pt5.x);

			int touching = 0;

			uchar* pixel;

			for (int i = xlow ; i < xhi ; i += 5)
			{
				pixel = cvPtr2D(undis_frame_painted, yhi, i);
				if (pixel[2] > 240) {
					touching = 1;
					break;
				}
			}

			sprintf(sendbuf, "%d 0", touching);
			SS.sendout(sendbuf);

		}

		if (recvbuf[0] == 'G') // get distance to arrow and direction
		{
			double dist = calculateDistance(arrowCentroid.y);

			sprintf(sendbuf, "%f %f 0", dist, arrowTheta);
			//cout << sendbuf << endl;
			SS.sendout(sendbuf);


		}

		// 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");

	// 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;
	}

	// 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
		if (detectRed == true)
		{
			processRed(undis_frame,undis_frame_painted);
		}

		if (detectArrow == true)
			findArrow(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;
		}
		else if (keyCode == (int)'-') {
			if (blueValue > 4) blueValue -= 5;
		}
		else if (keyCode == (int)'w') {
			
		}
		else if (keyCode == (int)'s') {

		}
		/*
		 * 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)'r') {
			detectRed = !detectRed;
		}
		/*
		 * toggle finding arrow
		 */
		else if (keyCode == (int)'a')
			detectArrow = !detectArrow;
		else if (keyCode == (int)'A') { // server command, not keyboard
			// do nothing
		}
		else if (keyCode == (int)'C') { // server command, not keyboard
			// do nothing
		}
		else if (keyCode == (int)'B') {
		}
		else if (keyCode == (int)'G') {
		}
		/*
		 * the spacebar advances the image in the VideoBMPs directory
		 */
		else if (keyCode == (int)' ') {
			if (showing_savedImage == 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);

				// 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


// 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 )
{
	if(r==g&&g==b)
	{
		*h=0;
		*s=0;
		*v=(mymax(r, mymax(g, b)))/255;
		return;
	}

	*v = mymax(r, mymax(g, b));
	*s = *v - mymin(r, mymin(g, b));
	*v /= 255;
	
	if(mymax(r, mymax(g, b)) == r)
		*h = (g-b)/ *s;
	else if(mymax(r, mymax(g, b)) == g)
		*h = 1.0/3.0 + (b-r)/ *s;
	else //if(mymax(r, mymax(g, b)) == b)
		*h = 2.0/3.0 + (r-g)/ *s;

//	if(*h < 0)
//		(*h)++;

	*s /= 255;
}

bool isArrowColor(int r, int g, int b, double hue, double sat, double val)
{
	int threshhold = 50;
	// current arrow is black on light, so just need rgb threshholds
	return (r < threshhold && g < threshhold && b < threshhold);
	//cout << hue << endl;
	//return ((r+g+b < 20)
		//&& (0.25 < hue) && (hue < 0.5)
		//&& (5*(b+r) < 3*g));
	//return isRed(hue, sat, val, r, g, b); // Might we just use red arrows?
}

bool inBounds(int x, int xlow, int xhi, int y, int ylow, int yhi) {
	return (x > xlow && x < xhi && y < yhi && y > ylow);
}

void closeImage(IplImage* f_p, int thresh)
{
	int r,g,b;
	uchar* painted; // pixel in painted image

	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 realy;
	for(int x = xlow; x < xhi; x++) {
		for(int y = ylow; y < yhi; y++) {
			realy = y;
			if (getImageAgainFromFile || getImageFromFile) 
			{
				realy = imageSize.height - realy;  // reverse if static image
			}

			painted = cvPtr2D(f_p,realy,x);    // get the pixel we (might) change

			uchar* newpainted;
			if(painted[2] > thresh) {
				for(int i = x-1; i <= x+1; i++) {
					for(int j = realy - 1; j <= realy + 1; j++) {
						if(inBounds(i,xlow,xhi,j,ylow,yhi)) {
							newpainted = cvPtr2D(f_p,j,i);
							newpainted[2] = thresh - 1;
						}
					}
				}
			}
		}
	}
}

void openImage(IplImage* f_p, int upthresh, int lowthresh)
{
	int r,g,b;
	uchar* painted; // pixel in painted image

	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 realy;
	for(int x = xlow; x < xhi; x++) {
		for(int y = ylow; y < yhi; y++) {
			realy = y;
			if (getImageAgainFromFile || getImageFromFile) 
			{
				realy = imageSize.height - realy;  // reverse if static image
			}

			painted = cvPtr2D(f_p,realy,x);    // get the pixel we (might) change

			uchar* newpainted;
			if(painted[2] > upthresh) {
				for(int i = x-1; i <= x+1; i++) {
					for(int j = realy - 1; j <= realy + 1; j++) {
						if(inBounds(i,xlow,xhi,j,ylow,yhi)) {
							newpainted = cvPtr2D(f_p,j,i);
							if(newpainted[2] < upthresh && newpainted[2] < lowthresh) {
								painted[2] = lowthresh;
								break;
							}
						}
					}
				}
			}
		}
	}

}

void findArrow(IplImage* frame, IplImage* f_p)
{
	int r,g,b;
	double hue,sat,val;

	uchar* color;
	uchar* painted;

	cvRectangle(f_p, pt4, pt5, CV_RGB(0,0,255), 1);


	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);
	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...
			int 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);

			if(isArrowColor(r,g,b,hue,sat,val))
			{
				painted[0] = 0;
				painted[1] = 0;
				painted[2] = 255;
			}
			else {
				painted[0] = 0;
				painted[1] = 0;
				painted[2] = 0;
			}
			
		}
	}

	openImage(f_p, 254, 1);
	openImage(f_p, 254, 1);
	openImage(f_p, 254, 1);
	openImage(f_p, 241, 1);
	closeImage(f_p, 253);
	closeImage(f_p, 251);
	closeImage(f_p, 249);
	closeImage(f_p, 247);
	closeImage(f_p, 245);
	closeImage(f_p, 243);
	closeImage(f_p, 241);
//	closeImage(f_p, 243);

	double ytotal = 0;
	double xtotal = 0;
	double num = 0;
	//num = 0;


	for (int x = xlow ; x < xhi ; ++x)
	{
		for (int y = ylow ; y < yhi ; ++y)
		{
			int 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
			painted = cvPtr2D(f_p,y,x);    // get the pixel we (might) change

			if (painted[2] > 230)
			{
				painted[2] = 255;
				ytotal += y;
				xtotal += x;
				num+=1;
			}

		}
	}
	 pixcount = num;
	if (num > 0)
	{
		arrowCentroid.x = xtotal / num;
		arrowCentroid.y = ytotal / num;
	} else 
	{
		arrowCentroid.x = arrowCentroid.y = 230;
	}


	double big_diam = 0.0;
	double best_theta = 0.0;
	CvPoint best1;
	best1.x = 160;
	best1.y = 70;
	CvPoint best2;
	best2.x = 160;
	best2.y = 70;

	for (int i = -2 ; i < 3 ; ++i) 
		for (int j = -2 ; j < 3 ; ++j) 
			//if (inBounds(arrowCentroid.x+j, xlow, xhi, arrowCentroid.y+i, ylow, yhi))
				cvPtr2D(f_p,arrowCentroid.y+i, arrowCentroid.x+j)[0] = 255;


	for (double theta = 0.0 ; theta <= 3.14 ; theta += (3.14 / 100.0)) // "forward ho!" is \frac{\pi}{2} radians
	{                                                               // not ricco ^
		int x1 = arrowCentroid.x;
		int y1 = arrowCentroid.y;
		int x2 = arrowCentroid.x;
		int y2 = arrowCentroid.y;

		// one kind of "out" 
		while(cvPtr2D(f_p, y1, x1)[2] > 240)
		{
			x1 += ceil(10.0*cos(theta));
			y1 += ceil(10.0*sin(theta));
			if(!inBounds(x1,xlow,xhi,y1,ylow,yhi))
				break;
		}
		
		// the other kind of "out"
		while (cvPtr2D(f_p, y2, x2)[2] > 240)
		{
			x2 -= ceil(10.0*cos(theta));
			y2 -= ceil(10.0*sin(theta));
			if(!inBounds(x1,xlow,xhi,y1,ylow,yhi))
				break;
		}
		
		double diam = sqrt(pow(x1 - x2 ,2) + pow(y1 - y2 ,2));

		if (diam > big_diam)
		{
			big_diam = diam;
			best_theta = theta;
			best1.x = x1;
			best1.y = y1;
			best2.x = x2;
			best2.y = y2;
		}


	}

	CvPoint midpoint;
	midpoint.x = (double)(best1.x + best2.x) / 2.0;
	midpoint.y = (double)(best1.y + best2.y) / 2.0;
	int right = 0;
	int left = 0;

	for (int x = xlow ; x < xhi ; ++x)
	{
		for (int y = ylow ; y < yhi ; ++y)
		{
			if (cvPtr2D(f_p, y, x)[2] > 240)
			{
				double phi = atan2((double)y - midpoint.y, (double)x - midpoint.x);
				double r = sqrt(pow(midpoint.x - x,2) + pow(midpoint.y -y,2));

				phi -= best_theta;

				double new_x = r*cos(phi) + midpoint.x;
				
				if (new_x >= midpoint.x)
					left++;
				else
					right++;
			}

		}
	}


	if (right >= left) // at the moment, this code is hot
	{
		// best2 is the point, best1 is the base
		// fancy trig to get the real world angle!
		double height = calculateDistance(best2.y) - calculateDistance(best1.y);
		double distance = (double)(best2.x - midpoint.x)/pixelsPerInch(best2.y) 
							+ (double)(midpoint.x - best1.x)/pixelsPerInch(best1.y);
//		cout << "height: " << height << endl;
//		cout << "dist : " << distance << endl;
		arrowTheta = (180.0/3.1415926)*atan2(height,distance);
		if (arrowTheta < 0)
			arrowTheta += 360.0;

		for (int i = -2 ; i < 3 ; ++i)
			for (int j = -2 ; j < 3 ; ++j)
				if (inBounds(best2.x+j, xlow, xhi, best2.y+i, ylow, yhi))
					cvPtr2D(f_p,best2.y+i, best2.x+j)[0] = 255;
	}
	else
	{
		// best1 is the point, best2 is the base
		// fancy trig to get the real world angle!
		double height = calculateDistance(best1.y) - calculateDistance(best2.y);
		double distance = (double)(best1.x - midpoint.x)/pixelsPerInch(best1.y) 
							+ (double)(midpoint.x - best2.x)/pixelsPerInch(best2.y);
//		cout << "height: " << height << endl;
//		cout << "dist : " << distance << endl;

		arrowTheta = (180.0/3.1415926)*atan2(height,distance);
		if (arrowTheta < 0)
			arrowTheta += 360.0;

		for (int i = -2 ; i < 3 ; ++i)
			for (int j = -2 ; j < 3 ; ++j)
				if (inBounds(best1.x+j, xlow, xhi, best1.y+i, ylow, yhi))
					cvPtr2D(f_p,best1.y+i, best1.x+j)[0] = 255;
	}	
	cvLine(f_p, best1, best2, CV_RGB(0,255,0),1,8);      // 1 == thickness, 8 = connectedness

//	cout << "arrow is at angle: " << arrowTheta << endl;
}


// 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"

	// 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* yavgs = new int[(double)(xhi-xlow) / 5.0];
	int* yweights = new int[(double)(xhi-xlow)/5.0];

	int cavg = 0;
	for (int x = xlow ; x < xhi ; x += 5)
	{
		double total = 0;
		double count = 0;

		for (int y = ylow ; y < yhi ; ++y)
		{
			color = cvPtr2D(frame,y,x);
			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
			RGBtoHSV(r,g,b,&hue,&sat,&val);

			if (isRed(hue,sat,val,r,g,b))
			{
				total += y;
				++count;
			}

		}
		if (count != 0)
			yavgs[cavg] = (int)(total / count);
		else
			yavgs[cavg] = -1;
		yweights[cavg++] = count;
	}

//	for (int i = 0 ; i < cavg ; ++i)
//		cout << yavgs[i] << endl;

	double sprod = 0;
	double avgx  = 0;
	double avgy  = 0;
	double sxsqr = 0;

	int pts = 0;

	for (int i = 0 ; i < cavg ; ++i)
	{
		if (yavgs[i] != -1) 
		{
			sprod += yweights[i]*((xlow + (5*i)) * (yavgs[i]));
			avgx  += yweights[i]*(xlow + (5*i));
			avgy  += yavgs[i]*yweights[i];
			sxsqr += yweights[i]*(xlow + (5*i)) * (xlow + (5*i));
			pts += yweights[i];
		}
	}
	//cavg -= skips;
	avgx /= pts;
	avgy /= pts;

	double myb = (sprod - pts * avgx * avgy) / (sxsqr - pts * avgx * avgx);
	double mya = ((avgy * sxsqr) - (avgx * sprod)) / (sxsqr - (pts * avgx * avgx));

	 mypt1.x = xlow;
	 mypt2.x = xhi;

	 mypt1.y = mya + (myb * xlow);
	 mypt2.y = mya + (myb * xhi);

	 // Line doesn't get printed until after the red does


	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...
			int 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);

			if(isRed(hue,sat,val,r,g,b))
			{
				painted[0] = 0;
				painted[1] = 0;
				painted[2] = 255;
			}

			//cout << myb << " is myb while pts is "<<pts<<endl;
			// We want to have the line be mostly horizontal, and we want a lot of red 
			// ..points in view. If these conditions are satisfied, we are probably
			// ..facing the red molding. If this is the case, print the circle. 
						// gratuitous drawing routine, just to show how it works
			//if ( x%4 != 0 && y%4 != 0 )
			//{
			//	painted[0] = blueValue;  // sets blue to 255
			//	painted[1] = 0;    // sets green to 0
			//	painted[2] = 0;    // sets red to 0
			//}

			
		}
	}	
	if (myb > -0.1 && myb < 0.1 && pts > 150)
	{
		cvCircle( f_p, pt4, 10,  CV_RGB(255,255,0), /*thickness*/ 1 );
		isWall = true;
		if (avgy == 0)
			wallDist=666;
		else
			wallDist = avgy;
	}
	else
		isWall = false;

	cvLine(f_p, mypt1, mypt2,CV_RGB(0,255,0),1,8);      // 1 == thickness, 8 = connectedness
}

//this function is HAWT
bool isRed(double h, double s, double v, double r, double g, double b)
{
	/*if(h<.2 && h>.05)
		if(s/v > .9 && s/v < 1.15)
			if(v>.1)
				return true;
				*/
	if(r+15>(g+b) && r>25 && g<75 && b<75)
	{
		if(h<.35 && h>-.055)
			if(s>.1)
				if(v>.05)	
					return true;
		if(h>.35 && h<.55)
			if(s>.4 && v>.4)
				return true;
			else if(s>.25 && v<.15)
				return true;
	}
	return false;
}

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 = 40; 
	pt4.y = 12;

	pt5.x = 280;
	pt5.y = 228;

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

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

}