/*
// Latest Revision of the Vision Code (4_25_2007)
//
// Finds the average pixel value of the goal and end of world colors.
// Creates a filter out of this value and returns the pixel percentage of both colors
// captured by all three cameras.
//
// Note: This has a lot of redundant code and can be made more modular.  
// camera_rev3 has various smoothing, sharpening, and filtering functions.
//
// Directions:  The code initially goes through a calibration routine where two colors can be calibrated for the goal color and end of world color.  The calibration is done using camera0.  Once the user places the color in front of the camera, it calculates the avg pixel value.  Once finished the user presses the “ESC” button on the key pad while clicking on the frame window.  Next the user is prompted to calibrate the end of world color in a similar manner.  After that the program streams all three cameras and returns the pixel percentage of both the goal and end of world color in the terminal window.
*/

#ifdef _CH_
#pragma package <opencv>
#endif

//#include "/usr/include/opencv/cv.h"
//#include "/usr/include/opencv/highgui.h"
#include "cv.h"
#include "highgui.h"
#include "cxcore.h"
//#include "cvcam.h"

#include <stdio.h>
#include <time.h>
#include <unistd.h>
#include <string.h>
#include <math.h>

/* 
* Takes in a source image and destination image and a array with a RGB color average
* Loops through each pixel in the source image and calculates the average pixel value.
* Displays greyscale version of the image based on the deviation of the RGB values from color_avg.
* The color_avg[] argument is overwritten with the calculated average RBG values.
* Returns the pixel percentage based on the number of pixels within 15% ofthe average value.
*
* The source image is unaltered, and the dest image holds the grey scale result
*/
double classify(IplImage *src, IplImage *dest, int color_avg[3])
{
	int i;
	int red_av = 0;
	int blue_av = 0;
	int green_av = 0;
	int temp;
	int pixel_count = 0;
	double pixel_percent = 0;
	unsigned char red, green, blue;
        unsigned char* ptrSource = (unsigned char*) src->imageData;
        unsigned char* ptrDest = (unsigned char*) dest->imageData;

        // loop through each pixel
        for(i = 0; i < dest->imageSize; i++)
        {
		red = ptrSource[2];
		green = ptrSource[0];
		blue = ptrSource[1];


		// converts to grayscale based on deviation from average pixel value
		// pixel value is 0-255 w/ 0 being black and 255 being white
		// so the darker the filtered image the better
		temp = abs(red-red_av/(dest->imageSize))/3 + abs(blue-blue_av/(dest->imageSize))/3 + abs(green-green_av/(dest->imageSize))/3;

		if (temp < 0.15*255)	// w/i 15% of avg pixel value
		{
			pixel_count++;
		}

		red_av = red_av + red;
		blue_av = blue_av + blue;
		green_av = green_av + green;

              // check for overflow/underflow
              if(temp > 255) temp = 255;
              if(temp < 0) temp = 0;

              *ptrDest = temp;

              // increment pointers
              ptrSource += 3;
              ptrDest++;
        }

	pixel_percent = ((double) pixel_count/(double) dest->imageSize)*100;

	//printf("temp: %i ",dest->imageSize);
	//printf("pixel_count: %i ,percent: %f \n",pixel_count,pixel_percent);
	//printf("red_av: %i ,green_av: %i, blue_av: %i \n",red_av/(dest->imageSize),green_av/(dest->imageSize),blue_av/(dest->imageSize));

	//red_tot_av = red_tot_av + red_av;

	color_avg[0] = red_av/(dest->imageSize);
	color_avg[1] = green_av/(dest->imageSize);
	color_avg[2] = blue_av/(dest->imageSize);

	return pixel_percent;

}

