VALUE
new_object(CvArr *arr, int is_binary = 0)
{
  VALUE object = rb_allocate(rb_class());
  cvMoments(arr, CVMOMENTS(object), is_binary);
  return object;
}

void trackObject(IplImage* imgThresh){
    // Calculate the moments of 'imgThresh'
    CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments));
    cvMoments(imgThresh, moments, 1);
    double moment10 = cvGetSpatialMoment(moments, 1, 0);
    double moment01 = cvGetSpatialMoment(moments, 0, 1);
    double area = cvGetCentralMoment(moments, 0, 0);

    // if the area<100, I consider that the there are no object in the image and it's because of the noise, the area is not zero 
    if(area>100){
        // calculate the position of the ball
        int posX = moment10/area;
        int posY = moment01/area;        
        
        if(debug>1 && lastX>=0 && lastY>=0 && posX>=0 && posY>=0)
        {
            // Draw a yellow line from the previous point to the current point
            cvLine(imgTracking, cvPoint(posX, posY), cvPoint(lastX, lastY), cvScalar(0,0,255), 4);
        }

        lastX = posX;
        lastY = posY;

        printf("%+05d %+05d\n", posX-halfWidth, posY-halfHeight);
    }else{
        printf("***** *****\n");
    }

    free(moments); 
}

void trackObject(IplImage* imgThresh){
   
	// Kalkulasi momen untuk image yg telah di konvert
    CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments));
    cvMoments(imgThresh, moments, 1);
    double moment10 = cvGetSpatialMoment(moments, 1, 0);
    double moment01 = cvGetSpatialMoment(moments, 0, 1);
    double area = cvGetCentralMoment(moments, 0, 0);

     // jika area <1000, sya memperhitungkan tidak ada objek di gambar karena akan mengganggu, dan areanya tidak nol
	if(area>1000){
		// Menghitung posisi objek
        int posX = moment10/area;
        int posY = moment01/area;        
        
       if(lastX>=0 && lastY>=0 && posX>=0 && posY>=0)
        {
			// Menggambar garis berwarna sesuai dengan HSV di Konfigurasi, dari titik pertama ke titik terakhir
            cvLine(imgTracking, cvPoint(posX, posY), cvPoint(lastX, lastY), cvScalar(lowH,lowS,lowV), 4);
        }

         lastX = posX;
        lastY = posY;
    }

     free(moments); 
} 

IplImage* threshImage(IplImage *imgOrig, CvScalar lower, CvScalar upper, int n)
{
	IplImage* imgHSV = cvCreateImage(cvGetSize(imgOrig), 8, 3);   //size, depth, channels
	cvCvtColor(imgOrig, imgHSV, CV_BGR2HSV);   //check!

	IplImage* imgThresh = cvCreateImage(cvGetSize(imgOrig), 8, 1);
	cvInRangeS(imgHSV, lower, upper, imgThresh);

	
	CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments));
	cvMoments (imgThresh, moments, 1);

	double moment10 = cvGetSpatialMoment(moments, 1, 0);
	double moment01 = cvGetSpatialMoment(moments, 0, 1);
	double area = cvGetSpatialMoment(moments, 0, 0);

	static int posX = 0;
	static int posY = 0;


	posX = moment10/area;
	posY = moment01/area;

	int curX = posX * XRATIO;
	int curY = posY * YRATIO;

	SetCursorPos(1366-curX, curY);

	delete moments;
	cvReleaseImage(&imgHSV);

	return imgThresh;
}

IplImage* getMoment(IplImage* img)
{
		//Calculate Position
		CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments));
		cvMoments(imgRed, moments, 1);
		double moment10 = cvGetSpatialMoment(moments, 1, 0);
		double moment01 = cvGetSpatialMoment(moments, 0, 1);
		double area   = cvGetCentralMoment(moments,0,0);
		static int posX = 0;
		static int posY = 0;
		int lastX = posX;
		int lastY = posY;
		posX = moment10/area;
		posY = moment01/area;
		CvPoint* center = new CvPoint();
		center->x = lastX;
		center->y = lastY;
		printf("%d lastX", lastX);
		printf("%d lastY", lastY);
		if(lastX != posX && lastY != posY)
		{
		  cvDrawCircle(img, *center, 25, CV_RGB(10,10,255), -1);
		}
		
   return 0;
}

/**
 *  Draw a circle in the middle of found areas
 *  @param Image to draw on
 *  @param Image to get moments from
 */
