#include "cvcam.h"
#include "highgui.h"
#include <cv.h>
#include <iostream>

#define WHITE		255
#define BLACK		0
#define THRESHOLD		30
static int index;

static IplImage* backImage =0;
static IplImage* previousImage =0;   //buffer for the previous image
static IplImage* previousImage2 =0;  //buffer for the previous image of previousImage

/**************************************************************************\
                          Labeling Function
\**************************************************************************/ 

IplImage* labeling(IplImage *ImReconst,IplImage *ImEtiqu)
{
    int i,x,y,xyOffset,temp,prec,precCol;
	int *tabEqui;
	int newEtiqu = 100;
	int nbCol = ImReconst->width;
	int nbLine = ImReconst->height;
	int tabEtiqu[256];
	int nbEtiquettes;

	tabEqui = (int*)malloc ((nbCol*nbLine) * sizeof (int));
    ImEtiqu = cvCloneImage(ImReconst);

	//Initalize array
	for( i=0 ; i<(nbCol*nbLine) ; i++ )
	{
		tabEqui[i] = i;
	}
	
	for( i=0 ; i<256 ; i++ )
	{
		tabEtiqu[i] = 0;
	}

     for ( xyOffset=0 ; xyOffset<(ImReconst->imageSize) ; xyOffset++ )
     {
         prec = xyOffset-1;//previous pixel 
         precCol = xyOffset-3*nbCol;//upper pixel 
         if(ImReconst->imageData[xyOffset] == -1)//if treated pixel is white
         {
          //if pixel n-1(previous) and n-nc(upper) are equal
          if(ImEtiqu->imageData[prec] == ImEtiqu->imageData[precCol])
          {
           //if pixel (n-1) and (n-nc) are BLACK
                if((ImEtiqu->imageData[prec] == BLACK) && (ImEtiqu->imageData[precCol] == BLACK))
                {
                //creation of a new label
                     ImEtiqu->imageData[xyOffset] = newEtiqu;
                     if(newEtiqu < 225)           //need to be verified if more than 255...
                        newEtiqu +=30;               
                     else newEtiqu = 30;
                }
                else	//if pixel (n-1) and (n-nc) are equal but not BLACK
                {
                     ImEtiqu->imageData[xyOffset] = tabEqui[ImEtiqu->imageData[prec]];
                     tabEqui[xyOffset-nbCol] = ImEtiqu->imageData[xyOffset];
                }
           }
           else 	 //if pixel (n-1) and (n-nc) already have a label or if they are WHITE
           if((ImEtiqu->imageData[prec] != BLACK) || (ImEtiqu->imageData[precCol] != BLACK))
           {
           //if pixel (n-nc)>(n-1)
                 if(ImEtiqu->imageData[precCol] > ImEtiqu->imageData[prec])
                 {
						//if pixel (n-1) already have a label
						if(ImEtiqu->imageData[prec] != BLACK)
						{
							ImEtiqu->imageData[xyOffset] = tabEqui[ImEtiqu->imageData[prec]];
							ImEtiqu->imageData[precCol] = ImEtiqu->imageData[xyOffset];
						}
						//if pixel (n-1) is BLACK
						else
						{
							ImEtiqu->imageData[xyOffset] = tabEqui[ImEtiqu->imageData[precCol]];
							tabEqui[ImEtiqu->imageData[precCol]] = ImEtiqu->imageData[xyOffset];  
						}
                 }
                 //if pixel (n-nc)<(n-1)
                 else
                 {
						//if pixel (n-nc) is not BLACK
						if(ImEtiqu->imageData[precCol] != BLACK)
						{
							ImEtiqu->imageData[xyOffset] = tabEqui[ImEtiqu->imageData[precCol]];
							ImEtiqu->imageData[xyOffset-3] = ImEtiqu->imageData[xyOffset];
						}
						//if pixel (n-nc) is BLACK
						else
						{
							ImEtiqu->imageData[xyOffset] = tabEqui[ImEtiqu->imageData[prec]];
							tabEqui[xyOffset-1] = tabEqui[ImEtiqu->imageData[xyOffset]];
       					}
			     }
           }
         }
     }

     for ( xyOffset=0 ; xyOffset<ImReconst->imageSize ; xyOffset++ )
     {
         if(ImReconst->imageData[xyOffset] == BLACK)//if current pixel is BLACK
         {
          ImEtiqu->imageData[xyOffset] = BLACK;
         }
     }

	//actualisation of the equivalence array
	for( i=0 ; i<(nbCol*nbLine) ; i++ )
	{
		temp = i;
		do
		{
			temp = tabEqui[temp];
		}
		while(tabEqui[i] != temp);
		tabEqui[i] = temp;
	}
	
	//actualisation of the picture with the labels
     for ( xyOffset=0 ; xyOffset<ImReconst->imageSize ; xyOffset++ )
     {
         if(ImReconst->imageData[xyOffset] == -1)//if current pixel is WHITE
         {
				//incrementation of the label array
				tabEtiqu[ImEtiqu->imageData[xyOffset]] ++;
		 }
	 }
	
	//calculate number of labels
	nbEtiquettes = 0;
	for( i=0 ; i<256 ; i++ )
	{
		if(tabEtiqu[i] != 0)
		{
			nbEtiquettes++;
		}
	}

	//free array
	if(tabEqui != NULL)
	{
		free(tabEqui);
		tabEqui = NULL;
	}

	return(ImEtiqu);
}

/**************************************************************************\
                          Callback Function
\**************************************************************************/ 