/* 
* Takes in a source image and destination image and a array with a RGB color average
* Loops through each pixel in the source image and calculates the average pixel value.
* Displays greyscale version of the image based on the deviation of the RGB values from      *color_target
* The color_avg[] argument is overwritten with the calculated average RBG values.
* Returns the pixel percentage based on the number of pixels within 15% ofthe average value.
*
* The source image is unaltered, and the dest image holds the grey scale result. 
* color_target[] is unaltered.
*/
double classify_target(IplImage *src, IplImage *dest,int color_avg[3], int color_target[3])
{
	int i;
	int red_av = 0;
	int blue_av = 0;
	int green_av = 0;
	int temp = 0;
	int pixel_count = 0;
	double pixel_percent = 0;
	unsigned char red, green, blue;
        unsigned char* ptrSource = (unsigned char*) src->imageData;
        unsigned char* ptrDest = (unsigned char*) dest->imageData;

        // loop through each pixel
        for(i = 0; i < dest->imageSize; i++)
        {
		red = ptrSource[2];
		green = ptrSource[0];
		blue = ptrSource[1];

		// converts to grayscale based on deviation from color_target pixel value
		// pixel value is 0-255 w/ 0 being black and 255 being white
		// so the darker the filtered image the better
		temp = abs(red-color_target[0])/3 + abs(blue-color_target[2])/3 + abs(green-color_target[1])/3;

		if (temp < 0.15*255)	// w/i 15% of avg pixel value
		{
			pixel_count++;
		}

		red_av = red_av + red;
		blue_av = blue_av + blue;
		green_av = green_av + green;

              // check for overflow/underflow
              if(temp > 255) temp = 255;
              if(temp < 0) temp = 0;

              *ptrDest = temp;

              // increment pointers
              ptrSource += 3;
              ptrDest++;
        }

	pixel_percent = ((double) pixel_count/(double) dest->imageSize)*100;

	//printf("pixel_count: %i ,percent: %f \n",pixel_count,pixel_percent);

	color_avg[0] = red_av/(dest->imageSize);
	color_avg[1] = green_av/(dest->imageSize);
	color_avg[2] = blue_av/(dest->imageSize);

	return pixel_percent;

}

/*
* Displays the cameras image in a frame and the greyscale filtered image in a frame.
* img0 is camera0 image, img1 is camera1 image, img2 is camera2 image  
* camera = 0,1,2 for those cameras, or 3 for all 3 cameras
* color_avg[] is an array of RGB values
* Calls classify() to filter the image.
*/
void processFrame(IplImage* img0, IplImage* img1, IplImage* img2, int camera, int color_avg[3])
{
		IplImage* imgRed0;
		IplImage* imgRed1;
		IplImage* imgRed2;

                // images used
                imgRed0 = cvCreateImage(cvSize(img0->width,img0->height), IPL_DEPTH_8U, 1);
		imgRed1 = cvCreateImage(cvSize(img1->width,img1->height), IPL_DEPTH_8U, 1);
		imgRed2 = cvCreateImage(cvSize(img2->width,img2->height), IPL_DEPTH_8U, 1);

                // apply the color filter and find pixel percent

		if(camera == 0)
		{
                	classify(img0, imgRed0, color_avg);

                        // show the quantized image
                        cvNamedWindow("img0", CV_WINDOW_AUTOSIZE);
                        cvShowImage("img0", img0);
                        cvNamedWindow("imgRed0", CV_WINDOW_AUTOSIZE);
                        cvShowImage("imgRed0", imgRed0);	
		}
		else if(camera == 1)
		{
			classify(img1, imgRed1, color_avg);

                        cvNamedWindow("img1", CV_WINDOW_AUTOSIZE);
                        cvShowImage("img1", img1);
                        cvNamedWindow("imgRed1", CV_WINDOW_AUTOSIZE);
                        cvShowImage("imgRed1", imgRed1);
		}
		else if(camera == 2)
		{
			classify(img2, imgRed2, color_avg);

                        cvNamedWindow("img2", CV_WINDOW_AUTOSIZE);
                        cvShowImage("img2", img2);
                        cvNamedWindow("imgRed2", CV_WINDOW_AUTOSIZE);
                        cvShowImage("imgRed2", imgRed2);
		}
		else if(camera == 3)	// All three cameras
		{
			classify(img0, imgRed0, color_avg);
			classify(img1, imgRed1, color_avg);
			classify(img2, imgRed2, color_avg);

                        // show the quantized image
                        cvNamedWindow("img0", CV_WINDOW_AUTOSIZE);
                        cvShowImage("img0", img0);
                        cvNamedWindow("imgRed0", CV_WINDOW_AUTOSIZE);
                        cvShowImage("imgRed0", imgRed0);

                        cvNamedWindow("img1", CV_WINDOW_AUTOSIZE);
                        cvShowImage("img1", img1);
                        cvNamedWindow("imgRed1", CV_WINDOW_AUTOSIZE);
                        cvShowImage("imgRed1", imgRed1);

                        cvNamedWindow("img2", CV_WINDOW_AUTOSIZE);
                        cvShowImage("img2", img2);
                        cvNamedWindow("imgRed2", CV_WINDOW_AUTOSIZE);
                        cvShowImage("imgRed2", imgRed2);
		}	


                /* release memory used by images */
                cvReleaseImage(&imgRed0);
		cvReleaseImage(&imgRed1);
 		cvReleaseImage(&imgRed2);

}