void ColorTracking::DrawPoint(IplImage *frame, IplImage *thresh)
{
	// NONE OF THIS IS USED AT THE MOMENT - THIS IS FOR REFERENCE
    // Calculate the moments to estimate the position of the items
    CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments));
    cvMoments(thresh, moments, 1);

    // The actual moment values
    double moment10 = cvGetSpatialMoment(moments, 1, 0);
    double moment01 = cvGetSpatialMoment(moments, 0, 1);
    double area = cvGetCentralMoment(moments, 0, 0);

    // Holding the last and current ball positions
    static int posX = 0;
    static int posY = 0;

    int lastX = posX;
    int lastY = posY;
    
    posX = moment10/area;
    posY = moment01/area;

    // We want to draw a line only if its a valid position
    if(lastX>0 && lastY>0 && posX>0 && posY>0)
    {
        // Just add a circle to the frame when it finds the color
        //addObjectToVideo(frame, cvPoint(posX, posY));

        // find x distance from middle to object
        drawWidthDiff(frame, cvPoint(posX, posY), middle);
    }
    
    delete moments;
}

void Segment::updateContour()
{
	contour = 0;

	cvFindContours(iplMask, storage, &contour, sizeof(CvContour), CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
	CvMoments moments;
	
//	CvSeq* c = contour;
//cerr << "-------------------------- " << c << endl;
	/*for( CvSeq* c = contour; c!=NULL; c=c->h_next ){
			for(int i = 0; i < c->total; i++){
				CvPoint* p = CV_GET_SEQ_ELEM( CvPoint, c, i );
				cerr << p->x << "," << p->y << endl;
			}
		}
*/
	cvMoments(contour, &moments);
	double m00, m10, m01;
	
	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;
	centroid = cvPoint(center_x, center_y);
}

static COMMAND_FUNC( do_centroid )
{
	OpenCV_Seq *ocv_seq_p;
	ocv_seq_p=PICK_OCV_SEQ("sequence");
	if( ocv_seq_p == NO_OPENCV_SEQ ) return;
	if( ocv_seq_p->ocv_seq == NULL ) {
		sprintf(ERROR_STRING, "do_centroid: sequence is NULL.\n");
		//WARN(ERROR_STRING);
		return;
	}
	CvMoments moments;
	double M00, M01, M10;
	float x,y;
	cvMoments(ocv_seq_p->ocv_seq, &moments, 1);
	M00 = cvGetSpatialMoment(&moments, 0, 0);
	M10 = cvGetSpatialMoment(&moments, 1, 0);
	M01 = cvGetSpatialMoment(&moments, 0, 1);
	x = (int)(M10/M00);
	y = (int)(M01/M00);
	/* char msg[30];
	sprintf(msg, "M00 = %f, M10 = %f, M01 = %f", M00, M10, M01);
	WARN(msg); */

	char number[30];
	sprintf(number, "%f", x);
	ASSIGN_VAR("centroid_x", number);
	sprintf(number, "%f", y);
	ASSIGN_VAR("centroid_y", number);


}

void tracker_bitmap_color::run()
{
  vsx_bitmap *bmp = in_bitmap->get_addr();

  //Check if there is any new image to process
  if(!(bmp && bmp->valid && bmp->timestamp && bmp->timestamp != m_previousTimestamp)){
#ifdef VSXU_DEBUG
    printf("Skipping frame after %d \n",m_previousTimestamp);
#endif
    return;
  }

  m_previousTimestamp = bmp->timestamp;  
  initialize_buffers(bmp->size_x, bmp->size_y);

  //Grab the input image
  m_img[FILTER_NONE]->imageData = (char*)bmp->data;

  //1)filter the image to the HSV color space
  cvCvtColor(m_img[FILTER_NONE],m_img[FILTER_HSV],CV_RGB2HSV);

  //2Threshold the image based on the supplied range of colors
  cvInRangeS( m_img[FILTER_HSV],
              cvScalar( (int)(in_color1->get(0)*255), (int)(in_color1->get(1)*255), (int)(in_color1->get(2)*255) ),
              cvScalar( (int)(in_color2->get(0)*255), (int)(in_color2->get(1)*255), (int)(in_color2->get(2)*255) ),
              m_img[FILTER_HSV_THRESHOLD] );

  //3)Now the math to find the centroid of the "thresholded image"
  //3.1)Get the moments
  cvMoments(m_img[FILTER_HSV_THRESHOLD],m_moments,1);
  double moment10 = cvGetSpatialMoment(m_moments,1,0);
  double moment01 = cvGetSpatialMoment(m_moments,0,1);
  double area = cvGetCentralMoment(m_moments,0,0);

  //3.2)Calculate the positions
  double posX =  moment10/area;
  double posY = moment01/area;

  //3.3) Normalize the positions
  posX = posX/bmp->size_x;
  posY = posY/bmp->size_y;

  //Finally set the result
#ifdef VSXU_DEBUG
  printf("Position: (%f,%f)\n",posX,posY);
#endif
  out_centroid->set(posX,0);
  out_centroid->set(posY,1);

  //Calculate the debug output only if requested
  if(m_compute_debug_out){
    m_compute_debug_out = false;
    cvCvtColor(m_img[FILTER_HSV_THRESHOLD],m_img[FILTER_HSV_THRESHOLD_RGB], CV_GRAY2RGB);

    m_debug = *bmp;
    m_debug.data = m_img[FILTER_HSV_THRESHOLD_RGB]->imageData;
    out_debug->set_p(m_debug);
  }
}

std::vector<int> Contours::getCentroid(){
	 std::vector<int> vec(2);
	
	 CvMoments * myMoments = (CvMoments*)malloc( sizeof(CvMoments) );	 
	 cvMoments( this->c, myMoments );
	 vec[0]=(int) myMoments->m10/myMoments->m00;
	 vec[1]=(int) myMoments->m01/myMoments->m00;
	 
	 free(myMoments);
	 return vec;
}

cv::Point2f BallIdentification::calcBallPose(CvSeq* contour){
    if (contour != NULL)
    {
        CvMoments moments;
        cvMoments(contour, &moments);
        cv::Point2f ballPose =  cv::Point2f(moments.m10/moments.m00, moments.m01/moments.m00);
        return ballPose;

    }
    return cv::Point2f(-1.0, -1.0);
}

static int hu_moments_test( void )
{
    const double success_error_level = 1e-7;
    CvSize size = { 512, 512 };
    int i;
    IplImage* img = atsCreateImage( size.width, size.height, 8, 1, 0 );
    CvMoments moments;
    CvHuMoments a, b;
    AtsRandState rng_state;

    int  seed = atsGetSeed();

    double nu20, nu02, nu11, nu30, nu21, nu12, nu03;
    double err = 0;
    char   buffer[100];

    atsRandInit( &rng_state, 0, 255, seed );
    atsbRand8u( &rng_state, (uchar*)(img->imageData), size.width * size.height );

    cvMoments( img, &moments, 0 );
    atsReleaseImage( img );

    nu20 = cvGetNormalizedCentralMoment( &moments, 2, 0 );
    nu11 = cvGetNormalizedCentralMoment( &moments, 1, 1 );
    nu02 = cvGetNormalizedCentralMoment( &moments, 0, 2 );

    nu30 = cvGetNormalizedCentralMoment( &moments, 3, 0 );
    nu21 = cvGetNormalizedCentralMoment( &moments, 2, 1 );
    nu12 = cvGetNormalizedCentralMoment( &moments, 1, 2 );
    nu03 = cvGetNormalizedCentralMoment( &moments, 0, 3 );

    cvGetHuMoments( &moments, &a );

    b.hu1 = nu20 + nu02;
    b.hu2 = sqr(nu20 - nu02) + 4*sqr(nu11);
    b.hu3 = sqr(nu30 - 3*nu12) + sqr(3*nu21 - nu03);
    b.hu4 = sqr(nu30 + nu12) + sqr(nu21 + nu03);
    b.hu5 = (nu30 - 3*nu12)*(nu30 + nu12)*(sqr(nu30 + nu12) - 3*sqr(nu21 + nu03)) +
            (3*nu21 - nu03)*(nu21 + nu03)*(3*sqr(nu30 + nu12) - sqr(nu21 + nu03));
    b.hu6 = (nu20 - nu02)*(sqr(nu30 + nu12) - sqr(nu21 + nu03)) +
            4*nu11*(nu30 + nu12)*(nu21 + nu03);
    b.hu7 = (3*nu21 - nu03)*(nu30 + nu12)*(sqr(nu30 + nu12) - 3*sqr(nu21 + nu03)) +
            (3*nu12 - nu30)*(nu21 + nu03)*(3*sqr(nu30 + nu12) - sqr(nu21 + nu03));

    for( i = 0; i < 7; i++ )
    {
        double t = rel_err( ((double*)&b)[i], ((double*)&a)[i] );
        if( t > err ) err = t;
    }

    sprintf( buffer, "Accuracy: %.4e", err );
    return trsResult( err > success_error_level ? TRS_FAIL : TRS_OK, buffer );
}

/*
 * call-seq:
 *   CvMoments.new(<i>src[,is_binary = nil]</i>)
 *
 * Calculates all moments up to third order of a polygon or rasterized shape.
 * <i>src</i> should be CvMat or CvPolygon.
 *
 * If is_binary = true, all the zero pixel values are treated as zeroes, all the others are treated as 1's.
 */
VALUE
rb_initialize(int argc, VALUE *argv, VALUE self)
{
  VALUE src, is_binary;
  rb_scan_args(argc, argv, "02", &src, &is_binary);
  if (!NIL_P(src)) {
    if (rb_obj_is_kind_of(src, cCvMat::rb_class()) || rb_obj_is_kind_of(src, cCvSeq::rb_class()))
      cvMoments(CVARR(src), CVMOMENTS(self), TRUE_OR_FALSE(is_binary, 0));
    else
      rb_raise(rb_eTypeError, "argument 1 (src) should be %s or %s.",
	       rb_class2name(cCvMat::rb_class()), rb_class2name(cCvSeq::rb_class()));
  }
  return self;      
}

void getObjectCentre(IplImage* img, int* posX, int *posY)
{
 	// Calculate the moments to estimate the position of the ball
    CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments));
    cvMoments(img, moments, 1);

    // The actual moment values
    double moment10 = cvGetSpatialMoment(moments, 1, 0);
    double moment01 = cvGetSpatialMoment(moments, 0, 1);
    double area = cvGetCentralMoment(moments, 0, 0);

    *posX = moment10/area;
    *posY = moment01/area;
    delete moments;
}

