bool EdisonSegmModule::updateModule()
{
    ImageOf<PixelRgb> *yrpImgIn;
    static int cycles = 0;

    yrpImgIn = _imgPort.read();
    if (yrpImgIn == NULL)   // this is the case if module is requested to quit while waiting for image
        return true;

    bool use_private_stamp;
    Stamp s;
    if(!_imgPort.getEnvelope(s))
    {
            cout << "No stamp found in input image. Will use private stamp" << endl;
            use_private_stamp = true;
    }
    else
    {
            cout << "Received image #" << s.getCount() << " generated at time " << s.getTime() << endl;
            use_private_stamp = false;
    }

    if(cycles == 0)
            _timestart = yarp::os::Time::now();
    cycles++;

    IplImage *iplimg = (IplImage*)yrpImgIn->getIplImage();

    //computing the ROI to crop the image
	/*struct _IplROI roi;
	roi.coi = 0; // all channels are selected
	roi.height = height_;
	roi.width = width_;
	roi.xOffset = ( orig_width_ - width_ ) / 2;
	roi.yOffset = ( orig_height_ - height_ ) / 2;*/
	
	//copying roi data to buffer
	/*iplimg->roi = &roi;
	cvCopy( iplimg, inputImage.getIplImage());*/

    //Rescale image if required
    if( (width_ != orig_width_) || (height_ != orig_height_ ) )
            cvResize(iplimg, inputImage.getIplImage(), CV_INTER_NN);
    else
            cvCopy( iplimg, inputImage.getIplImage());

    double edgetime = yarp::os::Time::now();
    //compute gradient and confidence maps
    BgEdgeDetect edgeDetector(gradWindRad);
    BgImage bgImage;
    bgImage.SetImage(inputImage_, width_, height_, true);
    edgeDetector.ComputeEdgeInfo(&bgImage, confMap_, gradMap_);
    //compute the weigth map
    for(int i = 0; i < width_*height_; i++) {
      if(gradMap_[i] > 0.02) {
		weightMap_[i] = mixture*gradMap_[i] + (1 - mixture)*confMap_[i];
      } else {
		weightMap_[i] = 0;
      }
    }
	///////////////////////////// This block can be parallelized
	cout << "Edge computation Time (ms): " << (yarp::os::Time::now() - edgetime)*1000.0 << endl;

	msImageProcessor iProc;
	if( dim_ == 3 )
		iProc.DefineImage(inputImage_, COLOR, height_, width_);
	else
	{	
		cvCvtColor(inputImage.getIplImage(), inputHsv.getIplImage(), CV_RGB2HSV);
		cvSplit(inputHsv.getIplImage(), inputHue.getIplImage(), 0, 0, 0);
		iProc.DefineImage(inputHue_, GRAYSCALE, height_, width_);
	}
	if(iProc.ErrorStatus) {
		cout << "MeanShift Error" << endl;
		return false;
	}
	iProc.SetWeightMap(weightMap_, threshold);
	if(iProc.ErrorStatus) {
		cout << "MeanShift Error" << endl;
		return false;
	}


	double filtertime = yarp::os::Time::now();
	iProc.Filter(sigmaS, sigmaR, speedup);
        if(iProc.ErrorStatus) {
		cout << "MeanShift Error" << endl;
		return false;
	}
	cout << "Mean Shift Filter Computation Time (ms): " << (yarp::os::Time::now() - filtertime)*1000.0 << endl;


    //obtain the filtered image
    iProc.GetResults(filtImage_);
	if(iProc.ErrorStatus) {
		cout << "MeanShift Error" << endl;
		return false;
	}
    
    //fuse regions
	double fusetime = yarp::os::Time::now();
    iProc.FuseRegions(sigmaR, minRegion);
    if(iProc.ErrorStatus) {
		cout << "MeanShift Error" << endl;
		return false;
	}
	cout << "Region Fusion Computation Time (ms): " << (yarp::os::Time::now() - fusetime)*1000.0 << endl;

    //obtain the segmented image
    iProc.GetResults(segmImage_);
    if(iProc.ErrorStatus) {
		cout << "MeanShift Error" << endl;
		return false;
	}

	//define the boundaries - do not need this
	/*
	RegionList *regionList        = iProc.GetBoundaries();
	int        *regionIndeces     = regionList->GetRegionIndeces(0);
	int        numRegions         = regionList->GetNumRegions();
	numBoundaries_ = 0;
	for(int i = 0; i < numRegions; i++) {
		numBoundaries_ += regionList->GetRegionCount(i);
	}
	if(boundaries_) delete [] boundaries_;
	boundaries_ = new int [numBoundaries_];
	for(int i = 0; i < numBoundaries_; i++) {
		boundaries_[i] = regionIndeces[i];
	}*/
		
	int regionCount; // how many regions have been found
	int *labels = NULL; //pointer for the labels (should this be released in the end?) 
	float *modes; //pointer for the Luv values (should this be released in the end?) 
	int *modePointCounts; //the area of each region (should this be released in the end?) 

	regionCount = iProc.GetRegions(&labels, &modes, &modePointCounts);
	int *labelp = (int*)labelImage.getRawImage();
	for(int i = 0; i < width_*height_; i++)
		labelp[i] = labels[i];
	
	IplImage *labelint = (IplImage*)labelImage.getIplImage();
	IplImage *labelchar = (IplImage*)labelView.getIplImage();

	cvConvert(labelint, labelchar);

	//prepare timestamps
	if(use_private_stamp)
	{
		_stamp.update();
		_labelPort.setEnvelope(_stamp);
		_labelViewPort.setEnvelope(_stamp);
		_viewPort.setEnvelope(_stamp);
		_filtPort.setEnvelope(_stamp);
		_rawPort.setEnvelope(_stamp);
	}
	else
	{
		_labelPort.setEnvelope(s);
		_labelViewPort.setEnvelope(s);
		_viewPort.setEnvelope(s);
		_filtPort.setEnvelope(s);
		_rawPort.setEnvelope(s);
	}

	ImageOf<PixelInt> &yrpImgLabel = _labelPort.prepare();
	//Rescale image if required
	if( (width_ != orig_width_) || (height_ != orig_height_ ) )
	{
		yrpImgLabel.resize(orig_width_, orig_height_);
		cvResize(labelImage.getIplImage(), yrpImgLabel.getIplImage(), CV_INTER_NN);
	}
	else
		yrpImgLabel = labelImage;
	_labelPort.write();

	ImageOf<PixelMono> &yrpImgDebug = _labelViewPort.prepare();
	//Rescale image if required
	if( (width_ != orig_width_) || (height_ != orig_height_ ) )
	{
		yrpImgDebug.resize(orig_width_, orig_height_);
		cvResize(labelView.getIplImage(), yrpImgDebug.getIplImage(), CV_INTER_NN);
	}
	else
		yrpImgDebug = labelView;
	_labelViewPort.write();


	ImageOf<PixelRgb> &yrpFiltOut = _filtPort.prepare();
	//Rescale image if required
	if( (width_ != orig_width_) || (height_ != orig_height_ ) )
	{
		yrpFiltOut.resize(orig_width_, orig_height_);
		cvResize(filtImage.getIplImage(), yrpFiltOut.getIplImage(), CV_INTER_NN);
	}
	else
		yrpFiltOut = filtImage;
	_filtPort.write();

	ImageOf<PixelRgb> &yrpImgView = _viewPort.prepare();
	//Rescale image if required
	if( (width_ != orig_width_) || (height_ != orig_height_ ) )
	{
		yrpImgView.resize(orig_width_, orig_height_);
		cvResize(segmImage.getIplImage(), yrpImgView.getIplImage(), CV_INTER_NN);
	}
	else
		yrpImgView = segmImage;
	_viewPort.write();

	ImageOf<PixelRgb> &yrpImgOut = _rawPort.prepare();
	yrpImgOut = *yrpImgIn;
	_rawPort.write();


	//report the frame rate
	if(cycles % 100 == 0)
	{
		double cps = ((double)cycles)/(yarp::os::Time::now() - _timestart);
		printf("fps: %02.2f\n", cps);
	}
    return true;
}
/*
 * main enterance point 
 */
