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

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

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

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

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

inline double threemax(double r, double g, double b) { return mymax(r, mymax(g, b)); }

inline double threemin(double r, double g, double b) { return mymin(r, mymin(g, b)); }

void RGBtoHSV( double r, double g, double b, double *h, double *s, double *v )
{
    // Based on in-class code
    double max = threemax(r,g,b);
    double min = threemin(r,g,b);
    *v = max / 255.0;
    *s = (max - min) / 255.0;

    double delta(max - min);

    if(int(delta) == 0)
	*h = 0;
    else
    {
	if (max == r)
	    *h = 0.0/3.0 + (g - b) / delta;
	else if (max == g)
	    *h = 1.0/3.0 + (b - r) / delta;
	else //if (max == b) - don't need it, you know
	    *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 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
    cvLine(f_p, pt4, pt5,CV_RGB(0,255,0),1,8);      // 1 == thickness, 8 = connectedness
    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);

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

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

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;

}