CvPoint FigureFrame::GetCenterOfGravity()
{
    if(this->Matrix.rows <= 0 | this->Matrix.cols <= 0)
        return cvPoint(0,0);
    
    
    IplImage* pImg = new IplImage(this->Matrix);
    
    cvMoments(pImg, &this->moments, 1);

    CvPoint pt;
    pt.x = (int)(this->moments.m10 / this->moments.m00) + this->x;
    pt.y = (int)(this->moments.m01 / this->moments.m00) + this->y;
    
    return pt;
}

/**
 * @internal
 * Extracts the relevant information of the found blobs
 * @note when this method is called, the found blobs should be stored in m_blobs member
 */
void BlobFinder::extractBlobsInformation()
{
  // Order blobs (from bigger to smaller) -> this way the most relevant are at the beginning
  std::sort( m_blobs.begin(), m_blobs.end(), std::greater< Blob >() );

  // Discard blobs (if there is more than the max)
  // TODO

  // To store contour moments
  CvMoments moment;

  // to read contour points
	CvSeqReader contourReader;
	CvPoint		  contourNode;

  // Calculate information about contours
  for( Blobs::size_type i = 0; (i < m_blobs.size()) && (i < m_maxBlobs); ++i )
  {
      // Current blob
      Blob& blob = m_blobs[ i ];

      // Get bbox
      blob.bbox = cvBoundingRect( blob.contour );

      // Get center through moments
      cvMoments( blob.contour, &moment );
      blob.center.x = (float)(moment.m10 / moment.m00);
      blob.center.y = (float)(moment.m01 / moment.m00);

      // Invert Y coordinate because our Y 0 is at top of the image,
      // and for Opencv is at the bottom of the image
      //blob.center.Y = inImage.GetHeight() - blob.center.Y;

    // Store the contour nodes
    cvStartReadSeq( blob.contour, &contourReader );
    for( int j = 0; j < blob.contour->total; ++j )
    {
	    // Read node of the contour
	    CV_READ_SEQ_ELEM( contourNode, contourReader );
	    blob.nodes.push_back( Point( (float)contourNode.x, (float)contourNode.y , 0) );
    }
  }

	// Store number of actual blobs
	m_nBlobs = min( (int)m_blobs.size(), (int)m_maxBlobs );
}

