/* OpenCV Hello world, with filtering of various filters 
 * Based on http://home.iitk.ac.in/~swagatk/faq/opencv.html#Colar_Based_Tracking 
 * Rick van der Zwet <info@rickvanderzwet.nl>
 */

#include <cv.h>
#include <cxcore.h>
#include <cvaux.h>
#include <highgui.h>
#include <stdio.h>

#define DEBUG 1

#define RVALUE(x,y,step) (x*step+y*3+2)
#define GVALUE(x,y,step) (x*step+y*3+1)
#define BVALUE(x,y,step) (x*step+y*3+0)

/* Used for playing with bounderies, returned max value of the pair */
int max(const int x, const int y) {
  return ((x > y) ? x : y);
}

/* Used for playing with bounderies, returned min value of the pair */
int min(const int x, const int y) {
  return ((x < y) ? x : y);

}

int InitSystem(int argc, char *argv[]) {
    // Don't care which camera to use. Use specified number elsewise
    int cam_number = -1;
    if (argc > 1)
    	cam_number = atoi(argv[1]);
    printf("Using camera %i\n", cam_number);
    CvCapture *capture = cvCaptureFromCAM(cam_number);
    int width = cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH);
    int height = cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT);


    uchar *filterdata, *outdata, *routdata, *goutdata, *boutdata, *targetdata;

    
    //Create a window
    cvNamedWindow("RGB_Image", CV_WINDOW_AUTOSIZE);
    cvMoveWindow("RGB_Image",0,0);
    cvNamedWindow("R_Image", CV_WINDOW_AUTOSIZE);    
    cvMoveWindow("R_Image",0,height);
    cvNamedWindow("G_Image", CV_WINDOW_AUTOSIZE);    
    cvMoveWindow("G_Image",width,0);
    cvNamedWindow("B_Image", CV_WINDOW_AUTOSIZE);    
    cvMoveWindow("B_Image",width,height);
    cvNamedWindow("F_Image", CV_WINDOW_AUTOSIZE);


    //Create Image Skeletons
    IplImage *image   = NULL;
    IplImage *output  = cvCreateImage( cvSize(width,height), 8, 3);
    IplImage *boutput = cvCreateImage( cvSize(width,height), 8, 3);
    IplImage *routput = cvCreateImage( cvSize(width,height), 8, 3);
    IplImage *goutput = cvCreateImage( cvSize(width,height), 8, 3);
    IplImage *filter  = cvCreateImage( cvSize(width,height), 8, 3);
    IplImage *target  = cvCreateImage( cvSize(width,height), 8, 3);


    // Some temp pointers
    int i, j, k, m, n, o, p;
    int r, g, b;
    int q;
	
    
    int cvalue = 0;
    int xmax = -10;
    int ymax = -10;
    // Video run starts here ...
    while (1)
    {                 
        image = cvQueryFrame(capture);
	
	// We need some input before running any further
	if (image == NULL) {
	  printf("Ehm no input in the image\n");
	  continue;
	}

        //This is needed for accessing pixel elements
        int step = image->widthStep/sizeof(uchar);

        //cvCopy(image, output, NULL);
	//Remove Noise from images
	cvSmooth(image,output,CV_BLUR, 3, 3, 3,3);
        cvCopy(image, routput, NULL);
        cvCopy(image, goutput, NULL);
        cvCopy(image, boutput, NULL);
	

        outdata = (uchar *) output->imageData;
        routdata = (uchar *) routput->imageData;
        goutdata = (uchar *) goutput->imageData;
        boutdata = (uchar *) boutput->imageData;
        targetdata = (uchar *) target->imageData;

	cvSub(output,filter,target,NULL);
	cvalue = 20;
        k = 0, p = 0, q = 0;
        for( m = 0; m < image->height; m++) //row
        {
            for( n = 0; n < image->width; n++) //column
            {
		//p = targetdata[RVALUE(m,n,step)] + targetdata[GVALUE(m,n,step)] + targetdata[BVALUE(m,n,step)];
		p = targetdata[RVALUE(m,n,step)] + targetdata[GVALUE(m,n,step)];
		if ( p > cvalue)
		{
			q++;
			if (q > 5) {
				cvalue = p;
				xmax = n;
				ymax = m;
				printf("Current value: %ix%i=%i\n", xmax, ymax, cvalue);
			}
		} else {
			q = 0;
		}
		routdata[BVALUE(m,n,step)] = 0; // clear blue part
		routdata[GVALUE(m,n,step)] = 0; // clear green part

		goutdata[BVALUE(m,n,step)] = 0; // clear blue part
		goutdata[RVALUE(m,n,step)] = 0; // clear red part

		boutdata[GVALUE(m,n,step)] = 0; // clear green part
		boutdata[RVALUE(m,n,step)] = 0; // clear red part
            }
        } 

	// Clear filter for new use
	// cvSetZero(filter);
	// ... or clone orginal image for mapping over it
	//cvCopy(goutput, filter, NULL);

        // Make it yellow
        //cvOr( routput, goutput, filter, NULL);

	// XXX: Use stuff like cvMat, cvGetRawData instead of internal hacking
	// Make green 'fade' out by the presence of many red as well
        // filterdata = (uchar *) filter->imageData;
	
	// Prepare combined output
	// Some way of making the picture more pretty
	//cvSmooth(filter,target, CV_MEDIAN, 3, 3, 3, 3);
        cvCopy(output,filter, NULL);
        cvCircle(target, cvPoint(xmax, ymax), 10, CV_RGB(255,0,0),4, 8, 0);
                        

        // Show all the images
        cvShowImage("RGB_Image", output);
        cvShowImage("R_Image", routput);
        cvShowImage("B_Image", boutput);       
        cvShowImage("G_Image", goutput);       
        cvShowImage("F_Image", target);       
                    
                
        // wait for any to press to quit
	// Eeks! also HACK to make it refresh properly
        if (cvWaitKey(20) != -1) {
		break;
	}
    } //for each frame ...


    cvReleaseCapture(&capture);
    cvDestroyWindow("RGB_Image");
    cvDestroyWindow("R_Image");
    cvDestroyWindow("G_Image");
    cvDestroyWindow("B_Image");
    cvDestroyWindow("F_Image");

    return 0;

}

// Get ourself into the highGUI main stuff
// Quick at 1.0.0, not needing in  2.0x anymore
int main(int argc, char *argv[]) {
   return InitSystem(argc, argv);
}