#include "cv.h"
#include "highgui.h"
#include <stdio.h>
#include <ctype.h>
#include "spline.h"
//#include "cond.h"
#include "sfilter.h"
#include "observer.h"
#include "setpwc.h"

#define SAMPLES 1000
//#define CAM_WIDTH 160
//#define CAM_HEIGHT 120
#define CAM_WIDTH 320
#define CAM_HEIGHT 240

IplImage *image = 0, *ycc = 0, *lum = 0, *edges = 0;
CvHistogram *hist = 0;

int backproject_mode = 0;
int select_object = 0;
int track_object = 0;
int show_hist = 1;
CvPoint origin;
CvRect selection;
CvRect track_window;
CvBox2D track_box;
CvConnectedComp track_comp;
int hdims = 16;
float hranges_arr[] = {0,180};
float* hranges = hranges_arr;
int vmin = 10, vmax = 120, smin = 45;
int disp_mode = 0;

bool play_mode, adv, edgy;
Observer ob;
SplineFilter sf(&ob);

// Redraws the image
void refresh()
{
	IplImage *i = edgy ? edges : image;
		
	ob.draw(i);
	sf.displayBest(i);
	cvShowImage( "Condensation Tracker", i );
}

// Mouse callback when input is from a live video source like a webcam.
void on_mouse( int event, int x, int y, int flags, void* param )
{
	if( !image )
		return;

	if( image->origin )
		y = image->height - y;

	switch( event )
	{
		case CV_EVENT_LBUTTONUP:
			// Add spline points or toggle display mode
			if (!ob.observing) {
				ob.addPoint(x,y);
			} else {
				edgy = !edgy;
			}
			break;
		case CV_EVENT_RBUTTONUP:
			// Toggle observing or not
			if (ob.observing) {
				ob.stopObserving();
			} else if (ob.st.s.points.size() > 3) {
				ob.startObserving();
				sf.nsamples(ob.last,SAMPLES);
			}
			break;
	}
}

// Mouse callback for running on a static video file 
void vid_mouse( int event, int x, int y, int flags, void *param )
{
	if( !image )
		return;

	if( image->origin )
		y = image->height - y;

	switch (event)
	{
		case CV_EVENT_LBUTTONUP:
			adv = true;
		break;
		case CV_EVENT_MBUTTONUP:
			// start the observer and the filter
			if (!ob.observing)
			{
				ob.startObserving();
				sf.nsamples(ob.last,SAMPLES);
			}
			else
				edgy = !edgy;
		break;
		case CV_EVENT_RBUTTONUP:
			// add a control point
			ob.addPoint(x,y);
			refresh();
		break;
		case CV_EVENT_LBUTTONDBLCLK:
			play_mode = ! play_mode;
		break;
	}
	
}

int main( int argc, char** argv )
{
	CvCapture* capture = 0;
	bool webcam = true;

	// Setup for live video feed, i.e. webcam
	if( argc == 1 || (argc == 2 && strlen(argv[1]) == 1 && isdigit(argv[1][0])))
	{
		capture = cvCaptureFromCAM( argc == 2 ? argv[1][0] - '0' : 0 );
		cvNamedWindow( "Condensation Tracker", 1 );
		cvSetMouseCallback( "Condensation Tracker", on_mouse, 0 );
		play_mode = true;
		int camfid = *(((int*)capture)+1);
		cout << "cam id: "<< camfid << endl;
		set_all_settings(camfid);
		cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH, CAM_WIDTH);
		cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT, CAM_HEIGHT);
	}
	// Setup for static video from a file
	else if (argc == 2)
	{
		capture = cvCaptureFromAVI( argv[1] ); 
		cvNamedWindow( "Condensation Tracker", 1 );
		cvSetMouseCallback( "Condensation Tracker", vid_mouse, 0 );
		play_mode = false;
		adv = false;
	}

	if( !capture ) {
		fprintf(stderr,"Could not initialize capturing...\n");
		return -1;
	}

	// Create the gui
	cvCreateTrackbar( "Thresh1", "Condensation Tracker", &vmin, 256, 0 );
	cvCreateTrackbar( "Thresh2", "Condensation Tracker", &vmax, 256, 0 );
	cvCreateTrackbar( "Normal Length", "Condensation Tracker", &smin, 256, 0 );

	// First frame and image are loaded outside the loop
	IplImage *frame = cvQueryFrame( capture );
	if( !frame )
		return 1;

	// Allocate the buffers 
	if( !image ) {
		image = cvCreateImage( cvGetSize(frame), 8, 3 );
		image->origin = frame->origin;
		ycc = cvCreateImage( cvGetSize(frame), 8, 3 );
		lum = cvCreateImage( cvGetSize(frame), 8, 1 );
		edges = cvCreateImage( cvGetSize(frame), 8, 1 );
	}
	cvCopy( frame, image, 0 );
	refresh();

	for(;;)
	{
		int i, bin_w, c;

		// Note: Don't remove this, as the WaitKey call processes the
		// event queue
		do {
			c = cvWaitKey(50);
		} while (!play_mode && !adv);
		adv = false;

		frame = cvQueryFrame( capture );
		if( !frame )
			break;

		cvCopy( frame, image, 0 );

		// Webcams are more intuitive when the work like a mirror; so
		// flip the display to mimic a mirror.
		if (webcam) {
			cvFlip(image,0,1);
		}

		// Convert to grayscale for edge detector
		cvCvtColor( image, lum, CV_RGB2GRAY );

		// Blur the image. This is required because my webcam is so
		// crappy, the Bayer interpolation leaves artefacts that break
		// the edge dector.
		cvSmooth( lum, lum, CV_BLUR,3,3);
		cvSmooth( lum, lum, CV_BLUR,5,5);

		// Apply edge detection to the blurred grayscale image
		cvCanny( lum, edges, vmin, vmax, 3);

		// Draw the splines and normals
		ob.observe(edges, sf.findBestSample());
		sf.updateSamples();

		// Actually display something
		refresh();

		// Update smin, as it comes from the slider
		ob.normalLength = smin;
	}

	cvReleaseCapture( &capture );
	cvDestroyWindow("CamShiftDemo");

	return 0;
}