void ThreadAnalyse::run() {
	int img_step = field->img->widthStep;
	int img_channels = field->img->nChannels;
	uchar * img_data = (uchar *) field->img->imageData;
	char key = -1;

	field->treat->origin = field->img->origin;
	int filtered_step = field->treat->widthStep;
	int filtered_channels = field->treat->nChannels;
	uchar * filtered_data_b = (uchar *) field->treat->imageData;

	for (int i = 0; i < field->img->width; i++) {
		for (int j = 0; j < field->img->height; j++) {
			if (i > field->box[XMIN] && i < field->box[XMAX] && j
					> field->box[YMIN] && j < field->box[YMAX] && (img_data[j
					* img_step + i * img_channels]) > field->color[MINB]
					&& (img_data[j * img_step + i * img_channels])
							< field->color[MAXB] && (img_data[j * img_step + i
					* img_channels + 1]) > field->color[MING] && (img_data[j
					* img_step + i * img_channels + 1]) < field->color[MAXG]
					&& (img_data[j * img_step + i * img_channels + 2])
							> field->color[MINR] && (img_data[j * img_step + i
					* img_channels + 2]) < field->color[MAXR]) {
				filtered_data_b[j * filtered_step + i * filtered_channels]
						= 255;
			} else {
				filtered_data_b[j * filtered_step + i * filtered_channels] = 0;
			}
		}
	}

	//cvErode(field->treat, field->treat, 0, 1);

	cvMoments(field->treat, moments, 1);

	// The actual moment values B
	double moment10 = cvGetSpatialMoment(moments, 1, 0);
	double moment01 = cvGetSpatialMoment(moments, 0, 1);
	double area = cvGetCentralMoment(moments, 0, 0);

	field->position->x = (int) moment10 / area;
	field->position->y = (int) moment01 / area;

	pthread_exit(NULL);
}