void mexFunction(
    int		  nlhs, 	/* number of expected outputs */
    mxArray	  *plhs[],	/* mxArray output pointer array */
    int		  nrhs, 	/* number of inputs */
    const mxArray	  *prhs[]	/* mxArray input pointer array */
    )
{
    if ( nrhs != 3 )
        mexErrMsgIdAndTxt("edison_wraper:main","Must have three inputs");
    if ( mxGetClassID(prhs[0]) != mxSINGLE_CLASS )
        mexErrMsgIdAndTxt("edison_wraper:main","fim must be of type single");
    if ( mxGetClassID(prhs[1]) != mxUINT8_CLASS )
        mexErrMsgIdAndTxt("edison_wraper:main","rgbim must be of type uint8");
    if ( mxGetClassID(prhs[2]) != mxSTRUCT_CLASS )
        mexErrMsgIdAndTxt("edison_wraper:main","parameters argument must be a structure");
    
    
    /* first argument - the image in whatever feature space it is given in */
    float * fimage = (float*)mxGetData(prhs[0]);
    const int* image_dims = mxGetDimensions(prhs[0]);
    int image_ndim = mxGetNumberOfDimensions(prhs[0]);
    unsigned int w = image_dims[1];
    unsigned int h = image_dims[2];
    unsigned int N = image_dims[0];
    int ii;
    
    unsigned char * rgbim = (unsigned char*)mxGetData(prhs[1]);
    const int * rgb_dims = mxGetDimensions(prhs[1]);
    if ( rgb_dims[1] != w || rgb_dims[2] != h )
        mexErrMsgIdAndTxt("edison_wraper:main","size of fim and rgbim do not match");
    
    /* second input - parameters structure */
    //   'steps'       - What steps the algorithm should perform:
    //               1 - only mean shift filtering
    //               2 - filtering and region fusion
    //               3 - full segmentation process [default]
    int steps;
    GetScalar(mxGetField(prhs[2], 0, "steps"), steps);
    
    //   'synergistic' - perform synergistic segmentation [true]|false
    bool syn;
    GetScalar(mxGetField(prhs[2], 0, "synergistic"), syn);
    
    //   'SpatialBandWidth' - segmentation spatial radius (integer) [7]
    unsigned int spBW;
    GetScalar(mxGetField(prhs[2], 0, "SpatialBandWidth"), spBW);

    //  'RangeBandWidth'   - segmentation feature space radius (float) [6.5]
    float fsBW;
    GetScalar(mxGetField(prhs[2], 0, "RangeBandWidth"), fsBW);

    //   'MinimumRegionArea'- minimum segment area (integer) [20]
    unsigned int minArea;
    GetScalar(mxGetField(prhs[2], 0, "MinimumRegionArea"), minArea);
    
    //   'SpeedUp'          - algorithm speed up {0,1,2} [1]
    int SpeedUp;
    enum SpeedUpLevel sul;
    GetScalar(mxGetField(prhs[2], 0, "SpeedUp"), SpeedUp);
    switch (SpeedUp) {
        case 1:
            sul = NO_SPEEDUP;
            break;
        case 2:
            sul = MED_SPEEDUP;
            break;
        case 3:
            sul = HIGH_SPEEDUP;
            break;
        default:
            mexErrMsgIdAndTxt("edison_wraper:main","wrong speedup value");
    }
    
    //   'GradientWindowRadius' - synergistic parameters (integer) [2]
    unsigned int grWin;
    GetScalar(mxGetField(prhs[2], 0, "GradientWindowRadius"), grWin);
    
    //   'MixtureParameter' - synergistic parameter (float 0,1) [.3]
    float aij;
    GetScalar(mxGetField(prhs[2], 0, "MixtureParameter"), aij);
    
    //   'EdgeStrengthThreshold'- synergistic parameter (float 0,1) [.3]
    float edgeThr;
    GetScalar(mxGetField(prhs[2], 0, "EdgeStrengthThreshold"), edgeThr);
    
    msImageProcessor ms;
    ms.DefineLInput(fimage, h, w, N); // image array should be after permute
    if (ms.ErrorStatus)
        mexErrMsgIdAndTxt("edison_wraper:edison","Mean shift define latice input: %s", ms.ErrorMessage);
    
    kernelType k[2] = {DefualtKernelType, DefualtKernelType};
    int P[2] = {DefualtSpatialDimensionality, N};
    float tempH[2] = {1.0, 1.0};
    ms.DefineKernel(k, tempH, P, 2); 
    if (ms.ErrorStatus)
        mexErrMsgIdAndTxt("edison_wraper:edison","Mean shift define kernel: %s", ms.ErrorMessage);

    mxArray * mxconf = NULL;
    float * conf = NULL;
    mxArray * mxgrad = NULL;
    float * grad = NULL;
    mxArray * mxwght = NULL;
    float * wght = NULL;
    
    if (syn) {
        /* perform synergistic segmentation */
        int maps_dim[2] = {w*h, 1};
        /* allcate memory for confidence and gradient maps */
        mxconf = mxCreateNumericArray(2, maps_dim, mxSINGLE_CLASS, mxREAL);
        conf = (float*)mxGetData(mxconf);
        mxgrad = mxCreateNumericArray(2, maps_dim, mxSINGLE_CLASS, mxREAL);
        grad = (float*)mxGetData(mxgrad);
        
        BgImage rgbIm;
        rgbIm.SetImage(rgbim, w, h, rgb_dims[0] == 3);
        BgEdgeDetect edgeDetector(grWin);
        edgeDetector.ComputeEdgeInfo(&rgbIm, conf, grad);
        
        mxwght = mxCreateNumericArray(2, maps_dim, mxSINGLE_CLASS, mxREAL);
        wght = (float*)mxGetData(mxgrad);
        
        for ( ii = 0 ; ii < w*h; ii++ ) {
            wght[ii] = (grad[ii] > .002) ? aij*grad[ii]+(1-aij)*conf[ii] : 0;
        }
        ms.SetWeightMap(wght, edgeThr);
        if (ms.ErrorStatus)
            mexErrMsgIdAndTxt("edison_wraper:edison","Mean shift set weights: %s", ms.ErrorMessage);

    }
    ms.Filter(spBW, fsBW, sul);
    if (ms.ErrorStatus)
        mexErrMsgIdAndTxt("edison_wraper:edison","Mean shift filter: %s", ms.ErrorMessage);

    if (steps == 2) {
        ms.FuseRegions(fsBW, minArea);
        if (ms.ErrorStatus)
            mexErrMsgIdAndTxt("edison_wraper:edison","Mean shift fuse: %s", ms.ErrorMessage);
    }
    
    if ( nlhs >= 1 ) {
        // first output - the feture space raw image
        plhs[0] = mxCreateNumericArray(image_ndim, image_dims, mxSINGLE_CLASS, mxREAL);
        fimage = (float*)mxGetData(plhs[0]);
        ms.GetRawData(fimage);
    }
    
    int* labels;
    float* modes;
    int* count;
    int RegionCount = ms.GetRegions(&labels, &modes, &count);

    if ( nlhs >= 2 ) {
        // second output - labeled image
        plhs[1] = mxCreateNumericArray(2, image_dims+1, mxINT32_CLASS, mxREAL);
        int* plabels = (int*)mxGetData(plhs[1]);
        for (ii=0; ii< w*h; ii++)
            plabels[ii] = labels[ii];
    }
    delete [] labels;
    int arr_dims[2];
    if ( nlhs >= 3 ) {
        // third output - the modes
        arr_dims[0] = N;
        arr_dims[1] = RegionCount;
        plhs[2] = mxCreateNumericArray(2, arr_dims, mxSINGLE_CLASS, mxREAL);
        fimage = (float*)mxGetData(plhs[2]);
        for (ii=0;ii<N*RegionCount; ii++)
            fimage[ii] = modes[ii];
    }
    delete [] modes;
    
    if ( nlhs >= 4 ) {
        // fourth output - region sizes (# of pixels)
        arr_dims[0] = 1;
        arr_dims[1] = RegionCount;
        plhs[3] = mxCreateNumericArray(2, arr_dims, mxINT32_CLASS, mxREAL);
        int * pc = (int*)mxGetData(plhs[3]);
        for (ii=0;ii<RegionCount; ii++)
            pc[ii] = count[ii];
    }   
    delete [] count;
    
    if ( !syn )
        return;
    
    if ( nlhs >= 5)
        // fifth output - gradient map
        plhs[4] = mxgrad;
    else
        mxDestroyArray(mxgrad);
    
    if ( nlhs >= 6)
        plhs[5] = mxconf;
    else
        mxDestroyArray(mxconf);
   
}