Index: liacs/mss/project/colour-tracking.c
===================================================================
--- liacs/mss/project/colour-tracking.c	(revision 13)
+++ liacs/mss/project/colour-tracking.c	(revision 14)
@@ -29,5 +29,8 @@
 int cvInitSystem(int argc, char *argv[]) {
     // Don't care which camera to use. Use specified number elsewise
-    int cam_number = atoi(argv[1]) > 0 ? atoi(argv[1]) : -1;
+    int cam_number = -1;
+    if (argc > 1)
+    	cam_number = atoi(argv[1]) > 0 ? atoi(argv[1]) : -1;
+    printf("Using camera %i\n", cam_number);
     CvCapture *capture = cvCaptureFromCAM(cam_number);
     int width = cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH);
@@ -35,5 +38,5 @@
 
 
-    uchar *filterdata, *outdata, *routdata, *goutdata, *boutdata;
+    uchar *filterdata, *outdata, *routdata, *goutdata, *boutdata, *targetdata;
 
     
@@ -66,4 +69,7 @@
 	
     
+    int cvalue = 0;
+    int xmax = -10;
+    int ymax = -10;
     // Video run starts here ...
     while (1)
@@ -90,10 +96,27 @@
         goutdata = (uchar *) goutput->imageData;
         boutdata = (uchar *) boutput->imageData;
+        targetdata = (uchar *) target->imageData;
 
-        k = 0, p = 0;
+	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
@@ -117,91 +140,10 @@
 	// 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;
-        for( m = 0; m < image->height; m++) //row
-        {
-            for( n = 0; n < image->width; n++) //column
-            {
-	    	r = outdata[RVALUE(m,n,step)];
-	    	g = outdata[GVALUE(m,n,step)];
-	    	b = outdata[BVALUE(m,n,step)];
-		// Many red, less blue and green. Good for laser pointer tracing
-		p = (r < 220) ? 0 : r;
-		p = (b < 180 || g < 180) ? 0 : p;
-		//p = (abs(p - filterdata[GVALUE(m,n,step)]) > 20) ? p : 0;
-		filterdata[GVALUE(m,n,step)] = p;
-	    }
-	}
-
-
-        int xmin = 0, xmax = 0;
-        int ymin = 0, ymax = 0;
-	int value = 0, cmax = 0;
-
-	// Find the 'very' green point and mark it's surroundings
-	const int stepSize = 5;
-        for(m = 0; m < image->height; m = m+stepSize)
-        {
-            for(n = 0; n < image->width; n = n+stepSize)
-            {
-		// Make it stop processing all points, pick the ones with high value
-		value = filterdata[GVALUE(m,n,step)];
-		if (value > cmax) 
-		{
-		    // detect 'square'
-		    p = 0;
-		    int SIZE = 3;
-		    for (i = m; i  < min(m+SIZE,height); i++) 
-                    {
-		    	for (j = n; j < min(n+SIZE,width); j++) 
-                        {
-			    value = filterdata[GVALUE(i,j,step)];
-			    if (value > cmax) 
-                            {
-				p++;
-			    }
-			}
-		    }
-
-			
-		    SIZE = 6;
-		    /* Make sure envirionment is 'low' */
-		    for (i = max(m-SIZE,0); i  < m; i++) 
-                    {
-		    	for (j = max(n-SIZE,0); j < n; j++) 
-                        {
-		            q = filterdata[GVALUE(i,j,step)];
-		            if (q > value) 
-                            {
-		        	p--;
-		            }
-		        }
-		    }
-		    for (i = min(m+SIZE,height); i  < min(m+(2*SIZE),height); i++) 
-                    {
-		        for (j= min(m+SIZE,width); j < min(m+(2*SIZE),width); j++) 
-                        {
-		            q = filterdata[GVALUE(i,j,step)];
-		            if (q > value) 
-                            {
-		        	p--;
-		            }
-		        }
-		    }
-
-		    if (p > SIZE) 
-		    {
-			    //printf("%ix%i=%i\n",n,m,value);
-			    cmax = value;
-			    xmax = n;
-			    ymax = m;
-		    }
-                }
-            }
-        }
+        // 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(filter,target, NULL);
+        cvCopy(output,filter, NULL);
         cvCircle(target, cvPoint(xmax, ymax), 10, CV_RGB(255,0,0),4, 8, 0);
                         
@@ -217,5 +159,5 @@
         // wait for any to press to quit
 	// Eeks! also HACK to make it refresh properly
-        if (cvWaitKey(20) != -1) {
+        if (cvWaitKey(200) != -1) {
 		break;
 	}
