/* 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>
#include "xdo.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)

/* XXX: Global variables */
xdo_t *xdo;

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

}

/* Convert the virtual mapping to screen size */
int ratio (const int size, const int current_size, const int orig_size) {
    double t = (double)size / current_size;
    t *= orig_size;
    // printf("%i/%i -> %i\n", size, current_size, (int)t);
    return((int)t);
}

int InitSystem(int argc, char *argv[]) {
    // Don't care which camera to use. Use specified number elsewise
    if (argc < 3) {
        printf("Usage: %s <cam number> <screen width> <screen height>\n",
            argv[0]);
        return(64);
    }
    const int cam_number = atoi(argv[1]);
    const int screen_width = atoi(argv[2]);
    const int screen_height = atoi(argv[3]);
    printf("INFO Using camera %i\n", cam_number);
    printf("INFO Screen size %ix%i\n", screen_width, screen_height);
    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 x_move, y_move;

    int retval;
    char arg1[4], arg2[4];
    int keycode;
    // Actually move the pointer to it's location
    int mouse_flag = 0;
    int r_window_flag = 1;
    int g_window_flag = 1;
    int b_window_flag = 1;
    // XXX: Hack to have the pointer posted on the second screen.
    int ex_screen_flag = 0;
    int x_ajust = -114;
    int y_ajust = -95;

    
    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);
        // Set to pointer to new location
        // Some Hacking to get the pointer synced
        if (mouse_flag) {
            x_move = ratio(xmax + x_ajust,width,screen_width);
            y_move = ratio(ymax + y_ajust,height,screen_height);
            if (ex_screen_flag) {
                x_move += screen_width;
            }
            xdo_mousemove(xdo, x_move, y_move);
        }
                        

        // Show all the images
        cvShowImage("RGB_Image", output);
        if (r_window_flag)
            cvShowImage("R_Image", routput);
        if (b_window_flag)
            cvShowImage("B_Image", boutput);       
        if (g_window_flag)
            cvShowImage("G_Image", goutput);       
        cvShowImage("F_Image", target);       
                    
                
        // Process some keyboard input for handy selection
        // Eeks! also HACK to make it refresh properly
        keycode = cvWaitKey(20);
        if (keycode != -1) {
            printf("Keycode: %i\n", keycode);
            if (keycode == 'q') {
		        break;
            } else if (keycode == 'm') {
                mouse_flag ^= 1;
            } else if (keycode == 'e') {
                ex_screen_flag ^= 1;
            // Hack to manually sync mouse pointer with laser pointer
            } else if (keycode == 'w' || keycode == 's' || keycode == 'a' || \
                keycode == 'd') { 
                if (keycode == 'w') {
                    y_ajust--;
                } else if (keycode == 's') {
                    y_ajust++;
                } else if (keycode == 'a') {
                    x_ajust--;
                } else if (keycode == 'd') {
                    x_ajust++;
                }
                printf("New x,y ajust: %i,%i\n",x_ajust,y_ajust);
            } else if (keycode == 'r' || keycode == 'g' || keycode == 'b') {
                printf("Toggle window: %c\n", keycode);
                if (keycode == 'r') {
                    r_window_flag ^= 1;
                } else if (keycode == 'g') {
                    g_window_flag ^= 1;
                } else if (keycode == 'b') {
                    b_window_flag ^= 1;
                }
            }
        }
    } //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[]) {
   int retval;
   printf("=== INSTRUCTION == \n\
Keybindings:\n\
e = toggle mouse on external screen mouse pointer\n\
m = toggle mouse tracking\n\
q = quit\n\
Mouse/laser pointer syncer:\n\
w = up\n\
s = down\n\
a = left\n\
f = right\n\
");
   xdo = xdo_new(getenv("DISPLAY"));
   retval = InitSystem(argc, argv);
   xdo_free(xdo);
   return(retval);
}