/*
* Displays the cameras image in a frame and the greyscale filtered image in a frame.
* img0 is camera0 image, img1 is camera1 image, img2 is camera2 image  
* camera = 0,1,2 for those cameras, or 3 for all 3 cameras
* color_avg[] is an array of RGB values, color_target[] is the avg RGB values of the goal
* or end of world colors.
* Calls classify_target() to filter the image.
* Returns the pixel percentage of the color_target.
*/
double processFrame_target(IplImage* img0, IplImage* img1, IplImage* img2, int camera, int color_avg[3], int color_target[3])
{
		double pixel_percent = 0;

		IplImage* imgRed0;
		IplImage* imgRed1;
		IplImage* imgRed2;

                // images used
                imgRed0 = cvCreateImage(cvSize(img0->width,img0->height), IPL_DEPTH_8U, 1);
		imgRed1 = cvCreateImage(cvSize(img1->width,img1->height), IPL_DEPTH_8U, 1);
		imgRed2 = cvCreateImage(cvSize(img2->width,img2->height), IPL_DEPTH_8U, 1);

                // apply the color filter and find pixel percent

		if(camera == 0)
		{
                	pixel_percent = classify_target(img0, imgRed0, color_avg, color_target);

                        // show the quantized image
                        cvNamedWindow("img0", CV_WINDOW_AUTOSIZE);
                        cvShowImage("img0", img0);
                        cvNamedWindow("imgRed0", CV_WINDOW_AUTOSIZE);
                        cvShowImage("imgRed0", imgRed0);	
		}
		else if(camera == 1)
		{
			pixel_percent = classify_target(img1, imgRed1, color_avg, color_target);

                        cvNamedWindow("img1", CV_WINDOW_AUTOSIZE);
                        cvShowImage("img1", img1);
                        cvNamedWindow("imgRed1", CV_WINDOW_AUTOSIZE);
                        cvShowImage("imgRed1", imgRed1);
		}
		else if(camera == 2)
		{
			pixel_percent = classify_target(img2, imgRed2, color_avg, color_target);

                        cvNamedWindow("img2", CV_WINDOW_AUTOSIZE);
                        cvShowImage("img2", img2);
                        cvNamedWindow("imgRed2", CV_WINDOW_AUTOSIZE);
                        cvShowImage("imgRed2", imgRed2);
		}


                /* release memory used by images */
                cvReleaseImage(&imgRed0);
		cvReleaseImage(&imgRed1);
 		cvReleaseImage(&imgRed2);

		return pixel_percent;

}


// MAIN ==================================================================
// =======================================================================

