Index: /liacs/mss/project/colour-tracking.c
===================================================================
--- /liacs/mss/project/colour-tracking.c	(revision 8)
+++ /liacs/mss/project/colour-tracking.c	(revision 9)
@@ -10,4 +10,20 @@
 #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 cvInitSystem(int argc, char *argv[]) {
@@ -19,5 +35,5 @@
 
 
-    uchar *routdata, *goutdata, *boutdata, *target;
+    uchar *filterdata, *outdata, *routdata, *goutdata, *boutdata;
 
     
@@ -41,8 +57,12 @@
     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 j, k, m, n, p;
+    int i, j, k, m, n, o, p;
+    int r, g, b;
+    int q;
+	
     
     // Video run starts here ...
@@ -66,4 +86,5 @@
 	
 
+        outdata = (uchar *) output->imageData;
         routdata = (uchar *) routput->imageData;
         goutdata = (uchar *) goutput->imageData;
@@ -75,50 +96,110 @@
             for( n = 0; n < image->width; n++) //column
             {
-		routdata[m*step+n*3+0] = 0; // clear blue part
-		routdata[m*step+n*3+1] = 0; // clear green part
-
-		goutdata[m*step+n*3+0] = 0; // clear blue part
-		goutdata[m*step+n*3+2] = 0; // clear red part
-
-		boutdata[m*step+n*3+1] = 0; // clear green part
-		boutdata[m*step+n*3+2] = 0; // clear red part
+		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);
+	// 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;
+        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
-        for(m = 0; m < image->height; m++)
+	const int stepSize = 5;
+        for(m = 0; m < image->height; m = m+stepSize)
         {
-            for(n = 0; n < image->width; n++)
+            for(n = 0; n < image->width; n = n+stepSize)
             {
 		// Make it stop processing all points, pick the ones with high value
-                if(goutdata[m*step+n*3+1] > 200)
-                {
-                    if(n < xmin)
-                        xmin = n;
-                    if(n > xmax)
-                        xmax = n;
-                    if(m < ymin)
-                        ymin = m;
-                    if(m > ymax)
-                        ymax = m;
+		value = filterdata[GVALUE(m,n,step)];
+		if (value > cmax) 
+		{
+		    // detect 'square'
+		    p = 0;
+		    const int SIZE = 2;
+		    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++;
+			    }
+			}
+		    }
+
+		    /* 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;
+		    }
                 }
             }
         }
-             
-        cvRectangle(output, cvPoint(xmin, ymin), cvPoint(xmax,ymax), CV_RGB(0,255,0),2, 8, 0);
-        cvCircle(filter, cvPoint((xmin+xmax)/2, (ymin+ymax)/2), 0, CV_RGB(255,0,0),2, 8, 0);
+	
+	// Prepare combined output
+	cvCopy(filter, target, NULL);
+        cvCircle(target, cvPoint(xmax, ymax), 10, CV_RGB(255,0,0),4, 8, 0);
                         
-
-
 
         // Show all the images
@@ -127,8 +208,9 @@
         cvShowImage("B_Image", boutput);       
         cvShowImage("G_Image", goutput);       
-        cvShowImage("F_Image", filter);       
+        cvShowImage("F_Image", target);       
                     
                 
-        //wait for a key to press, Eeks! HACK to make it refresh properly
+        // wait for any to press to quit
+	// Eeks! also HACK to make it refresh properly
         if (cvWaitKey(20) != -1) {
 		break;
@@ -148,4 +230,5 @@
 }
 
+// Get ourself into the highGUI main stuff
 int main(int argc, char *argv[]) {
    return cvInitSystem(argc, argv);
