#include "cv.h"

#include "highgui.h"

#include <stdio.h>

#include <ctype.h>

IplImage* image;

IplImage* lastImage;

IplImage* diffImage;

IplImage* bitImage;

double m00, m10, m01;

int p1,p2,c;

int tr=40;

/*void SetCaptureProperties(CvCapture* capture)

{

    cvSetCaptureProperty(capture, CV_CAP_PROP_FPS,24);

    cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH,640);

    cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT,480);

}

*/

void showCaptureProperties(CvCapture* capture)

{

    int  w,h;

    double f;

        w=(int)cvGetCaptureProperty(capture,CV_CAP_PROP_FRAME_WIDTH);

        h=(int)cvGetCaptureProperty(capture,CV_CAP_PROP_FRAME_HEIGHT);

        f=(int)cvGetCaptureProperty(capture,CV_CAP_PROP_FPS);

        printf("Capture properties (widthxheight - frames): %dx%d - %lf\n",w,h,f);

}

int main( int argc, char** argv )

{

    CvCapture* capture = 0;

    capture = cvCaptureFromCAM(0);

    cvSetCaptureProperty(capture, CV_CAP_PROP_FOURCC,'Y411');

    cvSetCaptureProperty(capture, CV_CAP_PROP_FPS,-1);

    cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH,640.0);

    cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT,480.0);

    //SetCaptureProperties(capture);

  // cvNamedWindow( "CamSub 1",1);

   //        cvCreateTrackbar("Threshold","CamSub 1",&tr,255,NULL); //creates trackbar 

  //  cvNamedWindow( "CamMorph",1);

    cvQueryFrame(capture);

    showCaptureProperties(capture);

for(;;)

    {

        IplImage* frame = 0;

                frame = cvQueryFrame( capture );

        if( !frame )

            break;

                //If is the first frame

                if(!image){

                image=cvCreateImage(cvSize(frame->width,frame->height),frame->depth,1);

                bitImage=cvCreateImage(cvSize(frame->width,frame->height),frame->depth,1);

                }

                cvCvtColor(frame,image,CV_BGR2GRAY);

        if(!lastImage)

        {

            //If no lastImage clone actual image;

                        lastImage=cvCloneImage(image);

                }

                 if(!diffImage)

                       {

                                        //Create image header same as frame but with 1 channel to gray

                                          diffImage=cvCreateImage(cvSize(frame->width,frame->height),frame->depth,1);

                            }

       //treshold image and store it into bitImage

        cvThreshold(image,bitImage,tr,255,CV_THRESH_BINARY_INV);

// create opening kernel

        IplConvKernel* kernel;

        kernel = cvCreateStructuringElementEx(38, 38, 1, 1, CV_SHAPE_ELLIPSE, NULL);

//Morphological opening of bitImage using a circular kernel and saving it into diffImage

                cvMorphologyEx(bitImage, diffImage, NULL, kernel, CV_MOP_OPEN, 1);

//showing the videos in the windows created

 //   cvShowImage("CamSub 1",bitImage);

 //  cvShowImage("CamMorph",diffImage);

//Calculations of the centers of gravity of the blobs            

      CvMoments moments;

      cvMoments(diffImage, &moments, 1);

      m00 = cvGetSpatialMoment(&moments, 0,0);

      m10 = cvGetSpatialMoment(&moments, 1,0);

      m01 = cvGetSpatialMoment(&moments, 0,1);

// TBD check that m00 != 0

 float center_x = m10/m00;

 float center_y = m01/m00;

p1 = (int)center_x;

p2 = (int)center_y;

//cast the centers into int numbers

  IplImage* image_rgb = cvCreateImage( cvSize(diffImage->width, diffImage->height), diffImage->depth, 3 );

     //cvCopy(diffImage,image_rgb,NULL);

 cvCircle(image_rgb ,cvPoint(p1,p2), 2, CV_RGB(5,255,255),3,8,0);

 cvShowImage("Map",image_rgb);       

 printf("center x %.1f , center y %.1f\n ",center_x,center_y);    

        c = cvWaitKey(10);

        if( (char) c == 27 )

            break;

            }

    cvReleaseCapture( &capture );

    cvDestroyWindow("CamSub1");

   // cvDestroyWindow("Map");

   // cvDestroyWindow("CamMorph");

    return 0;

}