int main() {

  CvCapture* capture0 = 0;
  CvCapture* capture1 = 0;
  CvCapture* capture2 = 0;
  IplImage* frame0_cp = 0;
  IplImage* frame1_cp = 0;
  IplImage* frame2_cp = 0;

  int color_avg_goal[] = {0,0,0};
  int color_avg_worldend[] = {0,0,0};
  int color_avg0[] = {0,0,0};
  int color_avg1[] = {0,0,0};
  int color_avg2[] = {0,0,0};

  double pixel_percent_goal0 = 0;
  double pixel_percent_worldend0 = 0;
  double pixel_percent_goal1 = 0;
  double pixel_percent_worldend1 = 0;
  double pixel_percent_goal2 = 0;
  double pixel_percent_worldend2 = 0;

  IplImage* filtered;

  // Displaying what is in front of camera0 ===============================================
  // ======================================================================================

  while(1)
  {

   capture0 = cvCaptureFromCAM(0);	// CV_CAP_ANY works, 0 works for back cam, 1 works for 						front left, 2 works for front right
   if( !capture0 ) {
     fprintf( stderr, "ERROR: capture0 is NULL \n" );
     getchar();
     return -1;
   }

    IplImage* frame0 = cvQueryFrame( capture0 );
    
    if( !frame0 ) {
      fprintf( stderr, "ERROR: frame0 is null...\n" );
      getchar();
      return -1;
    }

   frame0_cp = cvCloneImage(frame0);

   cvReleaseCapture( &capture0 );

   filtered = cvCreateImage(cvSize(frame0_cp->width,frame0_cp->height), IPL_DEPTH_8U, 1);

   processFrame(frame0_cp,frame0_cp,frame0_cp,0,color_avg0);  // display frames, camera0 only

   // Display average RGB values to terminal window:
   printf("color_avg0(red: %i, green: %i, blue: %i) \n",color_avg0[0],color_avg0[1],color_avg0[2]);


  // Calibration for goal color ============================================================
  // Once complete, press ESC in the frame window and place end of world color in front of camera0 

   printf("Please place goal color in front of camera0 and then click on image window and press ESC \n");

    // breaks if ESC key is pressed, Key=0x10001B under OpenCV 0.9.7(linux version),
    // remove higher bits using AND operator
    if( (cvWaitKey(10) & 255) == 27 ) break;

   } // ends while(1)
  
   
   capture0 = cvCaptureFromCAM(0);	// CV_CAP_ANY works, 0 works for back cam, 1 works for 						front left, 2 works for front right
   if( !capture0 ) {
     fprintf( stderr, "ERROR: capture0 is NULL \n" );
     getchar();
     return -1;
   }

    IplImage* frame0 = cvQueryFrame( capture0 );
    
    if( !frame0 ) {
      fprintf( stderr, "ERROR: frame0 is null...\n" );
      getchar();
      return -1;
    }

   frame0_cp = cvCloneImage(frame0);

   cvReleaseCapture( &capture0 );

   filtered = cvCreateImage(cvSize(frame0_cp->width,frame0_cp->height), IPL_DEPTH_8U, 1);

   processFrame(frame0_cp,frame0_cp,frame0_cp,0,color_avg_goal);  // displays frames, camera0 only

   // Display average RGB values to terminal window:
   printf("color_avg_goal(red: %i, green: %i, blue: %i) \n",color_avg_goal[0],color_avg_goal[1],color_avg_goal[2]);

   printf("GOAL CALIBRATION COMPLETE! \n");


  // Displaying what is in front of camera0 ===============================================
  // ======================================================================================
   
   while(1)
   {

   capture0 = cvCaptureFromCAM(0);	// CV_CAP_ANY works, 0 works for back cam, 1 works for 						front left, 2 works for front right
   if( !capture0 ) {
     fprintf( stderr, "ERROR: capture0 is NULL \n" );
     getchar();
     return -1;
   }

    IplImage* frame0 = cvQueryFrame( capture0 );
    
    if( !frame0 ) {
      fprintf( stderr, "ERROR: frame0 is null...\n" );
      getchar();
      return -1;
    }

   frame0_cp = cvCloneImage(frame0);

   cvReleaseCapture( &capture0 );

   filtered = cvCreateImage(cvSize(frame0_cp->width,frame0_cp->height), IPL_DEPTH_8U, 1);

   processFrame(frame0_cp,frame0_cp,frame0_cp,0,color_avg0);  // camera0 only

   printf("color_avg0(red: %i, green: %i, blue: %i) \n",color_avg0[0],color_avg0[1],color_avg0[2]);


   // Calibration for end of world color =======================================================
   // ==========================================================================================

   printf("Please place end of world color in front of camera0 and then click on image window and press ESC \n");
   
    // breaks if ESC key is pressed, Key=0x10001B under OpenCV 0.9.7(linux version),
    // remove higher bits using AND operator
 
    if( (cvWaitKey(10) & 255) == 27 ) break;

   } // Ends while(1)

   capture0 = cvCaptureFromCAM(0);	// CV_CAP_ANY works, 0 works for back cam, 1 works for 						front left, 2 works for front right
   if( !capture0 ) {
     fprintf( stderr, "ERROR: capture0 is NULL \n" );
     getchar();
     return -1;
   }

    frame0 = cvQueryFrame( capture0 );
    
    if( !frame0 ) {
      fprintf( stderr, "ERROR: frame0 is null...\n" );
      getchar();
      return -1;
    }

   frame0_cp = cvCloneImage(frame0);

   cvReleaseCapture( &capture0 );

   filtered = cvCreateImage(cvSize(frame0_cp->width,frame0_cp->height), IPL_DEPTH_8U, 1);

   processFrame(frame0_cp,frame0_cp,frame0_cp,0,color_avg_worldend);  // camera0 only

   printf("color_avg_worldend(red: %i, green: %i, blue: %i) \n",color_avg_worldend[0],color_avg_worldend[1],color_avg_worldend[2]);

   printf("END OF WORLD CALIBRATION COMPLETE! \n");


  // Show the image captured from all three cameras in the window and repeat.
  // Displays the pixel percentage of all three cameras for the end of world and goal colors.
  while( 1 ) {

   capture0 = cvCaptureFromCAM(0);	// CV_CAP_ANY works, 0 works for back cam, 1 works for 						front left, 2 works for front right
   if( !capture0 ) {
     fprintf( stderr, "ERROR: capture0 is NULL \n" );
     getchar();
     return -1;
   }

    frame0 = cvQueryFrame( capture0 );
    
    if( !frame0 ) {
      fprintf( stderr, "ERROR: frame0 is null...\n" );
      getchar();
      break;
    }

   frame0_cp = cvCloneImage(frame0);

   cvReleaseCapture( &capture0 );

   capture1 = cvCaptureFromCAM(1);	// CV_CAP_ANY works, 0 works for back cam, 1 works for 						front left, 2 works for front right
   if( !capture1 ) {
     fprintf( stderr, "ERROR: capture1 is NULL \n" );
     getchar();
     return -1;
   }

  IplImage* frame1 = cvQueryFrame( capture1 );
    
    if( !frame1 ) {
      fprintf( stderr, "ERROR: frame1 is null...\n" );
      getchar();
      break;
    }

   frame1_cp = cvCloneImage(frame1);
	
   cvReleaseCapture( &capture1 );

   capture2 = cvCaptureFromCAM(2);	// CV_CAP_ANY works, 0 works for back cam, 1 works for 						front left, 2 works for front right
   if( !capture2 ) {
     fprintf( stderr, "ERROR: capture2 is NULL \n" );
     getchar();
     return -1;
   }

  IplImage* frame2 = cvQueryFrame( capture2 );
    
    if( !frame2 ) {
      fprintf( stderr, "ERROR: frame2 is null...\n" );
      getchar();
      break;
    }

   frame2_cp = cvCloneImage(frame2);
	
   cvReleaseCapture( &capture2 );

   // Camera0 ========================================================================

   //pixel_percent_goal0 = classify_target(frame0_cp,filtered,color_avg,color_avg_goal);

   // Displays frames, calculates pixel percentages of goal, for camera0:
   pixel_percent_goal0 = processFrame_target(frame0_cp,frame1_cp,frame2_cp,0,color_avg0,color_avg_goal); 

   printf("pixel_percent_goal0: %f \n", pixel_percent_goal0);
   //printf("color_avg0g(red: %i, green: %i, blue: %i) \n",color_avg0[0],color_avg0[1],color_avg0[2]);
  //printf("color_avg_goal(red: %i, green: %i, blue: %i) \n",color_avg_goal[0],color_avg_goal[1],color_avg_goal[2]);

  // Displays frames, calculates pixel percentages of end of world, for camera0:
   pixel_percent_worldend0 = processFrame_target(frame0_cp,frame1_cp,frame2_cp,0,color_avg0,color_avg_worldend);

   printf("pixel_percent_worldend0: %f \n", pixel_percent_worldend0);
  // printf("color_avg0we(red: %i, green: %i, blue: %i) \n",color_avg0[0],color_avg0[1],color_avg0[2]);
  //printf("color_avg_worldend(red: %i, green: %i, blue: %i) \n",color_avg_worldend[0],color_avg_worldend[1],color_avg_worldend[2]);


   // Camera1 ========================================================================

  // pixel_percent_goal1 = classify_target(frame1_cp,filtered,color_avg1,color_avg_goal);

   pixel_percent_goal1 = processFrame_target(frame0_cp,frame1_cp,frame2_cp,1,color_avg1,color_avg_goal);

   printf("pixel_percent_goal1: %f \n", pixel_percent_goal1);
   //printf("color_avg1(red: %i, green: %i, blue: %i) \n",color_avg1[0],color_avg1[1],color_avg1[2]);

   pixel_percent_worldend1 = processFrame_target(frame0_cp,frame1_cp,frame2_cp,1,color_avg1,color_avg_worldend);

   printf("pixel_percent_worldend1: %f \n", pixel_percent_worldend1);

   // Camera2 ========================================================================

  // pixel_percent_goal2 = classify_target(frame2_cp,filtered,color_avg2,color_avg_goal);

   pixel_percent_goal2 = processFrame_target(frame0_cp,frame1_cp,frame2_cp,2,color_avg2,color_avg_goal);

   printf("pixel_percent_goal2: %f \n", pixel_percent_goal2);
   //printf("color_avg2(red: %i, green: %i, blue: %i) \n",color_avg2[0],color_avg2[1],color_avg2[2]);

   pixel_percent_worldend2 = processFrame_target(frame0_cp,frame1_cp,frame2_cp,2,color_avg2,color_avg_worldend);

   printf("pixel_percent_worldend2: %f \n", pixel_percent_worldend2);
  
  
    // Do not release the frame!

    //If ESC key pressed, Key=0x10001B under OpenCV 0.9.7(linux version),
    //remove higher bits using AND operator
    if( (cvWaitKey(10) & 255) == 27 ) break;

  }


  // Release the capture device housekeeping
  cvReleaseCapture( &capture0 );
  cvReleaseCapture( &capture1 );
  cvReleaseCapture( &capture2 );

  cvDestroyWindow( "img0" );
  cvDestroyWindow( "imgRed0" );
  cvDestroyWindow( "img1" );
  cvDestroyWindow( "imgRed1" );
  cvDestroyWindow( "img2" );
  cvDestroyWindow( "imgRed2" );

  cvReleaseImage(&frame0_cp);
  cvReleaseImage(&frame1_cp);
  cvReleaseImage(&frame2_cp);

  return 0;
}

#ifdef _EiC
main(1,"");
#endif