void callback(IplImage* image)
{
   IplImage* image1 = image;
   IplImage* grayImage = 0;      
   IplImage* averageImage = 0;   //Resulting image of the average 
   IplImage* sumImage = 0;       //Resulting image of the addition
   IplImage* subImage = 0;       //Resulting image of the substraction
   IplImage* thresholdImage = 0; //Resulting image of the thresolding
   IplImage* erodeImage = 0;     //Resulting image of erosion
   IplImage* dilateImage = 0;    //Resulting image of dilation
   IplImage* rebuildImage = 0;   //Resulting image of rebuilding
   IplImage* labelImage = 0;     //Resulting image of labeling
   int i,j;
   int iheight,iwidth;
   assert (image);
   
   index++;               //count the number of frame
   
   if ( index % 2 != 0)   //Reduce the frame rate by 2
   {
      iheight = image1->height;
      iwidth = image1->width;
   
      /* Print the original image in the window */   
      //cvShowImage("Original window",image1);
   
      /* BINARIZE IMAGE */  //Enter : image1 //Exit : grayImage
      grayImage = cvCreateImage(cvSize(iwidth,iheight),IPL_DEPTH_8U, 1);
      cvConvertImage(image1,grayImage,1);
      //cvShowImage("Binary",grayImage);  
  
      /* Initialize images */
      averageImage = cvCloneImage(grayImage);
      subImage = cvCloneImage(grayImage);
      sumImage = cvCloneImage(grayImage);  
      thresholdImage = cvCloneImage(grayImage); 
      erodeImage = cvCloneImage(grayImage);
      dilateImage = cvCloneImage(grayImage);
      rebuildImage = cvCloneImage(grayImage);
      labelImage = cvCloneImage(grayImage);
      
   
      /*the two first Frame need to be saved in the buffer */
      if (index < 4)
      {
         if(index == 1)
         {
                  backImage = cvCloneImage(grayImage);
         } 
         previousImage = cvCloneImage(grayImage);
         previousImage2 = cvCloneImage(grayImage);  
      }

      /*************************************/
      /************* Treatment *************/
      /*************************************/
   
      /* AVERAGE */ //Enter : grayImage //Exit : averageImage
      cvAdd(previousImage, grayImage, sumImage , NULL);
      cvAdd(sumImage, previousImage, sumImage , NULL);
      for( i=0 ; i<sumImage->widthStep ; i++ )
      {
         averageImage->imageData[i] = sumImage->imageData[i]/3;
      }

      /* SUBSTRACTION */  //Enter : averageImage //Exit : averageImage
      cvSub (backImage, averageImage, subImage, NULL);
      //cvShowImage("Sub",subImage);  
      
      /* THRESHOLDING */  //Enter : subImage //Exit : thresholdImage
      for (int i=0; i<subImage->imageSize; i++)
      {
         if (subImage->imageData[i] < THRESHOLD) thresholdImage->imageData[i] = BLACK;//black pixel
         else thresholdImage->imageData[i] = WHITE;//white pixel 
      }
      //cvShowImage("Threshold",thresholdImage);  
   
      /* EROSION */  //Enter : thresholdImage //Exit : erodeImage
      cvErode(thresholdImage,erodeImage,NULL,1);
      //cvShowImage("Erode",erodeImage);   
      
      /* DILATION */  //Enter : erodeImage //Exit : dilateImage
      cvDilate(erodeImage,dilateImage,NULL,1);
      
      /* REBUILDING */  //Enter : dilateImage //Exit : rebuildImage
      cvDilate(dilateImage,rebuildImage,NULL,2); //2-pass
      //cvShowImage("Rebuild",rebuildImage);
      
      /* LABELING */  //Enter : dilateImage //Exit : erodeImage
      labelImage = labeling(rebuildImage,labelImage);
   
      //save the treated frames in the previous frame for the average
      previousImage2 = cvCloneImage(previousImage);
      previousImage = cvCloneImage(grayImage);
   
      /* Print result on window */  
      cvShowImage("Labeling",labelImage);
   }
}

/**************************************************************************\
                          ShowCamVideo Function
\**************************************************************************/ 

int ShowCamVideo(HWND hwnd, int width, int height)
{
    VidFormat vidFmt={ width, height, 30.0};

    int ncams = cvcamGetCamerasCount( );
    cvcamSetProperty(0, CVCAM_PROP_ENABLE, CVCAMTRUE);
    cvcamSetProperty(0, CVCAM_PROP_CALLBACK,(void*)callback);
    cvcamSetProperty(0, CVCAM_PROP_WINDOW, &hwnd);   

    //Set Video Format Property
    cvcamSetProperty( 0, CVCAM_PROP_SETFORMAT, &vidFmt);

    if( !cvcamInit() )
        return 0;
    cvcamStart();
    return 1;
}

/**************************************************************************\
                          Main Function
\**************************************************************************/ 
int main( int argc, char** argv )
{
    index = 0;
    //cvNamedWindow("Binary", CV_WINDOW_AUTOSIZE);
    //cvNamedWindow("Sub", CV_WINDOW_AUTOSIZE);
    //cvNamedWindow("Threshold", CV_WINDOW_AUTOSIZE);
    //cvNamedWindow("Erode", CV_WINDOW_AUTOSIZE);
    //cvNamedWindow("Rebuild", CV_WINDOW_AUTOSIZE);
    cvNamedWindow("Labeling", CV_WINDOW_AUTOSIZE);
    cvNamedWindow("Original window", CV_WINDOW_AUTOSIZE);
    
    if( ShowCamVideo((HWND)cvGetWindowHandle("Original window"), 640, 480) )
    {
        cvWaitKey(0);
        cvcamStop();
    }
    cvcamExit();
}