double BlobContour::GetMoment(int p, int q)
{
	// is a valid moment?
	if (p < 0 || q < 0 || p > MAX_MOMENTS_ORDER || q > MAX_MOMENTS_ORDER) {
		return -1;
	}

	if (IsEmpty())
		return 0;

	// it is calculated?
	if (m_moments.m00 == -1) {
		cvMoments(GetContourPoints(), &m_moments);
	}

	return cvGetSpatialMoment(&m_moments, p, q);

}

/****************************************************************
** fit_params ***************************************************
****************************************************************/
CFitParamsReturnType fit_params( CvMoments *pState,
                                 double *x0, double *y0,
                                 double *Mu00,
                                 double *Uu11, double *Uu20, double *Uu02,
                                 int width, int height,
                                 unsigned char *img, int img_step )
{
  double Mu10, Mu01;
  double val;
  CvMat arr;

  arr = fi2cv_view(img, img_step, width, height);

  /* get moments */
  int binary=0; // not a binary image?
  CHKCV( cvMoments( &arr, pState, binary ), CFitParamsOtherError);

  /* calculate center of gravity from spatial moments */
  CHKCV( val = cvGetSpatialMoment( pState, 0, 0), CFitParamsOtherError);
  *Mu00 = val;

  CHKCV( Mu10 = cvGetSpatialMoment( pState, 1, 0), CFitParamsOtherError);
  CHKCV( Mu01 = cvGetSpatialMoment( pState, 0, 1), CFitParamsOtherError);

  if( *Mu00 != 0.0 )
  {
    *x0 = Mu10 / *Mu00;
    *y0 = Mu01 / *Mu00;
    /* relative to ROI origin */
  }
  else
  {
    return CFitParamsZeroMomentError;
  }

  /* calculate blob orientation from central moments */
  CHKCV( val = cvGetCentralMoment( pState, 1, 1), CFitParamsCentralMomentError);
  *Uu11 = val;
  CHKCV( val = cvGetCentralMoment( pState, 2, 0), CFitParamsCentralMomentError);
  *Uu20 = val;
  CHKCV( val = cvGetCentralMoment( pState, 0, 2), CFitParamsCentralMomentError);
  *Uu02 = val;
  return CFitParamsNoError;
}

IplImage* contour(IplImage* img)
{
    static int i;
    char fileName[20];
    CvMemStorage* store;
    IplImage* aux=NULL;
 
    if(aux == NULL)
    {
        aux = cvCreateImage(cvGetSize(img),8,1);
        store = cvCreateMemStorage(0);
    }  
    CvSeq * contours =0;
    cvFindContours(img,store,&contours);  //finding contours in an image
    cvZero(aux);
    //if(contours->total)
    {
      cvDrawContours(aux,contours,cvScalarAll(255),cvScalarAll(255),100);
    } 
     
    CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments));
    double M00, M01, M10;
    fruitCount=0;
    while(contours!=NULL)   //detects the moments means coords of individual contours
    {
        if( cvContourArea(contours,CV_WHOLE_SEQ) < 5 ) //detects only sizable objects
        {	
	        contours = contours->h_next;
	        continue;
	    }
        cvMoments(contours, moments);          
        M00 = cvGetSpatialMoment(moments,0,0); 
        M10 = cvGetSpatialMoment(moments,1,0); 
        M01 = cvGetSpatialMoment(moments,0,1); 
        centers[fruitCount].x = (int)(M10/M00);            //global variable, stores the centre coords of an object
        centers[fruitCount].y = (int)(M01/M00); 
        fruitCount++;                                          //important global variable, it represents the total no. of objects detected in the image if it is zero the no action :)
        contours = contours->h_next;
    }
    cvClearMemStorage(store);
    return aux;
}