#include <Eigen/Eigen>
#include <pcl/point_cloud.h>
#include <pcl/features/feature.h>
#include <pcl/common/common.h>

#include <string>
#include <climits>

//#include <ndt_map/oc_tree.h>
#include <ndt_map/ndt_map.h>
#include <ndt_map/lazy_grid.h>
#include <ndt_map/cell_vector.h>

#include <cstring>
#include <cstdio>

namespace perception_oru
{
/**
* loadPointCloud - You can call this if you are only interested in dealing with one scan
* without need for fusing several ones or representing empty space and occupancy
*
* Otherwise you should always call addPointCloud (or if you don't want occupancy then addPointCloudSimple)
*
* \param pc the PointCloud that is to be loaded
* \note every subsequent call will destroy the previous map!
*/
void NDTMap::loadPointCloud(const pcl::PointCloud<pcl::PointXYZ> &pc, double range_limit)
{
    if(index_ != NULL)
    {
        //std::cout<<"CLONE INDEX\n";
        SpatialIndex *si = index_->clone();
        //cout<<"allocating index\n";
        if(!isFirstLoad_)
        {
            //std::cout<<"deleting old index\n";
            delete index_;
        }
        isFirstLoad_ = false;
        index_ = si;
    }
    else
    {
        //NULL index in constructor, abort!
        //ERR("constructor must specify a non-NULL spatial index\n");
        return;
    }

    LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
    if(lz == NULL)
    {
	fprintf(stderr,"Unfortunately This works only with Lazygrid!\n");
	exit(1);
    }

    if(index_ == NULL)
    {
        //ERR("Problem creating index, unimplemented method\n");
        return;
    }

    pcl::PointCloud<pcl::PointXYZ>::const_iterator it = pc.points.begin();
    if(guess_size_)
    {
	double maxDist = 0;//, distCeil = 200;

	Eigen::Vector3d centroid(0,0,0);
	int npts = 0;
	while(it!=pc.points.end())
	{
	    Eigen::Vector3d d;
	    if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
	    {
		it++;
		continue;
	    }
	    d << it->x, it->y, it->z;
	    if(range_limit>0)
	    {
		if(d.norm()>range_limit)
		{
		    it++;
		    continue;
		}
	    }
	    centroid += d;
	    it++;
	    npts++;
	}

	centroid /= (double)npts;
	double maxz=-1000, minz=10000;
	//Eigen::Vector4f centroid(0,0,0,0);
	//pcl::compute3DCentroid(pc,centroid);

	//compute distance to furthest point
	it = pc.points.begin();
	while(it!=pc.points.end())
	{
	    Eigen::Vector3d d;
	    if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
	    {
		it++;
		continue;
	    }
	    if(range_limit>0)
	    {
		d << it->x, it->y, it->z;
		if(d.norm()>range_limit)
		{
		    it++;
		    continue;
		}
	    }
	    d << centroid(0)-it->x, centroid(1)-it->y, centroid(2)-it->z;
	    double dist = d.norm();
	    maxDist = (dist > maxDist) ? dist : maxDist;
	    maxz = ((centroid(2)-it->z) > maxz) ? (centroid(2)-it->z) : maxz;
	    minz = ((centroid(2)-it->z) < minz) ? (centroid(2)-it->z) : minz;
	    it++;
	}
	 //std::cout<<"Points = " <<pc.points.size()<<" maxDist = "<<maxDist<<std::endl;
	NDTCell *ptCell = new NDTCell();
	index_->setCellType(ptCell);
	delete ptCell;
	index_->setCenter(centroid(0),centroid(1),centroid(2));

	if(map_sizex >0 && map_sizey >0 && map_sizez >0)
	{
	    index_->setSize(map_sizex,map_sizey,map_sizez);
	}
	else
	{
	    index_->setSize(4*maxDist,4*maxDist,3*(maxz-minz));
	}
    }
    else
    {
	//set sizes
	NDTCell *ptCell = new NDTCell();
	index_->setCellType(ptCell);
	delete ptCell;
	index_->setCenter(centerx,centery,centerz);
	if(map_sizex >0 && map_sizey >0 && map_sizez >0)
	{
	    index_->setSize(map_sizex,map_sizey,map_sizez);
	}
    }

    //    ROS_INFO("centroid is %f,%f,%f", centroid(0),centroid(1),centroid(2));
    //    ROS_INFO("maxDist is %lf", maxDist);

    it = pc.points.begin();
    while(it!=pc.points.end())
    {
        Eigen::Vector3d d;
        if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
        {
            it++;
            continue;
        }
        if(range_limit>0)
        {
            d << it->x, it->y, it->z;
            if(d.norm()>range_limit)
            {
                it++;
                continue;
            }
        }
        index_->addPoint(*it);
	NDTCell *ptCell;
	lz->getNDTCellAt(*it,ptCell);
#ifdef REFACTORED
	if(ptCell!=NULL) {
	//    std::cerr<<"adding to update set\n";
	    update_set.insert(ptCell);
	}
#endif
        it++;
    }

    isFirstLoad_ = false;
}

/**
* loadPointCloudCentroid - A special load function to enable the matching of centroids (create alligned maps)
* \param pc the PointCloud that is to be loaded
* \note every subsequent call will destroy the previous map!
*/
void NDTMap::loadPointCloudCentroid(const pcl::PointCloud<pcl::PointXYZ> &pc, const Eigen::Vector3d &origin,
	const Eigen::Vector3d &old_centroid,const Eigen::Vector3d &map_size, double range_limit){

    if(index_ != NULL){
        SpatialIndex *si = index_->clone();
        if(!isFirstLoad_) delete index_;

        isFirstLoad_ = false;
        index_ = si;
    }
    else{
	return;
    }

    if(index_ == NULL) return; ///Should never happen!

    NDTCell *ptCell = new NDTCell();
    index_->setCellType(ptCell);
    delete ptCell;

    LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
    if(lz == NULL)
    {
	fprintf(stderr,"Unfortunately This works only with Lazygrid!\n");
	exit(1);
    }

    Eigen::Vector3d diff = origin - old_centroid;  //-origin;
    double cx=0, cy=0, cz=0;
    lz->getCellSize(cx, cy, cz);

    ///How many cell to each direction is the new origin from old one
    Eigen::Vector3d centroid;
    centroid(0) = old_centroid(0) + floor(diff(0) / cx) * cx;
    centroid(1) = old_centroid(1) + floor(diff(1) / cy) * cy;
    centroid(2) = old_centroid(2) + floor(diff(2) / cz) * cz;

    index_->setCenter(centroid(0),centroid(1),centroid(2));
    //index_->setCenter(origin(0),origin(1),origin(2));
    index_->setSize(map_size(0),map_size(1),map_size(2));
    //lz->initializeAll();

   //temporarily commented fprintf(stderr,"centroid is %lf,%lf,%lf (origin: %lf %lf %lf) (map_size %lf %lf %lf) N=%d", centroid(0),centroid(1),centroid(2), origin(0),origin(1),origin(2), map_size(0), map_size(1), map_size(2),(int)pc.size());
    // ROS_INFO("centroid is %f,%f,%f", centroid(0),centroid(1),centroid(2));
    // ROS_INFO("maxDist is %lf", maxDist);

    pcl::PointCloud<pcl::PointXYZ>::const_iterator it = pc.points.begin();

    while(it!=pc.points.end())
    {
	Eigen::Vector3d d;
	if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
	{
	    it++;
	    continue;
	}

        //        std::cout << "centoroid add point [" << it->x << "," << it->y << "," <<it->z <<std::endl;

	if(range_limit>0)
	{
	    d << it->x, it->y, it->z;
	    d = d-origin;
	    if(d.norm()>range_limit)
	    {
		it++;
		continue;
            }
	}

	//fprintf(stderr,"HEP!");
	index_->addPoint(*it);
	NDTCell *ptCell=NULL;
	lz->getNDTCellAt(*it,ptCell);
#ifdef REFACTORED
	if(ptCell!=NULL) {
          update_set.insert(ptCell);
          //          std::cout << "insert" << std::endl;
        }
        // else {
        //   std::cout << "invalid cell..." << *it << std::endl;
        //   int indX, indY, indZ;
        //   lz->getIndexForPoint(*it, indX, indY, indZ);
        //   std::cout << "ind : " << indX << "," << indY << "," << indZ << std::endl;
        // }
        {
          // int indX, indY, indZ;
          // lz->getIndexForPoint(*it, indX, indY, indZ);
          // //          std::cout << "ind : " << indX << "," << indY << "," << indZ << std::endl;
          // double dx,dy,dz;
          // lz->getCenter(dx,dy,dz);
          // //          std::cout << "center : " << dx << "," << dy << "," << dz << std::endl;
          // int indX, indY, indZ;
          // lz->getGridSize(indX, indY, indZ);
          // //          std::cout << "gridsize : " << indX << "," << indY << "," << indZ << std::endl;
        }
#endif
	it++;
    }

    isFirstLoad_ = false;
}

/**
 * Just adds the points, without raytracing and such
 */
void NDTMap::addPointCloudSimple(const pcl::PointCloud<pcl::PointXYZ> &pc,double maxz)
{
    if(isFirstLoad_)
    {
        loadPointCloud( pc);
        return;
    }

    pcl::PointCloud<pcl::PointXYZ>::const_iterator it = pc.points.begin();
    it = pc.points.begin();
    LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
    if(lz == NULL)
    {
	fprintf(stderr,"Unfortunately This works only with Lazygrid!\n");
	exit(1);
    }

    while(it!=pc.points.end())
    {
        if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
        {
            it++;
            continue;
        }
        if(it->z>maxz)
        {
            it++;
            continue;
        }
        index_->addPoint(*it);
	NDTCell *ptCell;
	lz->getNDTCellAt(*it,ptCell);
#ifdef REFACTORED
	if(ptCell!=NULL) update_set.insert(ptCell);
#endif
        it++;
    }

}

/**
* Add a distribution to the map
*/
void NDTMap::addDistributionToCell(const Eigen::Matrix3d &ucov, const Eigen::Vector3d &umean, unsigned int numpointsindistribution,
                                   float r, float g,float b,  unsigned int maxnumpoints, float max_occupancy)
{
    pcl::PointXYZ pt;
    pt.x = umean[0];
    pt.y = umean[1];
    pt.z = umean[2];
    LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
    if(lz==NULL)
    {
        fprintf(stderr,"NOT LAZY GRID!!!\n");
        exit(1);
    }
    NDTCell *ptCell = NULL;
    //    lz->getNDTCellAt(pt,ptCell);
    lz->getCellAtAllocate(pt,ptCell);

    if(ptCell != NULL)
    {
	//std::cout<<"BEFORE\n";
	//std::cout<<"had G "<<ptCell->hasGaussian_<<" occ "<<ptCell->getOccupancy()<<std::endl;
//	std::cout<<umean.transpose()<<" "<<numpointsindistribution <<std::endl;
//	std::cout<<ucov<<std::endl;
//	std::cout<<ptCell->getMean().transpose()<<std::endl;
//	std::cout<<ptCell->getCov()<<std::endl;
	ptCell->updateSampleVariance(ucov, umean, numpointsindistribution, true, max_occupancy,maxnumpoints);
	ptCell->setRGB(r,g,b);
//	std::cout<<"AFTER\n";
//	std::cout<<ptCell->getMean().transpose()<<std::endl;
//	std::cout<<ptCell->getCov()<<std::endl;
    }
    else {
      //      std::cerr << "addDistributionToCell: failed to get a cell to add the distribution to" << std::endl;
    }
}

///Get the cell for which the point fall into (not the closest cell)
bool NDTMap::getCellAtPoint(const pcl::PointXYZ &refPoint, NDTCell *&cell)
{
    LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
    if(lz==NULL)
    {
        fprintf(stderr,"NOT LAZY GRID!!!\n");
        exit(1);
    }
    lz->getNDTCellAt(refPoint,cell);
    return (cell != NULL);
}

bool NDTMap::getCellAtPoint(const pcl::PointXYZ &refPoint, NDTCell *&cell) const
{
    LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
    if(lz==NULL)
    {
        fprintf(stderr,"NOT LAZY GRID!!!\n");
        exit(1);
    }
    lz->getNDTCellAt(refPoint,cell);
    return (cell != NULL);
}

///Get the cell for which the point fall into (not the closest cell)
bool NDTMap::getCellAtAllocate(const pcl::PointXYZ &refPoint, NDTCell *&cell)
{
    LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
    if(lz==NULL)
    {
        fprintf(stderr,"NOT LAZY GRID!!!\n");
        exit(1);
    }
    lz->getCellAtAllocate(refPoint,cell);
    return (cell != NULL);
}

bool NDTMap::getCellAtAllocate(const pcl::PointXYZ &refPoint, NDTCell *&cell) const
{
    LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
    if(lz==NULL)
    {
        fprintf(stderr,"NOT LAZY GRID!!!\n");
        exit(1);
    }
    lz->getCellAtAllocate(refPoint,cell);
    return (cell != NULL);
}

/**
 * Adds a new cloud: NDT-OM update step
 */
void NDTMap::addPointCloud(const Eigen::Vector3d &origin, const pcl::PointCloud<pcl::PointXYZ> &pc, double classifierTh, double maxz,
	double sensor_noise, double occupancy_limit)
{
// 	std::cout << "Good function: addPointCloud" << std::endl;
// 	exit(0);
    if(isFirstLoad_)
    {
			loadPointCloud( pc);
			return;
    }
    if(index_ == NULL)
    {
			//ERR("Problem creating index, unimplemented method\n");
			return;
    }
    pcl::PointCloud<pcl::PointXYZ>::const_iterator it = pc.points.begin();

    LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
    if(lz==NULL)
    {
			fprintf(stderr,"NOT LAZY GRID!!!\n");
			exit(1);
    }
    pcl::PointXYZ po, pt;
    po.x = origin(0); po.y = origin(1); po.z = origin(2);
    NDTCell* ptCell = NULL;

#ifdef REFACTORED
    std::vector< NDTCell*> cells;
    bool updatePositive = true;
    double max_range = 200.;

    while(it!=pc.points.end())
    {
			if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
			{
					it++;
					continue;
			}

			Eigen::Vector3d diff;
			diff << it->x-origin(0), it->y-origin(1), it->z-origin(2);
			double l = diff.norm();

			if(l>max_range)
			{
          fprintf(stderr,"addPointCloud::Very long distance (%lf) :( \n",l);
					it++;
					continue;
			}

			cells.clear();
// 			std::cout << "Tracing the line" << std::endl;
			if(!lz->traceLine(origin,*it,diff,maxz,cells)) {
					it++;
					continue;
			}
// 			else{
// 				std::cout << "That is not true so it's good" << std::endl;
// 			}

			for(unsigned int i=0; i<cells.size(); i++)
			{
					ptCell = cells[i];
					if(ptCell != NULL)
					{
				double l2target = 0;
				if(ptCell->hasGaussian_)
				{
						Eigen::Vector3d out, pend,vpt;
						pend << it->x,it->y,it->z;
						double lik = ptCell->computeMaximumLikelihoodAlongLine(po, *it, out);
						l2target = (out-pend).norm();

						double dist = (origin-out).norm();
						if(dist > l) continue; ///< don't accept points further than the measurement

						l2target = (out-pend).norm(); ///<distance to endpoint

						double sigma_dist = 0.5 * (dist/30.0); ///test for distance based sensor noise
						double snoise = sigma_dist + sensor_noise;
						double thr =exp(-0.5*(l2target*l2target)/(snoise*snoise)); ///This is the probability of max lik point being endpoint
						lik *= (1.0-thr);
						if(lik<0.3) continue;
						lik = 0.1*lik+0.5; ///Evidence value for empty - alpha * p(x);
						double logoddlik = log( (1.0-lik)/(lik) );
						//ptCell->updateEmpty(logoddlik,l2target);
						//fprintf(stderr,"[E=%.2lf] ", logoddlik);
						ptCell->updateOccupancy(logoddlik, occupancy_limit);
						if(ptCell->getOccupancy()<=0) ptCell->hasGaussian_ = false;
				}
				else
				{
					 // ptCell->updateEmpty(-0.2,l2target); ///The cell does not have gaussian, so we mark that we saw it empty...
					 ptCell->updateOccupancy(-0.2, occupancy_limit);
					 if(ptCell->getOccupancy()<=0) ptCell->hasGaussian_ = false;
				}
					}
					//update_set.insert(ptCell);
			}
	if(updatePositive) {
	    ptCell = dynamic_cast<NDTCell*>(index_->addPoint(*it));
	    if(ptCell!=NULL) {
				update_set.insert(ptCell);
	    }
	}
	it++;
    }
    isFirstLoad_ = false;

#else
    double centerX,centerY,centerZ;
    lz->getCenter(centerX, centerY, centerZ);
    double cellSizeX,cellSizeY,cellSizeZ;
    lz->getCellSize(cellSizeX, cellSizeY, cellSizeZ);
    int sizeX,sizeY,sizeZ;
    lz->getGridSize(sizeX, sizeY, sizeZ);

    NDTCell ****dataArray = lz->getDataArrayPtr();

    double min1 = std::min(cellSizeX,cellSizeY);
    double min2 = std::min(cellSizeZ,cellSizeY);

    double resolution = std::min(min1,min2); ///Select the smallest resolution

    while(it!=pc.points.end())
    {
        if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
        {
            it++;
            continue;
        }
        Eigen::Vector3d diff;
        diff << it->x-origin(0), it->y-origin(1), it->z-origin(2);

        double l = diff.norm();
        int N = l / (resolution);

        if(l>200)
        {
            fprintf(stderr,"addPointCloud::Very long distance (%lf) :( \n",l);
            it++;
            continue;
        }
        if(resolution<0.01)
        {
            fprintf(stderr,"Resolution very very small (%lf) :( \n",resolution);
            it++;
            continue;
        }

        if(N <= 0)
        {
            fprintf(stderr,"N=%d (r=%lf l=%lf) :( ",N,resolution,l);
            it++;
            continue;
        }

        diff = diff/(float)N;

        bool updatePositive = true;
        if(it->z>maxz)
        {
            it++;
            continue;
        }

        int idxo=0, idyo=0,idzo=0;
        for(int i=0; i<N-1; i++)
        {
            pt.x = origin(0) + ((float)(i+1)) *diff(0);
            pt.y = origin(1) + ((float)(i+1)) *diff(1);
            pt.z = origin(2) + ((float)(i+1)) *diff(2);
            int idx,idy,idz;

            idx = (int)(((pt.x - centerX)/cellSizeX+0.5) + sizeX/2);
            idy = (int)(((pt.y - centerY)/cellSizeY+0.5) + sizeY/2);
            idz = (int)(((pt.z - centerZ)/cellSizeZ+0.5) + sizeZ/2);

            ///We only want to check every cell once, so
            ///increase the index if we are still in the same cell
            if(idx == idxo && idy==idyo && idz ==idzo)
            {
                continue;
            }
            else
            {
                idxo = idx;
                idyo=idy,idzo=idz;
            }
            /// Check the validity of the index
            if(idx < sizeX && idy < sizeY && idz < sizeZ && idx >=0 && idy >=0 && idz >=0)
            {
                //fprintf(stderr,"(in)");
                ptCell = (dataArray[idx][idy][idz]);
            }
            else
            {
                //fprintf(stderr,"(out)");
                continue;
            }

            if(ptCell != NULL)
            {
                double l2target = 0;
                if(ptCell->hasGaussian_)
                {
                    Eigen::Vector3d out, pend,vpt;
                    pend << it->x,it->y,it->z;
                    double lik = ptCell->computeMaximumLikelihoodAlongLine(po, pt, out);
                    l2target = (out-pend).norm();

                    double dist = (origin-out).norm();
                    if(dist > l) continue; ///< don't accept points further than the measurement

                    l2target = (out-pend).norm(); ///<distance to endpoint

                    double sigma_dist = 0.5 * (dist/30.0); ///test for distance based sensor noise
                    double snoise = sigma_dist + sensor_noise;
                    double thr =exp(-0.5*(l2target*l2target)/(snoise*snoise)); ///This is the probability of max lik point being endpoint
                    lik *= (1.0-thr);
                    lik = 0.2*lik+0.5; ///Evidence value for empty - alpha * p(x);
                    double logoddlik = log( (1.0-lik)/(lik) );
                    ptCell->updateEmpty(logoddlik,l2target);
                }
                else
                {
                    ptCell->updateEmpty(-0.2,l2target); ///The cell does not have gaussian, so we mark that we saw it empty...
                }
            }
            else
            {
                index_->addPoint(pt); ///Add fake point to initialize!
            }
        }

        if(updatePositive) index_->addPoint(*it);
        it++;
    }
    isFirstLoad_ = false;
#endif
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Add new pointcloud to map - Updates the occupancy using the mean values of
* a local map generated from an observation
*
* Performs raytracing, updates conflicts and adds points to cells
* computeNDTCells must be called after calling this
*
* @param &origin is the position of the sensor, from where the scan has been taken from.
* @param &pc is the pointcloud to be added
* @param &localmapsize The dimensions of the local map used for computing the gaussians
* @param maxnumpoints Defines the forgetting factor (default 100000) the smaller the value the faster the adaptation
* @param occupancy_limit Clamping threshold for log-odds value
* @param maxz threshold for the maximum z-coordinate value for the measurement point_cloud
* @param sensor_noise The expected standard deviation of the sensor noise
*/
void  NDTMap::addPointCloudMeanUpdate(const Eigen::Vector3d &origin,
	const pcl::PointCloud<pcl::PointXYZ> &pc,
	const Eigen::Vector3d &localmapsize,
	unsigned int maxnumpoints, float occupancy_limit,double maxz, double sensor_noise){

    if(isFirstLoad_){
	//fprintf(stderr,"First load... ");
	loadPointCloud(pc);
	computeNDTCells();
	//fprintf(stderr,"DONE!");
	return;
    }
    if(index_ == NULL){
	return;
    }
    LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
    if(lz==NULL){
	fprintf(stderr,"NOT LAZY GRID!!!\n");
	exit(1);
    }
    ///fprintf(stderr,"UPDATING\n");
    double centerX,centerY,centerZ;
    lz->getCenter(centerX, centerY, centerZ);
    double cellSizeX,cellSizeY,cellSizeZ;
    lz->getCellSize(cellSizeX, cellSizeY, cellSizeZ);
    int sizeX,sizeY,sizeZ;
    lz->getGridSize(sizeX, sizeY, sizeZ);

    Eigen::Vector3d old_centroid;
    old_centroid(0) = centerX;
    old_centroid(1) = centerY;
    old_centroid(2) = centerZ;

    double min1 = std::min(cellSizeX,cellSizeY);
    double min2 = std::min(cellSizeZ,cellSizeY);
    double resolution = std::min(min1,min2); ///Select the smallest resolution
		//fprintf(stderr,"1:");
    ///Lets first create a local ndmap (this really works only if we have lazy grid as well)
    perception_oru::NDTMap ndlocal(new perception_oru::LazyGrid(resolution));
    ndlocal.loadPointCloudCentroid(pc,origin, old_centroid, localmapsize, /*70.0*/-1.); // No range limit used here, assume that the point cloud is already filtered.

    ///Use Student-T
    //ndlocal.computeNDTCells(CELL_UPDATE_MODE_STUDENT_T);

    /// Use Sample variance
    ndlocal.computeNDTCells();

    ///TODO: This returns now copies --- should be pointers?
    std::vector<perception_oru::NDTCell*> ndts;
    ndts = ndlocal.getAllCells();
		//fprintf(stderr,"2(%u):",ndts.size());
    NDTCell *ptCell=NULL;

    pcl::PointXYZ pt;
    pcl::PointXYZ po;
    po.x = origin(0); po.y = origin(1); po.z = origin(2);
    //fprintf(stderr,"NUM = %d\n",ndts.size());
    int num_high = 0;
    for(unsigned int it=0;it<ndts.size();it++)
    {
	if(ndts[it] == NULL){
	    fprintf(stderr,"NDTMap::addPointCloudMeanUpdate::GOT NULL FROM MAP -- SHOULD NEVER HAPPEN!!!\n");
	    continue;
	}

	if(!ndts[it]->hasGaussian_){
	    fprintf(stderr,"NDTMap::addPointCloudMeanUpdate::NO GAUSSIAN!!!! -- SHOULD NEVER HAPPEN!!!\n");
	    continue;
	}

	int numpoints = ndts[it]->getN();

	if(numpoints<=0){
	    fprintf(stderr,"addPointCloudMeanUpdate::Number of points in distribution<=0!!");
	    continue;
	}
	Eigen::Vector3d diff;
	Eigen::Vector3d m = ndts[it]->getMean();
	diff = m-origin;

	double l = diff.norm();
	int NN = l / (resolution);

	if(l>200){
	    fprintf(stderr,"addPointCloudMeanUpdate::Very long distance (%lf) :( \n",l);
	    continue;
	}
	if(resolution<0.01){
	    fprintf(stderr,"addPointCloudMeanUpdate::Resolution very very small (%lf) :( \n",resolution);
	    continue;
	}
	if(NN < 0){
	    fprintf(stderr,"addPointCloudMeanUpdate::N=%d (r=%lf l=%lf) :( ",NN,resolution,l);
	    continue;
	}

	bool updatePositive = true;
	if(m(2)>maxz){
	    num_high++;
	    updatePositive = false; ///Lets update negative even though the measurement was too high
	}
	//std::cout<<"c: "<<ndts[it]->getCov()<<std::endl;

	diff = diff/(float)NN;

	//fprintf(stderr,"3(%d):",NN);
	int idxo=0, idyo=0,idzo=0;
	for(int i=0; i<NN-2; i++)
	{
	    pt.x = origin(0) + ((float)(i+1)) *diff(0);
	    pt.y = origin(1) + ((float)(i+1)) *diff(1);
	    pt.z = origin(2) + ((float)(i+1)) *diff(2);
	    int idx,idy,idz;

	    idx = (int)(((pt.x - centerX)/cellSizeX+0.5) + sizeX/2.0);
	    idy = (int)(((pt.y - centerY)/cellSizeY+0.5) + sizeY/2.0);
	    idz = (int)(((pt.z - centerZ)/cellSizeZ+0.5) + sizeZ/2.0);

	    ///We only want to check every cell once, so
	    ///increase the index if we are still in the same cell
	    if(idx == idxo && idy==idyo && idz ==idzo){
		continue;
	    }else{
		idxo = idx;idyo=idy;idzo=idz;
	    }
	    ptCell = NULL;
	    /// Check the validity of the index
	    lz->getNDTCellAt(idx,idy,idz,ptCell);
	    /*
	       if(ptCell != NULL){
	       if(!ptCell->hasGaussian_){
	       ptCell->updateOccupancy(-0.85*numpoints,occupancy_limit);
	       }else{
	       ptCell->updateOccupancy(-0.2*numpoints,occupancy_limit);
	       ptCell = lz->getClosestNDTCell(pt);
	       }
	       }*/

	    if(ptCell != NULL){
		double l2target = 0;
		if(ptCell->hasGaussian_)
		{
		    Eigen::Vector3d out, pend,vpt;
		    pend = m;

		    double lik = ptCell->computeMaximumLikelihoodAlongLine(po, pt, out);
		    l2target = (out-pend).norm();

		    double dist = (origin-out).norm();
		    if(dist > l) continue; ///< don't accept points further than the measurement

		    l2target = (out-pend).norm(); ///<distance to endpoint

		    double sigma_dist = 0.5 * (dist/30.0); ///test for distance based sensor noise
		    //double sigma_dist = 0;
		    double snoise = sigma_dist + sensor_noise;
		    double thr =exp(-0.5*(l2target*l2target)/(snoise*snoise)); ///This is the probability of max lik point being endpoint
		    lik *= (1.0-thr);
		    //lik = 0.4*lik+0.5+0.05; ///Evidence value for empty - alpha * p(x);
		    lik = 0.2*lik+0.5; ///Evidence value for empty - alpha * p(x);
		    double logoddlik = log( (1.0-lik)/(lik) );

		    //fprintf(stderr,"l=%lf ",numpoints*logoddlik);
		    ptCell->updateOccupancy(numpoints*logoddlik,occupancy_limit);
		    if(ptCell->getOccupancy()<0) ptCell->hasGaussian_ = false;
		}
		else
		{
		    ptCell->updateOccupancy(-0.85*numpoints,occupancy_limit); ///The cell does not have gaussian, so we mark that we saw it empty...
		    //ptCell->updateEmpty(-0.2*numpoints,l2target); ///The cell does not have gaussian, so we mark that we saw it empty...
		}
	    }else{
		ptCell = dynamic_cast<NDTCell*>(index_->addPoint(pt)); ///Add fake point to initialize!
	    }
	}
	if(updatePositive){
	    Eigen::Matrix3d ucov = ndts[it]->getCov();
	    float r,g,b;
	    ndts[it]->getRGB(r,g,b);
	    addDistributionToCell(ucov, m, numpoints, r, g,b,maxnumpoints,occupancy_limit); ///<FIXME:: local implementation can be faster?
	}
    }
    //fprintf(stderr,"| Gaussians=%d Invalid=%d |", ndts.size(), num_high);
    for(unsigned int i=0;i<ndts.size();i++) delete ndts[i];
    isFirstLoad_ = false;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/**
 * Adds one measurement to the map using NDT-OM update step
 * @return true if an inconsistency was detected
 */
bool NDTMap::addMeasurement(const Eigen::Vector3d &origin, pcl::PointXYZ endpoint, double classifierTh, double maxz, double sensor_noise)
{

    if(index_ == NULL)
    {
	return false;
    }

    bool retval = false;

    LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
    double centerX,centerY,centerZ;
    lz->getCenter(centerX, centerY, centerZ);
    double cellSizeX,cellSizeY,cellSizeZ;
    lz->getCellSize(cellSizeX, cellSizeY, cellSizeZ);
    int sizeX,sizeY,sizeZ;
    lz->getGridSize(sizeX, sizeY, sizeZ);

    //Cell<PointT> ****dataArray = lz->getDataArrayPtr();

    double min1 = std::min(cellSizeX,cellSizeY);
    double min2 = std::min(cellSizeZ,cellSizeY);

    double resolution = std::min(min1,min2); ///Select the smallest resolution

    if(lz==NULL)
    {
        fprintf(stderr,"NOT LAZY GRID!!!\n");
        exit(1);
    }
    NDTCell *ptCell=NULL;

    pcl::PointXYZ pt;
    pcl::PointXYZ po;
    po.x = origin(0),origin(1),origin(2);

    Eigen::Vector3d diff;
    diff << endpoint.x-origin(0), endpoint.y-origin(1), endpoint.z-origin(2);

    double l = diff.norm();
    if(l>200)
    {
        fprintf(stderr,"addMeasurement::Very long distance (%lf) :( \n",l);
        return false;
    }
    if(resolution<0.01)
    {
        fprintf(stderr,"Resolution very very small (%lf) :( \n",resolution);
        return false;
    }

    int NN = l / (resolution);
    if(NN <= 0) return false;
    diff = diff/(float)NN;
    //fprintf(stderr," (N=%d) ",NN);
    bool updatePositive = true;
    if(endpoint.z>maxz)
    {
        return false;
    }

    int idxo=0, idyo=0,idzo=0;
    for(int i=0; i<NN-2; i++)
    {
        pt.x = origin(0) + ((float)(i+1)) *diff(0);
        pt.y = origin(1) + ((float)(i+1)) *diff(1);
        pt.z = origin(2) + ((float)(i+1)) *diff(2);
        int idx,idy,idz;

        idx = (int)(((pt.x - centerX)/cellSizeX+0.5) + sizeX/2);
        idy = (int)(((pt.y - centerY)/cellSizeY+0.5) + sizeY/2);
        idz = (int)(((pt.z - centerZ)/cellSizeZ+0.5) + sizeZ/2);

        ///We only want to check every cell once, so
        ///increase the index if we are still in the same cell
        if(idx == idxo && idy==idyo && idz ==idzo)
        {
            continue;
        }
        else
        {
            idxo = idx;
            idyo=idy,idzo=idz;
        }
        /// Check the validity of the index
        //if(idx < sizeX && idy < sizeY && idz < sizeZ && idx >=0 && idy >=0 && idz >=0)
        //{
        //    ptCell = dynamic_cast<NDTCell<PointT> *>  (lz->getCellAt(idx,idy,idz)); //dataArray[idx][idy][idz]);
	lz->getNDTCellAt(idx,idy,idz,ptCell);
        //}
        //else
        //{
        //    continue;
        //}

        if(ptCell != NULL)
        {
            double l2target = 0;

            if(ptCell->hasGaussian_)
            {
                Eigen::Vector3d out, pend,vpt;

                pend << endpoint.x,endpoint.y,endpoint.z; ///< endpoint

                double lik = ptCell->computeMaximumLikelihoodAlongLine(po, pt, out);
                double dist = (origin-out).norm();
                if(dist > l) continue; ///< don't accept points further than the measurement

                l2target = (out-pend).norm(); ///<distance to endpoint
                //double thr =exp(-0.5*(l2target*l2target)/(sensor_noise*sensor_noise)); ///This is the probability of max lik point being endpoint
                //ptCell->updateEmpty(lik*(1-thr),l2target);

                double sigma_dist = 0.5 * (dist/30.0); ///test for distance based sensor noise
                double snoise = sigma_dist + sensor_noise;
                double thr =exp(-0.5*(l2target*l2target)/(snoise*snoise)); ///This is the probability of max lik point being endpoint
                lik *= (1.0-thr);
                lik = 0.2*lik+0.5; ///Evidence value for empty - alpha * p(x);
                double logoddlik = log( (1.0-lik)/(lik) );
                ptCell->updateEmpty(logoddlik,l2target);

                /*
                if(lik>thr){
                	retval = true;
                	ptCell->updateEmpty(lik,l2target);
                }*/
            }
            else
            {
                ptCell->updateEmpty(-0.1,l2target); ///The cell does not have gaussian, so we mark that we saw it empty...
            }
        }
        else
        {
            index_->addPoint(pt); ///Add fake point to initialize!
        }
    }

    if(updatePositive) index_->addPoint(endpoint);

    isFirstLoad_ = false;

    return retval;
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Get estimated depth
/// This goes through now the cells always to max depth and is not the most efficient solution
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

double NDTMap::getDepth(Eigen::Vector3d origin, Eigen::Vector3d dir, double maxDepth){
	Eigen::Vector3d ray_endpos=origin + dir * maxDepth;
	std::vector< NDTCell*> cells;

	Eigen::Vector3d diff = ray_endpos - origin;
	pcl::PointXYZ endP;
	endP.x = ray_endpos(0);
	endP.y = ray_endpos(1);
	endP.z = ray_endpos(2);

	LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
	if(lz==NULL){
		fprintf(stderr,"NOT LAZY GRID!!!\n");
		exit(1);
	}

	if(!lz->traceLine(origin, endP,diff,1000.0, cells)){
		return maxDepth+1.0;
	}
	//fprintf(stderr,"Got trace with %d Cells (%lf)\n",cells.size(),(ray_endpos-origin).norm());
	pcl::PointXYZ po;
	po.x = origin(0);
	po.y = origin(1);
	po.z = origin(2);

	Eigen::Vector3d out;
	bool hasML = false;

	for(unsigned int i=0;i<cells.size();i++){
		 if(cells[i]->hasGaussian_){
			double lik = cells[i]->computeMaximumLikelihoodAlongLine(po, endP, out);
			if(lik>0.1){
				//fprintf(stderr,"Got ML %lf (%lf)\n",lik,(out-origin).norm());
				hasML = true;
				break;
			}
		}
	}

	if(hasML) return (out - origin).norm();

	return (maxDepth+1.0);
}

double NDTMap::getDepthSmooth(Eigen::Vector3d origin,
                                      Eigen::Vector3d dir,
                                      double maxDepth,
                                      int n_neigh,
                                      double weight,
                                      double threshold,
                                      Eigen::Vector3d *hit)
{
    Eigen::Vector3d ray_endpos=origin + dir * maxDepth;
    std::vector< NDTCell*> cells, surr;

    Eigen::Vector3d diff = ray_endpos - origin;
    pcl::PointXYZ endP;
    endP.x = ray_endpos(0);
    endP.y = ray_endpos(1);
    endP.z = ray_endpos(2);

    LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
    if(lz==NULL){
	fprintf(stderr,"NOT LAZY GRID!!!\n");
	exit(1);
    }

    if(!lz->traceLine(origin, endP,diff,1000.0, cells)){
	return maxDepth+1.0;
    }

    pcl::PointXYZ startP, p;
    startP.x = origin(0);
    startP.y = origin(1);
    startP.z = origin(2);

    Eigen::Vector3d out;
    bool hasML = false;

    for(unsigned int i = 0; i < cells.size(); i++)
    {
	if(cells[i]->hasGaussian_)
	{
	    surr = lz->getClosestNDTCells(cells[i]->getCenter(), n_neigh, true);
	    double like = cells[i]->computeMaximumLikelihoodAlongLine(startP, endP, out);
	    p.x = out(0);
	    p.y = out(1);
	    p.z = out(2);
	    for (unsigned int k = 1u; k < surr.size(); ++k)
	    {
		like += weight * surr[k]->getLikelihood(p);
	    }
	    if(like > threshold)
	    {
		hasML = true;
		break;
	    }
	}
    }

    if (hasML)
    {
	if (hit != NULL)
	{
	    *hit = out;
	}
	return (out - origin).norm();
    }

    return (maxDepth+1.0);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void NDTMap::loadPointCloud(const pcl::PointCloud<pcl::PointXYZ> &pc, const std::vector<std::vector<size_t> > &indices)
{

  //    loadPointCloud(pc);
    // Specific function related to CellVector
    CellVector *cl = dynamic_cast<CellVector*>(index_);
    if (cl != NULL)
    {
        for (size_t i = 0; i < indices.size(); i++)
        {
            cl->addCellPoints(pc, indices[i]);
        }

    }
    else
    {
        //ERR("loading point clouds using indices are currently supported in CellVector index_.");
    }
}

void NDTMap::loadDepthImage(const cv::Mat& depthImage, DepthCamera<pcl::PointXYZ> &cameraParams)
{
    pcl::PointCloud<pcl::PointXYZ> pc;
    cameraParams.convertDepthImageToPointCloud(depthImage, pc);
    this->loadPointCloud(pc);
}

pcl::PointCloud<pcl::PointXYZ> NDTMap::loadDepthImageFeatures(const cv::Mat& depthImage, std::vector<cv::KeyPoint> &keypoints,
        size_t &supportSize, double maxVar, DepthCamera<pcl::PointXYZ> &cameraParams, bool estimateParamsDI, bool nonMean)
{
    std::vector<cv::KeyPoint> good_keypoints;
    Eigen::Vector3d mean;
    pcl::PointXYZ mn;
    pcl::PointCloud<pcl::PointXYZ> cloudOut;
    CellVector *cl = dynamic_cast<CellVector*>(index_);
    if(cl==NULL)
    {
        std::cerr<<"wrong index type!\n";
        return cloudOut;
    }
    for (size_t i=0; i<keypoints.size(); i++)
    {
        if(!estimateParamsDI)
        {
            pcl::PointCloud<pcl::PointXYZ> points;
            pcl::PointXYZ center;
            cameraParams.computePointsAtIndex(depthImage,keypoints[i],supportSize,points,center);
            NDTCell *ndcell = new NDTCell();
            typename pcl::PointCloud<pcl::PointXYZ>::iterator it = points.points.begin();
            while (it!= points.points.end() )
            {
                if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
                {
                    it++;
                    continue;
                }
                ndcell->addPoint(*it);
                it++;
            }
            ndcell->computeGaussian();
            if(ndcell->hasGaussian_)
            {
                Eigen::Vector3d evals = ndcell->getEvals();
                if(sqrt(evals(2)) < maxVar)
                {
                    if (nonMean)
                    {
                        if(std::isnan(center.x) ||std::isnan(center.y) ||std::isnan(center.z))
                        {
                            continue;
                        }
                        mn = center;
                    }
                    else
                    {
                        mean = ndcell->getMean();
                        mn.x = mean(0);
                        mn.y = mean(1);
                        mn.z = mean(2);
                    }
                    cloudOut.points.push_back(mn);
                    ndcell->setCenter(mn);
                    cl->addCell(ndcell);
                    good_keypoints.push_back(keypoints[i]);
                }
            }
        }
        else
        {
            assert(nonMean = false); // Not implemented / used.
            Eigen::Vector3d mean;
            Eigen::Matrix3d cov;
            cameraParams.computeParamsAtIndex(depthImage,keypoints[i],supportSize,mean,cov);
            NDTCell *ndcell = new NDTCell();
            ndcell->setMean(mean);
            ndcell->setCov(cov);

            if(ndcell->hasGaussian_)
            {
                Eigen::Vector3d evals = ndcell->getEvals();
                //std::cout<<evals.transpose()<<std::endl;
                if(sqrt(evals(2)) < maxVar)
                {
                    mean = ndcell->getMean();
                    mn.x = mean(0);
                    mn.y = mean(1);
                    mn.z = mean(2);
                    cloudOut.points.push_back(mn);
                    ndcell->setCenter(mn);
                    cl->addCell(ndcell);
                    good_keypoints.push_back(keypoints[i]);
                }
            }

        }
    }

    //TODO
    keypoints = good_keypoints;
    return cloudOut;
}

void NDTMap::computeMaximumLikelihoodPointCloudWithRangePairs(const Eigen::Vector3d &origin,
                                                              const pcl::PointCloud<pcl::PointXYZ> &pc,
                                                              const Eigen::Vector3d &virtualOrigin,
                                                              pcl::PointCloud<pcl::PointXYZ> &pc_out,
                                                              std::vector<std::pair<double,double> > &ranges,
                                                              double max_range = 130.) const
{
    pc_out.clear();
    if(isFirstLoad_ || index_ == NULL)
    {
        return;
    }
    pcl::PointCloud<pcl::PointXYZ>::const_iterator it = pc.points.begin();

    LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
    if(lz==NULL)
    {
        fprintf(stderr,"NOT LAZY GRID!!!\n");
        exit(1);
    }
    pcl::PointXYZ po;
    po.x = origin(0); po.y = origin(1); po.z = origin(2);
    NDTCell* ptCell = NULL;

    // Step through the points, compute the directions and for now do the full line tracing...
    std::vector< NDTCell*> cells;
    bool updatePositive = true;

    while(it!=pc.points.end())
    {
        if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
        {
            it++;
            continue;
        }

        Eigen::Vector3d diff;
        diff << it->x-origin(0), it->y-origin(1), it->z-origin(2);
        double l = diff.norm();

        cells.clear();

        // Trace line uses the diff to check that we never go beyond the diff value...
        // Hacky for now but force the diff to be a bit larger -> 3 meters...
        diff *= ((l+3.)/l);
        if(!lz->traceLine(virtualOrigin,*it,diff,1000.0,cells)) {
            it++;
            continue;
        }

        for(unsigned int i=0; i<cells.size(); i++)
        {
            ptCell = cells[i];
            if(ptCell != NULL)
            {
                double l2target = 0;
                if(ptCell->hasGaussian_)
                {
                    Eigen::Vector3d out;
                    double lik = ptCell->computeMaximumLikelihoodAlongLine(po, *it, out);
                    //std::cerr << "lik : " << lik << std::endl;
                    if (lik > 0.) {
                        ranges.push_back(std::pair<double,double>(l, (out-origin).norm()));
                        pc_out.push_back(pcl::PointXYZ(out[0], out[1], out[2]));
                    }
                }
            }
        }
        it++;
    }
}

void NDTMap::computeConflictingPoints(const Eigen::Vector3d &origin,
                              const pcl::PointCloud<pcl::PointXYZ> &pc,
                              pcl::PointCloud<pcl::PointXYZ> &pc_out,
                              pcl::PointCloud<pcl::PointXYZ> &pc2_out,
                              double likelihoodFactor) const
{
    pc_out.clear(); pc2_out.clear();
    if(isFirstLoad_ || index_ == NULL)
    {
        return;
    }
    pcl::PointCloud<pcl::PointXYZ>::const_iterator it = pc.points.begin();

    LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
    if(lz==NULL)
    {
        fprintf(stderr,"NOT LAZY GRID!!!\n");
        exit(1);
    }
    pcl::PointXYZ po;
    po.x = origin(0); po.y = origin(1); po.z = origin(2);
    NDTCell* ptCell = NULL;

    while(it!=pc.points.end())
    {
        if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
        {
            it++;
            continue;
        }

        Eigen::Vector3d diff;
        diff << it->x-origin(0), it->y-origin(1), it->z-origin(2);
        double l = diff.norm();

        // Get the cell
        ptCell = lz->getClosestNDTCell(*it);
        if (ptCell == NULL) {
            pc_out.push_back(*it);
            it++;
            continue;
        }
        // if (ptCell->getOccupancyRescaled() < 0.8) {
        //     pc_out.push_back(*it);
        //     it++;
        //     continue;
        // }

        if (!ptCell->hasGaussian_) {
            pc_out.push_back(*it);
            it++;
            continue;
        }

        double lik = ptCell->getLikelihood(*it);
        //        std::cerr << "lik : " << lik << " " << std::flush;
        lik = (lik < 0) ? 0 : lik;
        Eigen::Vector3d out, pt;
        pt << it->x, it->y, it->z;
        double lik_trace = ptCell->computeMaximumLikelihoodAlongLine(po, *it, out);
        //        std::cerr << " lik : " << lik << " lik_trace : " << lik_trace << " lik/lik_trace : " << lik/lik_trace << std::endl;

        // if (lik / lik_trace < likelihoodFactor) {
        //     pc_out.push_back(*it);
        // }
        //        if (lik < likelihoodFactor) {
        //        std::cerr << " " << norm << std::flush;
        double norm = (pt - out).norm();
        //        if (norm > 0.8)
        // if (fabs(pt(2) - out(2)) > 0.2)
        // {
        //     pc_out.push_back(*it);
        //     pc2_out.push_back(pcl::PointXYZ(out(0),out(1),out(2)));
        // }
        pc_out.push_back(pcl::PointXYZ(out(0),out(1),out(2)));
        it++;
    }
}

#if 0
void
NDTMap::computeMaximumLikelihoodPointRangesForPoseSet(const std::vector<Eigen::Affine3d> &poses,
                                                       const pcl::PointCloud<pcl::PointXYZ> &pc,
                                                       const Eigen::Vector3d &virtualOrigin,
                                                       Eigen::MatrixXd &predictedRanges,
                                                      Eigen::VectorXd &rawRanges) const {
    if(isFirstLoad_ || index_ == NULL)
    {
        return;
    }
    LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
    if(lz==NULL)
    {
        fprintf(stderr,"NOT LAZY GRID!!!\n");
        exit(1);
    }

    pcl::PointCloud<pcl::PointXYZ>::const_iterator it = pc.points.begin();

    pcl::PointXYZ po;
    po.x = origin(0); po.y = origin(1); po.z = origin(2);
    NDTCell* ptCell = NULL;

    // Step through the points, compute the directions and for now do the full line tracing...
    std::vector< NDTCell*> cells;
    bool updatePositive = true;

    while(it!=pc.points.end())
    {
        if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
        {
            it++;
            continue;
        }

        Eigen::Vector3d diff;
        diff << it->x-origin(0), it->y-origin(1), it->z-origin(2);
        double l = diff.norm();

        cells.clear();

        // Trace line uses the diff to check that we never go beyond the diff value...
        // Hacky for now but force the diff to be a bit larger -> 3 meters...
        diff *= ((l+3.)/l);
        if(!lz->traceLine(virtualOrigin,*it,diff,1000.0,cells)) {
            it++;
            continue;
        }

        for(unsigned int i=0; i<cells.size(); i++)
        {
            ptCell = cells[i];
            if(ptCell != NULL)
            {
                double l2target = 0;
                if(ptCell->hasGaussian_)
                {
                    Eigen::Vector3d out;
                    double lik = ptCell->computeMaximumLikelihoodAlongLine(po, *it, out);
                    //std::cerr << "lik : " << lik << std::endl;
                    if (lik > 0.) {
                        ranges.push_back(std::pair<double,double>(l, (out-origin).norm()));
                        pc_out.push_back(pcl::PointXYZ(out[0], out[1], out[2]));
                    }
                }
            }
        }
        it++;
    }
}
#endif

void NDTMap::computeMaximumLikelihoodRanges(const Eigen::Vector3d &origin,
                                            const Eigen::VectorXd &rawRanges,
                                            const std::vector<Eigen::Vector3d> &dirs,
                                            Eigen::VectorXd &ranges) const {

    if(isFirstLoad_ || index_ == NULL)
    {
        return;
    }

    LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
    if(lz==NULL)
    {
        fprintf(stderr,"NOT LAZY GRID!!!\n");
        exit(1);
    }
    pcl::PointXYZ po;
    po.x = origin(0); po.y = origin(1); po.z = origin(2);
    NDTCell* ptCell = NULL;

    // Step through the points, compute the directions and for now do the full line tracing...
    std::vector< NDTCell*> cells;
    bool updatePositive = true;

    double resolution = this->getSmallestCellSizeInMeters();
    ranges.resize(dirs.size());
    ranges.setZero();

    for (unsigned int i = 0; i < dirs.size(); i++)
    {
        cells.clear();

        // TODO update traceLine to use the rawRanges as a bias in selecting the cells.
        // Use the difference to be 2*the resolution.
        // Eigen::Vector3d p1 = origin+(rawRanges[i]-resolution)*dirs[i];
        // Eigen::Vector3d p2 = origin+(rawRanges[i]+resolution)*dirs[i];
        // if(!lz->traceLine(p1, p2, dirs[i]*2*resolution,1000.0,cells)) {
        //     continue;
        // }
        Eigen::Vector3d p1 = origin;
        Eigen::Vector3d p2 = origin+dirs[i]*100.;
        // Eigen::Vector3d p2 = origin+(rawRanges[i]+resolution)*dirs[i];
        if(!lz->traceLine(p1, p2, dirs[i]*100.,1000.0,cells)) {
            continue;
        }

        double max_lik = 0.;
        double range = 0.;
        for(unsigned int j=0; j<cells.size(); j++)
        {
            ptCell = cells[j];
            if(ptCell != NULL)
            {
                if(ptCell->hasGaussian_)
                {
                    Eigen::Vector3d out;
                    double lik = ptCell->computeMaximumLikelihoodAlongLine(p1, p2, out);

                    if (lik > max_lik) {
                        max_lik = lik;
                        range = (out-origin).norm();
                        ranges[i] = range;
                    }
                }
            }
        }

        //        std::cout << "ranges[" << i << "] : " << ranges[i] << "   -   " << rawRanges[i] << std::endl;

    }
}

/** Helper function, computes the  NDTCells
*/
void NDTMap::computeNDTCells(int cellupdatemode, unsigned int maxnumpoints, float occupancy_limit, Eigen::Vector3d origin, double sensor_noise)
{
    CellVector *cv = dynamic_cast<CellVector*>(index_);

    conflictPoints.clear();
#ifdef REFACTORED
    typename std::set<NDTCell*>::iterator it = update_set.begin();
    while (it != update_set.end())
    {
        NDTCell *cell = *it;
        if(cell!=NULL)
        {
            cell->computeGaussian(cellupdatemode,maxnumpoints, occupancy_limit, origin,sensor_noise);
            ///Process the conflict points
            if(cell->points_.size()>0)
            {
                for(unsigned int i=0; i<cell->points_.size(); i++) conflictPoints.push_back(cell->points_[i]);
                cell->points_.clear();
            }
            if (cv!=NULL)
            {
                // Set the mean to the cell's centre.
                Eigen::Vector3d mean = cell->getMean();
                pcl::PointXYZ pt;
                pt.x = mean[0];
                pt.y = mean[1];
                pt.z = mean[2];

                cell->setCenter(pt);
            }
        }
        else
        {

        }
        it++;
    }
    update_set.clear();

    CellVector *cl = dynamic_cast<CellVector*>(index_);
    if(cl!=NULL)
    {
        cl->initKDTree();
    }
#else
typename SpatialIndex::CellVectorItr it = index_->begin();
    //typename std::set<NDTCell<PointT>*>::iterator it = update_set.begin();
    while (it != index_->end())
    //while (it != update_set.end())
    {
        NDTCell *cell = (*it);
        //NDTCell<PointT> *cell = *it;
        if(cell!=NULL)
        {
            cell->computeGaussian(cellupdatemode,maxnumpoints, occupancy_limit, origin,sensor_noise);
            ///Process the conflict points
            if(cell->points_.size()>0)
            {
                for(unsigned int i=0; i<cell->points_.size(); i++) conflictPoints.push_back(cell->points_[i]);
                cell->points_.clear();
            }
            if (cv!=NULL)
            {
                // Set the mean to the cell's centre.
                Eigen::Vector3d mean = cell->getMean();
                pcl::PointXYZ pt;
                pt.x = mean[0];
                pt.y = mean[1];
                pt.z = mean[2];

                cell->setCenter(pt);
            }
        }
        else
        {

        }
        it++;
    }
    //update_set.clear();

    CellVector *cl = dynamic_cast<CellVector*>(index_);
    if(cl!=NULL)
    {
        cl->initKDTree();
    }
#endif

}

/**
* Computes the normaldistribution parameters and leaves the points a
*/
void NDTMap::computeNDTCellsSimple(bool keepPoints)
{
    CellVector *cv = dynamic_cast<CellVector*>(index_);

    conflictPoints.clear();

    typename SpatialIndex::CellVectorItr it = index_->begin();

    while (it != index_->end())
    {
        NDTCell *cell = (*it);
        if(cell!=NULL)
        {
            cell->computeGaussianSimple();

            if (cv!=NULL)
            {
                // Set the mean to the cell's centre.
                Eigen::Vector3d mean = cell->getMean();
                pcl::PointXYZ pt;
                pt.x = mean[0];
                pt.y = mean[1];
                pt.z = mean[2];

                cell->setCenter(pt);
            }
            if (!keepPoints) {
              cell->points_.clear();
            }
        }
        else
        {

        }
        it++;
    }

    CellVector *cl = dynamic_cast<CellVector*>(index_);
    if(cl!=NULL)
    {
        cl->initKDTree();
    }
}

/** output methods for saving the map in the jff format
 */
int NDTMap::writeToJFF(const char* filename)
{

    if(filename == NULL)
    {
        //ERR("problem outputing to jff\n");
        return -1;
    }

    FILE * jffout = fopen(filename, "w+b");

    fwrite(_JFFVERSION_, sizeof(char), strlen(_JFFVERSION_), jffout);

    switch(this->getMyIndexInt())
    {
    case 1:
        writeCellVectorJFF(jffout);
        break;
    case 2:
        //writeOctTreeJFF(jffout);
        break;
    case 3:
        writeLazyGridJFF(jffout);
        break;
    default:
        //ERR("unknown index type\n");
        return -1;
    }

    fclose(jffout);

    return 0;
}

int NDTMap::writeCellVectorJFF(FILE * jffout)
{
    int indexType[1] = {1};
    fwrite(indexType, sizeof(int), 1, jffout);

    // TODO: add CellVector specific stuff

    typename SpatialIndex::CellVectorItr it = index_->begin();
    while (it != index_->end())
    {
        NDTCell *cell = (*it);
        if(cell!=NULL)
        {
            if(cell->hasGaussian_)
            {
                // TODO: add index specific content smartly
                if(cell->writeToJFF(jffout) < 0)
                    return -1;
            }
        }
        else
        {
            // do nothing
        }
        it++;
    }

    return 0;

}

int NDTMap::writeOctTreeJFF(FILE * jffout)
{
    int indexType[1] = {2};
    fwrite(indexType, sizeof(int), 1, jffout);

    // TODO: add OctTree specific stuff

    typename SpatialIndex::CellVectorItr it = index_->begin();
    while (it != index_->end())
    {
        NDTCell *cell = (*it);
        if(cell!=NULL)
        {
            if(cell->hasGaussian_)
            {
                // TODO: add index specific content smartly
                if(cell->writeToJFF(jffout) < 0)
                    return -1;
            }
        }
        else
        {
            // do nothing
        }
        it++;
    }

    return 0;

}

int NDTMap::writeLazyGridJFF(FILE * jffout)
{
    int indexType[1] = {3};
    fwrite(indexType, sizeof(int), 1, jffout);

    // add LazyGrid specific stuff
    double sizeXmeters, sizeYmeters, sizeZmeters;
    double cellSizeX, cellSizeY, cellSizeZ;
    double centerX, centerY, centerZ;
    LazyGrid *ind = dynamic_cast<LazyGrid*>(index_);

    ind->getGridSizeInMeters(sizeXmeters, sizeYmeters, sizeZmeters);
    ind->getCellSize(cellSizeX, cellSizeY, cellSizeZ);
    ind->getCenter(centerX, centerY, centerZ);

    double lazyGridData[9] = { sizeXmeters, sizeYmeters, sizeZmeters,
                               cellSizeX,   cellSizeY,   cellSizeZ,
                               centerX,     centerY,     centerZ
                             };

    fwrite(lazyGridData, sizeof(double), 9, jffout);

    fwrite(ind->getProtoType(), sizeof(NDTCell), 1, jffout);

    // loop through all active cells
    typename SpatialIndex::CellVectorItr it = index_->begin();
    while (it != index_->end())
    {
	if((*it)->writeToJFF(jffout) < 0)	return -1;
        it++;
    }

    return 0;

}

/** method to load NDT maps from .jff files
USAGE:	create NDTMap with desired index and PointType (index type is
checked, but Point type is NOT checked) via e.g.

lslgeneric::NDTMap<pcl::PointXYZ> nd1(
new lslgeneric::LazyGrid<pcl::PointXYZ>(0.4)); --> (*)

and then call

nd1.loadFromJFF("map0027.jff");

 *) use this constructor so index is not initialized and attributes
 can be set manually
 */
int NDTMap::loadFromJFF(const char* filename)
{

    FILE * jffin;

    if(filename == NULL)
    {
        JFFERR("problem outputing to jff");
    }

    jffin = fopen(filename,"r+b");
    if (jffin == NULL) {
      JFFERR("file not found");
      std::cerr << "Failed to open : " << filename << std::endl;
      return -5;
    }

    char versionBuf[16];
    if(fread(&versionBuf, sizeof(char), strlen(_JFFVERSION_), jffin) <= 0)
    {
        JFFERR("reading version failed");
    }
    versionBuf[strlen(_JFFVERSION_)] = '\0';

    int indexType;
    if(fread(&indexType, sizeof(int), 1, jffin) <= 0)
    {
        JFFERR("reading version failed");
    }

    if(indexType != this->getMyIndexInt())
    {
        switch(indexType)
        {
        case 1:
            std::cerr << "Map uses CellVector\n";
            return -1;
            break;
        case 2:
            std::cerr << "Map uses OctTree\n";
            return -2;
            break;
        case 3:
            std::cerr << "Map uses LazyGrid\n";
            return -3;
            break;
        }
    }

    switch(indexType)
    {
    case 1:
    {
        CellVector* cv = dynamic_cast<CellVector * >(index_);
        if(cv->loadFromJFF(jffin) < 0)
        {
            JFFERR("Error loading CellVector");
        }
        break;
    }
#if 0
    case 2:
    {
        OctTree* tr = dynamic_cast<OctTree*>(index_);
        if(tr->loadFromJFF(jffin) < 0)
        {
            JFFERR("Error loading OctTree");
        }
        break;
    }
#endif
    case 3:
    {
	std::cerr << "Map uses LazyGrid\n";
        LazyGrid* gr = dynamic_cast<LazyGrid*>(index_);
        if(gr->loadFromJFF(jffin) < 0)
        {
            JFFERR("Error loading LazyGrid");
        }
        break;
    }
    default:
        JFFERR("error casting index");
    }

    NDTCell *ptCell = new NDTCell();
    index_->setCellType(ptCell);
    delete ptCell;

    fclose(jffin);

   // std::cout << "map loaded successfully " << versionBuf << std::endl;

    isFirstLoad_ = false;

    return 0;

}

/** method to load NDT maps from .jff files
USAGE:	create NDTMap with desired index and PointType (index type is
checked, but Point type is NOT checked) via e.g.

lslgeneric::NDTMap<pcl::PointXYZ> nd1(
new lslgeneric::LazyGrid<pcl::PointXYZ>(0.4)); --> (*)

and then call

nd1.loadFromJFF("map0027.jff");

 *) use this constructor so index is not initialized and attributes
 can be set manually
 */
int NDTMap::loadFromJFF(FILE * jffin)
{

    char versionBuf[16];
    if(fread(&versionBuf, sizeof(char), strlen(_JFFVERSION_), jffin) <= 0)
    {
        JFFERR("reading version failed");
    }
    versionBuf[strlen(_JFFVERSION_)] = '\0';

    int indexType;
    if(fread(&indexType, sizeof(int), 1, jffin) <= 0)
    {
        JFFERR("reading version failed");
    }

    if(indexType != this->getMyIndexInt())
    {
        switch(indexType)
        {
        case 1:
            std::cerr << "Map uses CellVector\n";
            return -1;
            break;
        case 2:
            std::cerr << "Map uses OctTree\n";
            return -2;
            break;
        case 3:
            std::cerr << "Map uses LazyGrid\n";
            return -3;
            break;
        }
    }

    switch(indexType)
    {
    case 1:
    {
        CellVector* cv = dynamic_cast<CellVector * >(index_);
        if(cv->loadFromJFF(jffin) < 0)
        {
            JFFERR("Error loading CellVector");
        }
        break;
    }
#if 0
    case 2:
    {
        OctTree* tr = dynamic_cast<OctTree*>(index_);
        if(tr->loadFromJFF(jffin) < 0)
        {
            JFFERR("Error loading OctTree");
        }
        break;
    }
#endif
    case 3:
    {
	std::cerr << "Map uses LazyGrid\n";
        LazyGrid* gr = dynamic_cast<LazyGrid*>(index_);
        if(gr->loadFromJFF(jffin) < 0)
        {
            JFFERR("Error loading LazyGrid");
        }
        break;
    }
    default:
        JFFERR("error casting index");
    }

    NDTCell *ptCell = new NDTCell();
    index_->setCellType(ptCell);
    delete ptCell;

   // std::cout << "map loaded successfully " << versionBuf << std::endl;

    isFirstLoad_ = false;

    return 0;

}

/// returns the current spatial index as a string (debugging function)
std::string NDTMap::getMyIndexStr() const
{
    CellVector* cl = dynamic_cast<CellVector * >(index_);
    if(cl!=NULL)
    {
        return std::string("CellVector");
    }
#if 0
    OctTree<PointT>* tr = dynamic_cast<OctTree<PointT>*>(index_);
    if(tr!=NULL)
    {
        return std::string("OctTree");
    }
#endif
    LazyGrid *gr = dynamic_cast<LazyGrid*>(index_);
    if(gr!=NULL)
    {
        return std::string("LazyGrid<PointT>");
    }

    return std::string("Unknown index type");
}

/// returns the current spatial index as an integer (debugging function)
int NDTMap::getMyIndexInt() const
{
    CellVector* cl = dynamic_cast<CellVector * >(index_);
    if(cl!=NULL)
    {
        return 1;
    }
#if 0
    OctTree<PointT>* tr = dynamic_cast<OctTree<PointT>*>(index_);
    if(tr!=NULL)
    {
        return 2;
    }
#endif
    LazyGrid *gr = dynamic_cast<LazyGrid*>(index_);
    if(gr!=NULL)
    {
        return 3;
    }

    return -1;
}

//computes the *negative log likelihood* of a single observation
double NDTMap::getLikelihoodForPoint(pcl::PointXYZ pt)
{
    //assert(false);
    double uniform=0.00100;
    NDTCell* ndCell = NULL;

#if 0
    OctTree<PointT>* tr = dynamic_cast<OctTree<PointT>*>(index_);

    if(tr==NULL)
    {
#endif
	LazyGrid *gr = dynamic_cast<LazyGrid*>(index_);
	if(gr==NULL)
	{
	    //cout<<"bad index - getLikelihoodForPoint\n";
	    return uniform;
	}
	ndCell = gr->getClosestNDTCell(pt);
#if 0
    }
    else
    {
        ndCell = tr->getClosestNDTCell(pt);
    }
#endif
    if(ndCell == NULL) return uniform;

    double prob = ndCell->getLikelihood(pt);
    prob = (prob<0) ? 0 : prob; //uniform!! TSV
    return prob;
}

/*
//use trilinear interpolation from available immediate neighbors
template<typename PointT>
double NDTMap<PointT>::getLikelihoodForPointWithInterpolation(PointT pt) {

    //ATM only for grid map
    //     tll------tlr
    //     /|       /|
    //    / |      / |
    //  tul------tur |    z
    //   | bll----|-blr   ^  y
    //   | /      | /     | /
    //   |/       |/      |/
    //  bul------bur      ---> x
    double uniform=0;//0.00100;
    Cell* cell = NULL;
    NDTCell<PointT>* ndCell = NULL;
    double cumProb = 0;
    double weight = 0;
    int evals = 1;

    LazyGrid<PointT> *gr = dynamic_cast<LazyGrid<PointT>*>(index_);
    if(gr==NULL) {
	//cout<<"bad index - getLikelihoodForPointWithInterpolation\n";
	return uniform;
    }
    cell = gr->getCellForPoint(pt);
    if(cell == NULL) return uniform;

    //get coordinates of cell
    int indXn, indYn, indZn;
    PointT centerGrid, sizeCell, centerCell;
    int sizeGridX, sizeGridY,sizeGridZ;
    centerCell = cell->getCenter();
    gr->getCenter(centerGrid.x,centerGrid.y,centerGrid.z);
    gr->getGridSize(sizeGridX,sizeGridY,sizeGridZ);
    gr->getCellSize(sizeCell.x,sizeCell.y,sizeCell.z);
    gr->getIndexForPoint(pt,indXn,indYn,indZn);

    double x,y,z;
    x = (pt.x - centerCell.x)/sizeCell.x;
    y = (pt.y - centerCell.y)/sizeCell.y;
    z = (pt.z - centerCell.z)/sizeCell.z;
    if(x <0 ) x = 0;
    if(y <0 ) y = 0;
    if(z <0 ) z = 0;
    if(x >1 ) x = 1;
    if(y >1 ) y = 1;
    if(z >1 ) z = 1;

    //bul
    double prob = 0;
    ndCell = dynamic_cast<NDTCell<PointT>*> (cell);
    if(ndCell != NULL) {
	prob = ndCell->getLikelihood(pt);
	prob = (prob<0) ? uniform : prob;
	weight = (1 - x + 1 - y + 1 - z)/(3.0);
	if(weight < 0) cerr<<weight<<endl;
	cumProb += prob*weight;
	//cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
	evals++;
    }

    //tul
    Cell* c = gr->getCellAt(indXn,indYn,indZn+1);
    if(c != NULL) {
	ndCell = dynamic_cast<NDTCell<PointT>*> (c);
	if(ndCell != NULL) {
	    prob = ndCell->getLikelihood(pt);
	    prob = (prob<0) ? uniform : prob;
	    weight = (1 - x + 1 - y + z)/(3.0);
	    if(weight < 0) cerr<<weight<<endl;
	    cumProb += prob*weight;
	    //cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
	    evals++;
	}
    }
    //tur
    c = gr->getCellAt(indXn+1,indYn,indZn+1);
    if(c != NULL) {
	ndCell = dynamic_cast<NDTCell<PointT>*> (c);
	if(ndCell != NULL) {
	    prob = ndCell->getLikelihood(pt);
	    prob = (prob<0) ? uniform : prob;
	    weight = (x + 1-y + z )/(3.0);
	    if(weight < 0) cerr<<weight<<endl;
	    cumProb += prob*weight;
	    //cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
	    evals++;
	}
    }
    //tll
    c = gr->getCellAt(indXn,indYn+1,indZn+1);
    if(c != NULL) {
	ndCell = dynamic_cast<NDTCell<PointT>*> (c);
	if(ndCell != NULL) {
	    prob = ndCell->getLikelihood(pt);
	    prob = (prob<0) ? uniform : prob;
	    weight = (1-x + y + z )/(3.0);
	    if(weight < 0) cerr<<weight<<endl;
	    cumProb += prob*weight;
	    //cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
	    evals++;
	}
    }
    //tlr
    c = gr->getCellAt(indXn+1,indYn+1,indZn+1);
    if(c != NULL) {
	ndCell = dynamic_cast<NDTCell<PointT>*> (c);
	if(ndCell != NULL) {
	    prob = ndCell->getLikelihood(pt);
	    prob = (prob<0) ? uniform : prob;
	    weight = (x + y + z )/(3.0);
	    if(weight < 0) cerr<<weight<<endl;
	    cumProb += prob*weight;
	    //cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
	    evals++;
	}
    }
    //bur
    c = gr->getCellAt(indXn+1,indYn,indZn);
    if(c != NULL) {
	ndCell = dynamic_cast<NDTCell<PointT>*> (c);
	if(ndCell != NULL) {
	    prob = ndCell->getLikelihood(pt);
	    prob = (prob<0) ? uniform : prob;
	    weight = (x + 1-y + 1-z )/(3.0);
	    if(weight < 0) cerr<<weight<<endl;
	    cumProb += prob*weight;
	    //cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
	    evals++;
	}
    }
    //bll
    c = gr->getCellAt(indXn,indYn+1,indZn);
    if(c != NULL) {
	ndCell = dynamic_cast<NDTCell<PointT>*> (c);
	if(ndCell != NULL) {
	    prob = ndCell->getLikelihood(pt);
	    prob = (prob<0) ? uniform : prob;
	    weight = (1-x + y + 1-z )/(3.0);
	    if(weight < 0) cerr<<weight<<endl;
	    cumProb += prob*weight;
	    //cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
	    evals++;
	}
    }
    //blr
    c = gr->getCellAt(indXn+1,indYn+1,indZn);
    if(c != NULL) {
	ndCell = dynamic_cast<NDTCell<PointT>*> (c);
	if(ndCell != NULL) {
	    prob = ndCell->getLikelihood(pt);
	    prob = (prob<0) ? uniform : prob;
	    weight = (x + y + 1-z )/(3.0);
	    if(weight < 0) cerr<<weight<<endl;
	    cumProb += prob*weight;
	    //cout<<"\t"<<weight<<" "<<prob<<" --> "<<cumProb<<endl;
	    evals++;
	}
    }

    //cout<<"== "<<cumProb<<endl;
    return cumProb;
}
*/

std::vector<NDTCell*> NDTMap::getInitializedCellsForPoint(const pcl::PointXYZ pt) const
{
    std::vector<NDTCell*> cells;
    LazyGrid *gr = dynamic_cast<LazyGrid*>(index_);
    if(gr==NULL)
    {
        //cout<<"bad index - getCellsForPoint\n";
        return cells;
    }
    cells = gr->getClosestCells(pt);
    return cells;

}

std::vector<NDTCell*> NDTMap::getCellsForPoint(const pcl::PointXYZ pt, int n_neigh, bool checkForGaussian) const
{
    //assert(false);
    std::vector<NDTCell*> cells;
    LazyGrid *gr = dynamic_cast<LazyGrid*>(index_);
    if(gr==NULL)
    {
        //cout<<"bad index - getCellsForPoint\n";
        return cells;
    }
    cells = gr->getClosestNDTCells(pt,n_neigh,checkForGaussian);
    return cells;

    //OctTree<PointT>* tr = dynamic_cast<OctTree<PointT>*>(index_);
    //if(tr==NULL) {
    //}
}

bool NDTMap::getCellForPoint(const pcl::PointXYZ &pt, NDTCell* &out_cell, bool checkForGaussian) const
{

    out_cell = NULL;
    CellVector *cl = dynamic_cast<CellVector*>(index_);
    if(cl!=NULL)
    {
        out_cell = cl->getClosestNDTCell(pt);
        return true;
    }
#if 0
    OctTree<PointT>* tr = dynamic_cast<OctTree<PointT>*>(index_);
    if(tr!=NULL)
    {
        out_cell = tr->getClosestNDTCell(pt);
        return true;
    }
#endif
    LazyGrid *gr = dynamic_cast<LazyGrid*>(index_);
    if(gr!=NULL)
    {
        out_cell = gr->getClosestNDTCell(pt,checkForGaussian);
        return true;
    }
    //cout<<"bad index - getCellForPoint\n";
    return false;
}

NDTMap* NDTMap::pseudoTransformNDTMap(Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T)
{
    NDTMap* map = new NDTMap(new CellVector(), true);
    CellVector* idx = dynamic_cast<CellVector*> (map->getMyIndex());
    typename SpatialIndex::CellVectorItr it = index_->begin();

    while (it != index_->end())
    {
        NDTCell *cell = (*it);
	if(cell->hasGaussian_)
	{

	    Eigen::Vector3d mean = cell->getMean();
	    Eigen::Matrix3d cov = cell->getCov();
	    mean = T*mean;
	    ///NOTE: The rotation of the covariance fixed by Jari 6.11.2012
	    cov = T.rotation()*cov*T.rotation().transpose();
	    NDTCell* nd = (NDTCell*)cell->clone();
	    nd->setMean(mean);
	    nd->setCov(cov);
	    idx->addNDTCell(nd);
	}
        it++;
    }
    return map;
}

std::vector<NDTCell*> NDTMap::pseudoTransformNDT(Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T) const
{

    std::vector<NDTCell*> ret;
    typename SpatialIndex::CellVectorConstItr it = index_->begin();
    while (it != index_->end())
    {
        NDTCell *cell = (*it);
        if(cell!=NULL)
        {
            if(cell->hasGaussian_)
            {
                Eigen::Vector3d mean = cell->getMean();
                Eigen::Matrix3d cov = cell->getCov();
                mean = T*mean;
                ///NOTE: The rotation of the covariance fixed by Jari 6.11.2012
                cov = T.rotation()*cov*T.rotation().transpose();
                NDTCell* nd = (NDTCell*)cell->clone();
                nd->setMean(mean);
                nd->setCov(cov);
                ret.push_back(nd);
            }
        }
        else
        {
            //ERR("problem casting cell to NDT!\n");
        }
        it++;
    }
    return ret;
}

NDTCell* NDTMap::getCellIdx(unsigned int idx) const
{
    CellVector *cl = dynamic_cast<CellVector*>(index_);
    if (cl != NULL)
    {
        return cl->getCellIdx(idx);
    }
    return NULL;
}

std::vector<perception_oru::NDTCell*> NDTMap::getAllCells() const
{

    std::vector<NDTCell*> ret;
    typename SpatialIndex::CellVectorItr it = index_->begin();
    while (it != index_->end())
    {
        NDTCell *cell = (*it);
	if(cell->hasGaussian_)
	{
	    NDTCell* nd = cell->copy();
	    ret.push_back(nd);
	}
        it++;
    }
    return ret;
}

std::vector< boost::shared_ptr<perception_oru::NDTCell> > NDTMap::getAllCellsShared() const
{

    std::vector< boost::shared_ptr< NDTCell > > ret;
    typename SpatialIndex::CellVectorItr it = index_->begin();
    while (it != index_->end())
    {

        NDTCell *cell = (*it);
		if(cell->hasGaussian_)
		{

			NDTCell* nd = cell->copy();
			boost::shared_ptr< NDTCell > smart_pointer(nd);
// 			NDTCell** ndd = &nd;
			ret.push_back(smart_pointer);
		}
        it++;
    }
    std::cout << "Return " << ret.size() << std::endl;
    return ret;
}

std::vector<perception_oru::NDTCell*> NDTMap::getAllInitializedCells() const
{
    std::vector<NDTCell*> ret;
    typename SpatialIndex::CellVectorItr it = index_->begin();
    while (it != index_->end())
    {
	NDTCell* nd = (*it)->copy();
	ret.push_back(nd);
        it++;
    }
    return ret;
}

std::vector< boost::shared_ptr<perception_oru::NDTCell> > NDTMap::getAllInitializedCellsShared() const
{
    std::vector<boost::shared_ptr<NDTCell> > ret;
    typename SpatialIndex::CellVectorItr it = index_->begin();
    while (it != index_->end())
    {
		NDTCell* nd = (*it)->copy();
		boost::shared_ptr< NDTCell > smart_pointer(nd);
		ret.push_back(smart_pointer);
        it++;
    }
    return ret;
}

int NDTMap::numberOfActiveCells()
{
    int ret = 0;
    if(index_ == NULL) return ret;
    typename SpatialIndex::CellVectorItr it = index_->begin();
    while (it != index_->end())
    {
	if((*it)->hasGaussian_)
	{
	    ret++;
	}
	it++;
    }
    return ret;
}

int NDTMap::numberOfActiveCells() const
{
    int ret = 0;
    if(index_ == NULL) return ret;
    typename SpatialIndex::CellVectorItr it = index_->begin();
    while (it != index_->end())
    {
	if((*it)->hasGaussian_)
	{
	    ret++;
	}
	it++;
    }
    return ret;
}
NDTCell* NDTMap::getCellAtID(int x,int y,int z) const {
    NDTCell* cell;
    LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
    lz->getCellAt(x,y,z,cell);
    return cell;
}

bool NDTMap::insertCell(NDTCell cell){
    LazyGrid* gr = dynamic_cast<LazyGrid*>(index_);
    gr->insertCell(*cell.copy());
}
std::string NDTMap::ToString(){
   std::stringstream ss;
   if(index_!=NULL)
    ss<<"NDTMap: index= \n"<<index_->ToString()<<std::endl;
return ss.str();

}
}


/*
 * Pedersen.cpp
 *
 *  Created on: 04.10.2010
 *      Author: stephaniebayer
 */
#include "Pedersen.h"
#include "G_q.h"
#include <NTL/ZZ.h>
NTL_CLIENT

#include "Mod_p.h"
#include "multi_expo.h"
#include <stdio.h>
#include <time.h>
#include <vector>
#include <fstream>

extern double time_cm;

Pedersen::Pedersen() {
	// TODO Auto-generated constructor stub

}

//Generates an instance of Pedersen with group H, which is able to commit up to n elements
Pedersen::Pedersen(long n, G_q H){
	long i;
	bool b;
	ZZ ran;
	Mod_p temp;
	G = H;
	gen = new vector<Mod_p>(n+1);
	SetSeed(to_ZZ(time(0)));
/*	string name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"ck for pedersen ";*/
	for (i =0; i <=n; i++)
	{
		b = true;
		while(b){
			ran = RandomBnd(G.get_ord());
			temp = G.get_gen().expo(ran);
			if (G.is_generator(temp))
			{
				(*gen).at(i)=temp;
			//	ost<<temp<<" ";
				b = false;
			}
		}
	}
//	ost<<endl;
}

/*Generates an instance of Pedersen with group G_q subset of Z_p and order o and generator g,
 * and which is able to commit up to n elements */
Pedersen::Pedersen(Mod_p g, long o, long p, long n){
	long i;
	bool b;
	ZZ ran;
	G = G_q(g, o, p);
	gen = new vector<Mod_p>(n+1);
	SetSeed(to_ZZ(time(0)));
	for (i =0; i <=n; i++)
	{
		b = true;
		while(b){
		ran = RandomBnd(G.get_mod());
		if (G.is_generator(ran))
		{
			gen-> at(i)=Mod_p(ran,G.get_mod());
			b = false;
		}
		}
	}

}

/*Generates an instance of Pedersen with group G_q subset of Z_p and order o and generator g,
 * and which is able to commit up to n elements */
Pedersen::Pedersen(Mod_p g, long o, ZZ p, long n){
	long i;
	bool b;
	ZZ ran;
	G = G_q(g, o, p);
	gen = new vector<Mod_p>(n+1);
	SetSeed(to_ZZ(time(0)));
	for (i =0; i <=n; i++)
	{
		b = true;
		while(b){
			ran = RandomBnd(G.get_mod());
			if (G.is_generator(ran))
			{
				gen-> at(i)=Mod_p(ran,G.get_mod());
				b = false;
			}
		}
	}
}

/*Generates an instance of Pedersen with group G_q subset of Z_p and order o and generator g,
 * and which is able to commit up to n elements */
Pedersen::Pedersen(Mod_p g, ZZ o, ZZ p, long n){
	long i;
	bool b;
	ZZ ran;
	G = G_q(g, o, p);
	gen = new vector<Mod_p>(n+1);
	SetSeed(to_ZZ(time(0)));
	for (i =0; i <=n; i++)
	{
		b = true;
		while(b){
			ran = RandomBnd(G.get_mod());
			if (G.is_generator(ran))
			{
				gen-> at(i)=Mod_p(ran,G.get_mod());
				b = false;
			}
		}
	}
	gen_prec = 0;

}

/*Generates an instance of Pedersen with group G_q subset of Z_p and order o and the generator g has value g_val,
 * and which is able to commit up to n elements */
Pedersen::Pedersen(long g_val, long o, long p, long n){
	long i;
	bool b;
	ZZ ran;
	G = G_q(g_val, o, p);
	gen = new vector<Mod_p>(n+1);
	SetSeed(to_ZZ(time(0)));
	for (i =0; i <=n; i++)
	{
		b = true;
		while(b){
			ran = RandomBnd(G.get_mod());
			if (G.is_generator(ran))
			{
				gen-> at(i)=Mod_p(ran,G.get_mod());
				b = false;
			}
		}
	}
	gen_prec = 0;

}

/*Generates an instance of Pedersen with group G_q subset of Z_p and order o and the generator g has value g_val,
 * and which is able to commit up to n elements */
Pedersen::Pedersen(long g_val, long o, ZZ p, long n){
	long i;
	bool b;
	ZZ ran;
	G = G_q(g_val, o, p);
	gen = new vector<Mod_p>(n+1);
	SetSeed(to_ZZ(time(0)));
	for (i =0; i <=n; i++)
	{
		b = true;
		while(b){
			ran = RandomBnd(G.get_mod());
			if (G.is_generator(ran))
			{
				gen-> at(i)=Mod_p(ran,G.get_mod());
				b = false;
			}
		}
	}
	gen_prec = 0;
}

/*Generates an instance of Pedersen with group G_q subset of Z_p and order o and the generator g has value g_val,
 * and which is able to commit up to n elements */
Pedersen::Pedersen(long g_val, ZZ o, ZZ p, long n){
	long i;
	bool b;
	ZZ ran;
	G = G_q(g_val, o, p);
	gen = new vector<Mod_p>(n+1);
	SetSeed(to_ZZ(time(0)));
	for (i =0; i <=n; i++)
	{
		b = true;
		while(b){
			ran = RandomBnd(G.get_mod());
			if (G.is_generator(ran))
			{
				gen->at(i)=Mod_p(ran,G.get_mod());
				b = false;
			}
		}
	}
	gen_prec = 0;

}

/*Generates an instance of Pedersen with group G_q subset of Z_p and order o and the generator g has value g_val,
 * and which is able to commit up to n elements */
Pedersen::Pedersen(ZZ g_val, long o, long p, long n){
	long i;
	bool b;
	ZZ ran;
	G = G_q(g_val, o, p);
	gen = new vector<Mod_p>(n+1);
	SetSeed(to_ZZ(time(0)));
	for (i =0; i <=n; i++)
	{
		b = true;
		while(b){
			ran = RandomBnd(G.get_mod());
			if (G.is_generator(ran))
			{
				gen->at(i)=Mod_p(ran,G.get_mod());
				b = false;
			}
		}
	}
	gen_prec = 0;
}

/*Generates an instance of Pedersen with group G_q subset of Z_p and order o and the generator g has value g_val,
 * and which is able to commit up to n elements */
Pedersen::Pedersen(ZZ g_val, long o, ZZ p, long n){
	long i;
	bool b;
	ZZ ran;
	G = G_q(g_val, o, p);
	gen = new vector<Mod_p>(n+1);
	SetSeed(to_ZZ(time(0)));
	for (i =0; i <=n; i++)
	{
		b = true;
		while(b){
			ran = RandomBnd(G.get_mod());
			if (G.is_generator(ran))
			{
				gen->at(i)=Mod_p(ran,G.get_mod());
				b = false;
			}
		}
	}
	gen_prec = 0;
}

/*Generates an instance of Pedersen with group G_q subset of Z_p and order o and the generator g has value g_val,
 * and which is able to commit up to n elements */
Pedersen::Pedersen(ZZ g_val, ZZ o, ZZ p, long n){
	long i;
	bool b;
	ZZ ran;
	G = G_q(g_val, o, p);
	gen = new vector<Mod_p>(n+1);
	SetSeed(to_ZZ(time(0)));
	for (i =0; i <=n; i++)
	{
		b = true;
		while(b){
			ran = RandomBnd(G.get_mod());
			if (G.is_generator(ran))
			{
				gen->at(i)=Mod_p(ran,G.get_mod());
				b = false;
			}
		}
	}
	gen_prec = 0;
}

/*Generates an instance of Pedersen with group G_q subset of Z_p, p is the modular value of g
 * and order o and the generator g, and which is able to commit up to n elements */
Pedersen::Pedersen(Mod_p g, long o,  long n){
	long i;
	bool b;
	ZZ ran;
	G = G_q(g, o);
	gen = new vector<Mod_p>(n+1);
	SetSeed(to_ZZ(time(0)));
	for (i =0; i <=n; i++)
	{
		b = true;
		while(b){
			ran = RandomBnd(G.get_mod());
			if (G.is_generator(ran))
			{
				gen->at(i)=Mod_p(ran,G.get_mod());
				b = false;
			}
		}
	}

	gen_prec = 0;
}

/*Generates an instance of Pedersen with group G_q subset of Z_p, p is the modular value of g
 * and order o and the generator g, and which is able to commit up to n elements */
Pedersen::Pedersen(Mod_p g, ZZ o,  long n){
	long i;
	bool b;
	ZZ ran;
	G = G_q(g, o);
	gen = new vector<Mod_p>(n+1);
	SetSeed(to_ZZ(time(0)));
	for (i =0; i <=n; i++)
	{
		b = true;
		while(b){
			ran = RandomBnd(G.get_mod());
			if (G.is_generator(ran))
			{
				gen->at(i)=Mod_p(ran,G.get_mod());
				b = false;
			}
		}
	}
	gen_prec = 0;
}

/*Generates an instance of Pedersen with group G_q subset of Z_p, and order o,
 *  and which is able to commit up to n elements */
Pedersen::Pedersen( long o, long mod,  long n){
	long i;
	bool b;
	ZZ ran;
	G = G_q(o,mod);
	gen = new vector<Mod_p>(n+1);
	SetSeed(to_ZZ(time(0)));
	for (i =0; i <=n; i++)
	{
		b = true;
		while(b){
			ran = RandomBnd(G.get_mod());
			if (G.is_generator(ran))
			{
				gen->at(i)=Mod_p(ran,G.get_mod());
				b = false;
			}
		}
	}
	gen_prec = 0;
}

/*Generates an instance of Pedersen with group G_q subset of Z_p, and order o,
 *  and which is able to commit up to n elements */
Pedersen::Pedersen( long o, ZZ mod,  long n){
	long i;
	bool b;
	ZZ ran;
	G = G_q(o,mod);
	gen = new vector<Mod_p>(n+1);
	SetSeed(to_ZZ(time(0)));
	for (i =0; i <=n; i++)
	{
		b = true;
		while(b){
			ran = RandomBnd(G.get_mod());
			if (G.is_generator(ran))
			{
				gen->at(i)=Mod_p(ran,G.get_mod());
				b = false;
			}
		}
	}
	gen_prec = 0;
}

/*Generates an instance of Pedersen with group G_q subset of Z_p, and order o,
 *  and which is able to commit up to n elements */
Pedersen::Pedersen( ZZ o, ZZ mod,  long n){
	long i;
	bool b;
	ZZ ran;
	G = G_q(o,mod);
	gen = new vector<Mod_p>(n+1);
	SetSeed(to_ZZ(time(0)));
	for (i =0; i <=n; i++)
	{
		b = true;
		while(b){
			ran = RandomBnd(G.get_mod());
			if (G.is_generator(ran))
			{
				gen->at(i)=Mod_p(ran,G.get_mod());
				b = false;
			}
		}
	}
	gen_prec = 0;
}

Pedersen::~Pedersen() {

//	delete gen;
	// TODO Auto-generated destructor stub
}

//returns the group of the instance
G_q Pedersen::get_group() const{

	return G;
}

void Pedersen:: set_omega(int o1, int o2, int o3){

	omega_expo = o1;
	omega_ll = o2;
	omega_sw = o3;
	gen_prec = precomp(gen->at(0).get_val(), gen->at(1).get_val());
}

//returns the number of generators used, it is possible to commit up to size()-1 values
long Pedersen::get_length() const{
	return gen->size();
}

//returns a list of all generators
vector<Mod_p>* Pedersen::get_gen()const{
	return gen;
}

//Calculates the commitment to the values in t using randomness r
Mod_p Pedersen::commit(const vector<ZZ>*  t, ZZ ran){
	double tstart, tstop, ttime;
	ZZ temp,temp_1,mod;
	long i;

	tstart = (double)clock()/CLOCKS_PER_SEC;
	mod = G.get_mod();
	if ((long) t->size() > (long) gen->size())
	{cout << "to many elements to commit to" << endl;}
	else
	{
		PowerMod(temp, gen->at(0).get_val(),ran, mod);
		for (i = 1; i<=(long) t->size(); i++)
		{
			PowerMod(temp_1,gen->at(i).get_val(),t->at(i-1),mod);
			MulMod(temp,temp,temp_1, mod);
		}
	}
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_cm+=ttime;
	return Mod_p(temp, mod);
}

//Calculates the commitment to the values in t using randomness r
Mod_p Pedersen::commit(const vector<Mod_p>*  t,  ZZ ran){
	double tstart, tstop, ttime;
	ZZ temp,temp_1;
	ZZ mod = G.get_mod();
	long i;

	tstart = (double)clock()/CLOCKS_PER_SEC;
	if ((long) t->size()> (long) gen->size())
	{cout << "to many elements to commit to" << endl;}
	else
	{
		PowerMod(temp,gen->at(0).get_val(),ran, mod);
		for (i = 1; i<=(long) t->size(); i++)
		{
			PowerMod(temp_1,gen->at(i).get_val(),t->at(i-1).get_val(),mod);
			MulMod(temp , temp,temp_1,mod);
		}
	}
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_cm+=ttime;
	return Mod_p(temp,mod);
}

//Calculates the commitment to t using randomness r
Mod_p Pedersen::commit(const vector<Mod_p>*  t){
	double tstart, tstop, ttime;
	ZZ temp,temp_1;
	long i;
	ZZ ran;
	ZZ mod = G.get_mod();

	tstart = (double)clock()/CLOCKS_PER_SEC;
	SetSeed(to_ZZ(time(0)));
	ran = RandomBnd(mod);
     cout<< "ran: " << ran << endl;
	if ((long) t->size()> (long) gen->size())
	{cout << "to many elements to commit to" << endl;}
	else
	{
		PowerMod(temp,gen->at(0).get_val(),ran, mod);
		for (i = 1; i<=(long) t->size(); i++)
		{
			PowerMod(temp_1,gen->at(i).get_val(),t->at(i-1).get_val(),mod);
			MulMod(temp , temp, temp_1,mod);
		}
	}
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_cm+=ttime;
	return Mod_p(temp, mod);
}

//Calculates the commitment to  t using randomness r
Mod_p Pedersen::commit(ZZ t,  ZZ ran){

	ZZ temp,temp_1;
	ZZ mod = G.get_mod();

	//cout<<" in com ";
	PowerMod(temp,gen->at(0).get_val(),ran, mod);
	PowerMod(temp_1,gen->at(1).get_val(),t,mod);
	MulMod(temp , temp,temp_1,mod);

	return Mod_p(temp, mod);
}

//Calculates the commitment to t using randomness r
Mod_p Pedersen::commit(Mod_p t,  long ran){

	ZZ temp,temp_1;
	ZZ mod = G.get_mod();
	PowerMod(temp,gen->at(0).get_val(),ran, mod);
	PowerMod(temp_1,gen->at(1).get_val(),t.get_val(),mod);
	MulMod(temp , temp,temp_1,mod);

	return Mod_p(temp,mod);
}

//Calculates the commitment to t using randomness r
Mod_p Pedersen::commit(Mod_p t, ZZ ran){
	ZZ temp,temp_1;
	ZZ mod = G.get_mod();
	PowerMod(temp,gen->at(0).get_val(),ran, mod);
	PowerMod(temp_1,gen->at(1).get_val(),t.get_val(),mod);
	MulMod(temp,temp,temp_1,mod);

	return Mod_p(temp, mod);
}

//Calculates the commitments to the values in t, generating a random number
Mod_p Pedersen::commit(Mod_p t){

	ZZ temp,temp_1;
	ZZ ran;
	ZZ mod = G.get_mod();
	SetSeed(to_ZZ(time(0)));
	ran = RandomBnd(mod);
     cout<< "ran: " << ran << endl;

	PowerMod(temp,gen->at(0).get_val(),ran, mod);
	PowerMod(temp_1,gen->at(1).get_val(),t.get_val(),mod);
	MulMod(temp,temp,temp_1,mod);

	return Mod_p(temp, mod);
}

//Calculates the commitment to the values in t using randomness r
Mod_p Pedersen::commit_opt(const vector<ZZ>*  t, ZZ ran){
	double tstart, tstop, ttime;
	ZZ temp;
	Mod_p temp_1;
	long length = t->size();
	long i;

	tstart = (double)clock()/CLOCKS_PER_SEC;
	if (length > (long) gen->size())
	{cout << "to many elements to commit to" << endl;}
	else
	{
		/*if(length <= 1000){
			vector<ZZ>* expo = new vector<ZZ>(length+1);
			expo->at(0)=ran;
			for(i=1; i<=length; i++){
					expo->at(i) = t->at(i-1);
			}
			multi_expo::multi_expo_LL(temp_1, gen, expo, omega_ll);
			temp = temp_1.get_val();
		} else {*/
			multi_expo::expo_mult(temp, t, ran, omega_expo, gen);
		//}
	}

	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_cm+=ttime;
	return Mod_p(temp, G.get_mod());
}

//Calculates the commitment to  t using randomness r
Mod_p Pedersen::commit_sw(ZZ t,  ZZ ran){

	ZZ temp;
	multi_expo::multi_expo_sw(temp,ran, t, omega_sw, gen_prec);

	return Mod_p(temp, G.get_mod());
}

//Calculates the commitment to t using randomness r
Mod_p Pedersen::commit_sw(Mod_p t,  long ran){

	ZZ temp;
	multi_expo::multi_expo_sw(temp,to_ZZ(ran), t.get_val(), omega_sw, gen_prec);

	return Mod_p(temp, G.get_mod());
}

//Calculates the commitment to t using randomness r
Mod_p Pedersen::commit_sw(Mod_p t, ZZ ran){

	ZZ temp;
	multi_expo::multi_expo_sw(temp,to_ZZ(ran), t.get_val(), omega_sw, gen_prec);
	return Mod_p(temp, G.get_mod());
}

//Calculates the commitments to the values in t, generating a random number
Mod_p Pedersen::commit_sw(Mod_p t){

	ZZ temp;
	ZZ ran;
	ZZ mod = G.get_mod();
	SetSeed(to_ZZ(time(0)));
	ran = RandomBnd(mod);

	multi_expo::multi_expo_sw(temp,to_ZZ(ran), t.get_val(), omega_sw, gen_prec);

	return Mod_p(temp, mod);
}

void Pedersen::operator =(const Pedersen& el){
	G = el.get_group();
	gen = el.get_gen();
}

long Pedersen::to_long(vector<int>* bit_r){

	long  t, length;
	double two,i;
	two = 2;

	length =bit_r->size();
	t=0;
	for(i = 0; i<length; i++ ){
		t = t+bit_r->at(i)*pow(two,i);
	}
	return t;
}

void Pedersen::to_long(long& t, vector<int>* bit_r){

	long   length;
	double two,i;
	two = 2;

	length =bit_r->size();
	t=0;
	for(i = 0; i<length; i++ ){
		t = t+bit_r->at(i)*pow(two,i);
	}
	//return t;
}

vector<vector<ZZ>* >* Pedersen::precomp(ZZ g, ZZ h){
	vector<vector<ZZ>* >* pre;
	vector<ZZ>* t_1;
	vector<ZZ>* t_2;
	ZZ temp_1, temp_2;
	ZZ mod = G.get_mod();
	double two;
	long i,t;

	pre = new vector<vector<ZZ> *>(2);
	two = 2;
	t = pow(two, omega_sw-1);
	t_1 = new vector<ZZ>(t);
	t_2 = new vector<ZZ>(t);

	temp_1 = sqr(g);
	temp_2 = sqr(h);
	t_1->at(0)= g;
	t_2->at(0)= h;
	for(i = 1; i<t; i++){
		t_1->at(i)= MulMod(t_1->at(i-1),temp_1,mod);
		t_2->at(i)= MulMod(t_2->at(i-1),temp_2,mod);
	}

	pre->at(0)= t_1;
	pre->at(1)= t_2;
	return pre;
}


//==================================================================================================
/*!
  @file

  @copyright 2012-2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_DEFINITION_INVPI_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_DEFINITION_INVPI_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/dispatch/function/make_callable.hpp>
#include <boost/dispatch/hierarchy/functions.hpp>
#include <boost/dispatch/as.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    struct invpi_ : boost::dispatch::constant_value_<invpi_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,invpi_,boost::dispatch::constant_value_<invpi_>);
      BOOST_SIMD_REGISTER_CONSTANT(0, 0X3EA2F983, 0X3FD45F306DC9C883LL);
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag,invpi_);
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::invpi_,invpi);
  }

  template<typename T> BOOST_FORCEINLINE auto Invpi()
  BOOST_NOEXCEPT_DECLTYPE(detail::invpi( boost::dispatch::as_<T>{}))
  {
    return detail::invpi( boost::dispatch::as_<T>{} );
  }
} }

#endif


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/strong_components.hpp>
#include <boost/graph/random.hpp>
#include <boost/random.hpp>
#include <stack>
#include <deque>
#include <iostream>
#include <vector>

//==============================================================================
//
struct node_properties
{
  int component;
};
struct edge_properties
{
};
struct graph_properties
{
  int number_of_components;
};

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
                              node_properties,
                              edge_properties,
                              graph_properties>
                                graph_t;
typedef typename boost::graph_traits<graph_t>::vertex_descriptor vertex_t;
typedef typename boost::graph_traits<graph_t>::edge_descriptor edge_t;

graph_t graph;
std::vector<vertex_t> vertices;

int nn = 10;

std::vector<int> r_index(nn, 0);
std::vector<int> root(nn, 0);
std::deque<int> vS;
std::deque<int> iS;
int vindex = 1;
int c = nn - 1; // component number

void beginVisiting(int v)
{ // total: n(1 + 2w)
    vS.push_front(v);
    iS.push_front(0);
    root[v] = true;
    r_index[v] = vindex;
    vindex = vindex + 1;
}

void finishVisiting(int v)
{
    // Take this vertex off the call stacks
    vS.pop_front();
    iS.pop_front();
    // Update component information
    if (root[v])
    {
        vindex = vindex - 1;
        while (!vS.empty() && (r_index[v] <= r_index[vS.back()]))
        {
            int w = vS.back();
            vS.pop_back();
            r_index[w] = c;
            vindex = vindex - 1;
        }
        r_index[v] = c;
        c = c - 1;
    }
    else
    {
        vS.push_back(v);
    }
}

bool beginEdge(int v, int k)
{
    auto edges = boost::edges(graph);
    for (auto it = edges.first; it != edges.second; ++it)
    {
        int t = boost::target(*it, graph);
        int s = boost::source(*it, graph);
        if (v == s)
        {
            if (r_index[t] == 0)
            {
                iS.pop_front();
                iS.push_front(k + 1);
                beginVisiting(t);
                return true;
            }
            else
            {
                return false;
            }
        }
    }
}

void finishEdge(int v, int k)
{
    //int[] g_edges = graph.edges(v);
    //int w = g_edges[k];
    auto edges = boost::edges(graph);
    for (auto it = edges.first; it != edges.second; ++it)
    {
        int t = boost::target(*it, graph);
        int s = boost::source(*it, graph);
        if (v == s)
        {
            if (r_index[t] < r_index[v])
            {
                r_index[v] = r_index[t];
                root[v] = false;
            }
        }
    }
    return;
}

void visitLoop()
{
    int v = vS.front();
    int i = iS.front();
    auto edges = boost::edges(graph);
    size_t e = boost::num_edges(graph);
    while (i <= e)
    {
        if (i > 0)
        {
            finishEdge(v, i-1);
        }
        if (i < e && beginEdge(v, i))
        {
            return;
        }
        i++;
        std::cout<<"visitI \n" << i;
    }
    finishVisiting(v);
}

void visitI(int v)
{
    beginVisiting(v);

    while (!vS.empty())
    {
        visitLoop();
    }
    return;
}

//==============================================================================
int main()
{

    ///graph 1

    // vertices.push_back(boost::add_vertex(graph));
    // vertices.push_back(boost::add_vertex(graph));
    // vertices.push_back(boost::add_vertex(graph));
    // vertices.push_back(boost::add_vertex(graph));
    // vertices.push_back(boost::add_vertex(graph));
    // vertices.push_back(boost::add_vertex(graph));

    // boost::add_edge(vertices.at(0), vertices.at(1), graph);
    // boost::add_edge(vertices.at(1), vertices.at(2), graph);
    // boost::add_edge(vertices.at(2), vertices.at(0), graph);
    // boost::add_edge(vertices.at(2), vertices.at(3), graph);
    // boost::add_edge(vertices.at(3), vertices.at(4), graph);
    // boost::add_edge(vertices.at(4), vertices.at(5), graph);
    // boost::add_edge(vertices.at(5), vertices.at(3), graph);

    //graph2
    vertices.push_back(boost::add_vertex(graph));
    vertices.push_back(boost::add_vertex(graph));
    vertices.push_back(boost::add_vertex(graph));
    vertices.push_back(boost::add_vertex(graph));
    vertices.push_back(boost::add_vertex(graph));
    vertices.push_back(boost::add_vertex(graph));
    vertices.push_back(boost::add_vertex(graph));
    vertices.push_back(boost::add_vertex(graph));
    vertices.push_back(boost::add_vertex(graph));
    vertices.push_back(boost::add_vertex(graph));
    boost::add_edge(vertices.at(0), vertices.at(4), graph);
    boost::add_edge(vertices.at(0), vertices.at(1), graph);
    boost::add_edge(vertices.at(4), vertices.at(0), graph);
    boost::add_edge(vertices.at(4), vertices.at(1), graph);
    boost::add_edge(vertices.at(4), vertices.at(5), graph);
    boost::add_edge(vertices.at(5), vertices.at(6), graph);
    boost::add_edge(vertices.at(6), vertices.at(4), graph);
    boost::add_edge(vertices.at(1), vertices.at(2), graph);
    boost::add_edge(vertices.at(2), vertices.at(7), graph);
    boost::add_edge(vertices.at(2), vertices.at(3), graph);
    boost::add_edge(vertices.at(3), vertices.at(1), graph);
    boost::add_edge(vertices.at(8), vertices.at(9), graph);
    boost::add_edge(vertices.at(9), vertices.at(8), graph);

    /* boost::add_edge(vertices.at(0), vertices.at(1), graph);
    boost::add_edge(vertices.at(2), vertices.at(3), graph);
    boost::add_edge(vertices.at(3), vertices.at(4), graph);
    boost::add_edge(vertices.at(4), vertices.at(5), graph);
    boost::add_edge(vertices.at(7), vertices.at(8), graph);
    boost::add_edge(vertices.at(8), vertices.at(9), graph);
    boost::add_edge(vertices.at(9), vertices.at(7), graph);*/

    //random generation;
    boost::mt19937 rng;
    boost::generate_random_graph(graph, nn, 8, rng);
    size_t e = boost::num_edges(graph);
    size_t n = boost::num_vertices(graph);
    std::cout << "generated " << e << " edges, " << n << " vertices\n";

    // graph[boost::graph_bundle].number_of_components =
    //     boost::connected_components(graph,
    //                                 boost::get(&node_properties::component, graph));

    //std::cout << "Without adaption:\n-----------------" << std::endl;
    //print_graph(graph);
    //print_components(graph);
    visitI(0);

    std::cout
        << "\n Components number SCC1: ----------" << c << std::endl;
    std::cout << "r_index: ----------" << std::endl;
    for (auto i = r_index.begin(); i != r_index.end(); ++i)
        std::cout << *i << ' ';
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2009-2011 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STAR_COMB_HPP
#define BOOST_GEOMETRY_STAR_COMB_HPP

#include <iostream>
#include <string>
#include <vector>

#include <boost/timer.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/math/constants/constants.hpp>

#include <test_overlay_p_q.hpp>

template <typename Polygon, typename AddFunctor>
inline void make_star(Polygon& polygon, AddFunctor functor,
            int count, double factor1, double factor2,
            double offset = 0.0,
            bool close = true,
            double orientation = 1.0)
{
    // Create star
    double cx = 25.0;
    double cy = 25.0;

    double dx = 50.0;
    double dy = 50.0;

    double a1 = factor1 * 0.5 * dx;
    double b1 = factor1 * 0.5 * dy;
    double a2 = factor2 * 0.5 * dx;
    double b2 = factor2 * 0.5 * dy;

    double delta = orientation * boost::math::constants::pi<double>() * 2.0 / (count - 1);
    double angle = offset * delta;
    double x0, y0;
    bool first = true;
    for (int i = 0; i < count - 1; i++, angle += delta)
    {
        bool even = i % 2 == 0;
        double x = cx + (even ? a1 : a2) * sin(angle);
        double y = cy + (even ? b1 : b2) * cos(angle);
        functor(polygon, x, y, i);
        if (first)
        {
            x0 = x;
            y0 = y;
            first = false;
        }

    }
    if (close)
    {
        functor(polygon, x0, y0, count);
    }
}

template <typename Vector>
void ccw_pushback(Vector& vector, double x, double y, int)
{
    vector.push_back(boost::make_tuple(x, y));
}

template <typename Polygon, typename AddFunctor>
inline void make_comb(Polygon& polygon, AddFunctor functor,
            int count,
            bool close = true,
            bool clockwise = true)
{
    int n = 0;

    if (! clockwise)
    {
        typedef boost::tuple<double, double>  tup;
        typedef std::vector<tup> vector_type;
        vector_type vec;

        // Create in clockwise order
        make_comb(vec, ccw_pushback<vector_type>, count, close, true);

        // Add in reverse
        // (For GCC 3.4 have it const)
        vector_type const& v = vec;
        for (vector_type::const_reverse_iterator
            it = v.rbegin(); it != v.rend(); ++it)
        {
            functor(polygon, it->get<0>(), it->get<1>(), n++);
        }
        return;
    }

    // Create comb
    functor(polygon, 25.0, 0.0, n++);
    functor(polygon, 0.0, 25.0, n++);
    functor(polygon, 25.0, 50.0, n++);

    // Function parameters
    double diff = (25.0 / (count - 0.5)) / 2.0;

    double b1 = -25.0;
    double b2 = 25.0 - diff * 2.0;

    double x1 = 50.0, x2 = 25.0;

    for (int i = 0; i < count - 1; i++)
    {
        functor(polygon, x1, (x1 + b1), n++); x1 -= diff;
        functor(polygon, x1, (x1 + b1), n++); x1 -= diff;
        functor(polygon, x2, (x2 + b2), n++); x2 -= diff;
        functor(polygon, x2, (x2 + b2), n++); x2 -= diff;
    }
    functor(polygon, x1, (x1 + b1), n++);

    if (close)
    {
        functor(polygon, 25.0, 0.0, 4);
    }
}

#endif // BOOST_GEOMETRY_STAR_COMB_HPP