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

#include <string>
#include <climits>

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

namespace perception_oru
{

/** Main implemented method for now. Loads a single point cloud into an NDTOccupancyMap.
 */
template<typename PointT>
void NDTOccupancyMap<PointT>::loadPointCloud(const Eigen::Vector3d &origin, const pcl::PointCloud<PointT> &pc)
{
    if(index_ != NULL)
    {
        //std::cout<<"CLONE INDEX\n";
        SpatialIndex<PointT> *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;
    }

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

    double maxDist = 0;//, distCeil = 200;

    typename pcl::PointCloud<PointT>::const_iterator it = pc.points.begin();
    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;
        centroid += d;
        it++;
        npts++;
    }

    centroid /= (double)npts;

    //Eigen::Vector4f centroid(0,0,0,0);
    //pcl::compute3DCentroid(pc,centroid);

    //compute distance to furthest point
    it = pc.points.begin();
    double maxz=0, minz=10000;

    while(it!=pc.points.end())
    {
        Eigen::Vector3d d;
        if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
        {
            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++;
    }
    fprintf(stderr,"minz=%lf maxz=%lf diff=%lf",minz,maxz,maxz-minz);

    // cout<<"Points = " <<pc.points.size()<<" maxDist = "<<maxDist<<endl;

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

    index_->setCenter(centroid(0),centroid(1),centroid(2));
    index_->setSize(3*maxDist,3*maxDist,2*(maxz-minz));

    /*
    		LazyGrid<PointT> *gr = dynamic_cast<LazyGrid<PointT>*>(index_);
    		if(gr!=NULL) {
    			fprintf(stderr,"Lazy Grid initialization!!\n");
    			gr->initializeAll();
    			fprintf(stderr,"Done!");
    		}else{
    			fprintf(stderr,"ERROR: This implementation is only for the Lazy Grid - Sorry!");
    			exit(1);
    		}
    */

//    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())
    {
        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();
        unsigned int N = l / resolution;

        diff = diff/(float)N;
        PointT pt;
        for(unsigned int i=0; i<N; 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);
            //fprintf(stderr,"Requesting point for (%.3f %.3f %.3f) target = (%.3f %.3f %.3f).... ",pt.x,pt.y,pt.z,it->x, it->y, it->z );
            NDTCell<PointT> *ptCell = dynamic_cast<NDTCell<PointT> *> (index_->getCellForPoint(pt));
            //fprintf(stderr,"Done\n");
            if(ptCell != NULL)
            {
                //fprintf(stderr,"Updating point for (%.3f %.3f %.3f) target = (%.3f %.3f %.3f).... ",pt.x,pt.y,pt.z,it->x, it->y, it->z );
                ptCell->updateEmpty();
            }
            else
            {
                //fprintf(stderr,"The cell is NULL\n");
            }
        }
        //pt.x = it->x; pt.y = it->y; pt.z = it->z;
        //NDTCell<PointT> *ptCell = dynamic_cast<NDTCell<PointT> *> (index_->getCellForPoint(pt));
        //ptCell->updateOccupancy(0.5);

        index_->addPoint(*it);
        it++;
    }

    isFirstLoad_ = false;
}

/**
* Adds a new cloud - NOTE::highly Experimental
*
*/
template<typename PointT>
void NDTOccupancyMap<PointT>::addPointCloud(const Eigen::Vector3d &origin, const pcl::PointCloud<PointT> &pc)
{
    if(isFirstLoad_)
    {
        loadPointCloud(origin, pc);
    }

    if(index_ == NULL)
    {
        //ERR("Problem creating index, unimplemented method\n");
        return;
    }
    typename pcl::PointCloud<PointT>::const_iterator it = pc.points.begin();
    it = pc.points.begin();
    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();
        unsigned int N = l / (resolution);

        diff = diff/(float)N;
        PointT pt;
        for(unsigned int i=0; i<N; 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);
            NDTCell<PointT> *ptCell = dynamic_cast<NDTCell<PointT> *> (index_->getCellForPoint(pt));

            if(ptCell != NULL)
            {
                //fprintf(stderr,"Updating point for (%.3f %.3f %.3f) target = (%.3f %.3f %.3f).... ",pt.x,pt.y,pt.z,it->x, it->y, it->z );
                ptCell->updateEmpty();
            }
            else
            {
                //fprintf(stderr,"The cell is NULL\n");
            }
        }

        index_->addPoint(*it);
        it++;
    }
    isFirstLoad_ = false;
}

template<typename PointT>
void NDTOccupancyMap<PointT>::loadPointCloud(const pcl::PointCloud<PointT> &pc, const std::vector<std::vector<size_t> > &indices)
{

    loadPointCloud(pc);
    // Specific function related to CellVector
    CellVector<PointT> *cl = dynamic_cast<CellVector<PointT>*>(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_.");
    }
}

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

template<typename PointT>
pcl::PointCloud<PointT> NDTOccupancyMap<PointT>::loadDepthImageFeatures(const cv::Mat& depthImage, std::vector<cv::KeyPoint> &keypoints,
        size_t &supportSize, double maxVar, DepthCamera<PointT> &cameraParams, bool estimateParamsDI, bool nonMean)
{
    std::vector<cv::KeyPoint> good_keypoints;
    Eigen::Vector3d mean;
    PointT mn;
    pcl::PointCloud<PointT> cloudOut;
    CellVector<PointT> *cl = dynamic_cast<CellVector<PointT>*>(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<PointT> points;
            PointT center;
            cameraParams.computePointsAtIndex(depthImage,keypoints[i],supportSize,points,center);
            NDTCell<PointT> *ndcell = new NDTCell<PointT>();
            typename pcl::PointCloud<PointT>::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<PointT> *ndcell = new NDTCell<PointT>();
            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;
}

/** Helper function, computes the  NDTCells
  */
template<typename PointT>
void NDTOccupancyMap<PointT>::computeNDTCells(int cellupdatemode)
{
    CellVector<PointT> *cv = dynamic_cast<CellVector<PointT>*>(index_);

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

    while (it != index_->end())
    {
        NDTCell<PointT> *cell = dynamic_cast<NDTCell<PointT>*> (*it);
        //fprintf(stderr,"*");
        if(cell!=NULL)
        {
            //fprintf(stderr,"Something is not null!\n");
            cell->computeGaussian(cellupdatemode);
            if (cv!=NULL)
            {
                // Set the mean to the cell's centre.
                Eigen::Vector3d mean = cell->getMean();
                //cout << "mean : " << mean << std::endl;
                PointT pt;
                pt.x = mean[0];
                pt.y = mean[1];
                pt.z = mean[2];

                cell->setCenter(pt);
            }
        }
        else
        {
            //ERR("problem casting cell to NDT!\n");
        }
        it++;
    }

    LazyGrid<PointT> *lz = dynamic_cast<LazyGrid<PointT>*>(index_);
    if(lz!=NULL)
    {
        lz->initKDTree();
    }

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

/** output method, saves as vrml the oct tree and all the ellipsoids
 */
template<typename PointT>
void NDTOccupancyMap<PointT>::writeToVRML(const char* filename)
{

    if(filename == NULL)
    {
        //ERR("problem outputing to vrml\n");
        return;
    }
    /*
       std::string fn(filename);
       fn = "oct_"+fn;

       FILE *fo = fopen(fn.c_str(),"w");
       if(fo == NULL) {
       ERR("problem outputing to vrml\n");
       return;
       }
       fprintf(fo,"#VRML V2.0 utf8\n");
       writeToVRML(fo,true);
       fclose(fo);
     */

    FILE *fout = fopen(filename,"w");
    if(fout == NULL)
    {
        //ERR("problem outputing to vrml\n");
        return;
    }
    fprintf(fout,"#VRML V2.0 utf8\n");
    writeToVRML(fout);
    fclose(fout);
}

/** helper output method
 */
template<typename PointT>
void NDTOccupancyMap<PointT>::writeToVRML(FILE* fout)
{
    if(fout == NULL)
    {
        //ERR("problem outputing to vrml\n");
        return;
    }

    //move the ellipsoid stuff to NDTCell
    typename SpatialIndex<PointT>::CellVectorItr it = index_->begin();
    while (it != index_->end())
    {
        NDTCell<PointT> *cell = dynamic_cast<NDTCell<PointT>*> (*it);
        //	double xs,ys,zs;
        if(cell!=NULL)
        {
            cell->writeToVRML(fout);
        }
        else
        {
            //	    ERR("problem casting cell to NDT!\n");
        }
        it++;
    }

}
template<typename PointT>
void NDTOccupancyMap<PointT>::writeToVRML(FILE* fout, Eigen::Vector3d col)
{
    if(fout == NULL)
    {
        //ERR("problem outputing to vrml\n");
        return;
    }

    //move the ellipsoid stuff to NDTCell
    typename SpatialIndex<PointT>::CellVectorItr it = index_->begin();
    while (it != index_->end())
    {
        NDTCell<PointT> *cell = dynamic_cast<NDTCell<PointT>*> (*it);
        if(cell!=NULL)
        {
            cell->writeToVRML(fout,col);
        }
        else
        {
        }
        it++;
    }
}

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

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

//computes the *negative log likelihood* of a single observation
template<typename PointT>
double NDTOccupancyMap<PointT>::getLikelihoodForPoint(PointT pt)
{
    //assert(false);
    double uniform=0.00100;
    NDTCell<PointT>* ndCell = NULL;
    OctTree<PointT>* tr = dynamic_cast<OctTree<PointT>*>(index_);

    if(tr==NULL)
    {
        LazyGrid<PointT> *gr = dynamic_cast<LazyGrid<PointT>*>(index_);
        if(gr==NULL)
        {
            //cout<<"bad index - getLikelihoodForPoint\n";
            return uniform;
        }
        ndCell = gr->getClosestNDTCell(pt);
    }
    else
    {
        ndCell = tr->getClosestNDTCell(pt);
    }
    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 NDTOccupancyMap<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;
}
*/

template<typename PointT>
std::vector<NDTCell<PointT>*> NDTOccupancyMap<PointT>::getCellsForPoint(const PointT pt, double radius)
{
    //assert(false);
    std::vector<NDTCell<PointT>*> cells;
    OctTree<PointT>* tr = dynamic_cast<OctTree<PointT>*>(index_);
    if(tr==NULL)
    {
        LazyGrid<PointT> *gr = dynamic_cast<LazyGrid<PointT>*>(index_);
        if(gr==NULL)
        {
            //cout<<"bad index - getCellsForPoint\n";
            return cells;
        }
        cells = gr->getClosestNDTCells(pt,radius);
        return cells;
    }
    //TODO:implement for ocTree
    return cells;
}

template<typename PointT>
bool NDTOccupancyMap<PointT>::getCellForPoint(const PointT &pt, NDTCell<PointT> *&out_cell)
{

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

    LazyGrid<PointT> *gr = dynamic_cast<LazyGrid<PointT>*>(index_);
    if(gr!=NULL)
    {
        //fprintf(stderr,"Lazy!!\n");
        out_cell = gr->getClosestNDTCell(pt);
        return true;
    }
    //cout<<"bad index - getCellForPoint\n";
    return false;

    /*
    if(c==NULL) {
    //we have to find the closest leaf cell
    c = tr->getClosestLeafCell(pt);
    }
    if(c==NULL) {
    cout<<"null cell\n";
    return false;
    }
    NDTCell* ndCell = dynamic_cast<NDTCell*>(c);
    if(ndCell == NULL) {
    cout<<"not ndt cell\n";
    return false;
    }
    if(!ndCell->hasGaussian) {
    Eigen::Vector3d dim;
    ndCell->getDimensions(dim(0),dim(1),dim(2));
    typename SpatialIndex<PointT>::CellVectorItr neigh;
    index->getNeighbors(pt,20*dim.norm(),neigh);
    double minDist = INT_MAX;
    cout<<"neighs "<<neigh.size()<<endl;
    for(int i=0; i<neigh.size(); i++) {
        NDTCell *n = dynamic_cast<NDTCell*>(neigh[i]);
        if(n==NULL) continue;
        if(n->hasGaussian) {
    	double d = lslgeneric::geomDist(n->getCenter(),pt);
    	if(d<minDist) {
    	    ndCell = n;
    	}
        }
    }
    if(!ndCell->hasGaussian) {
        cout<<"no gaussian\n";
        return false;
    }
    }
    out_cell = ndCell;
    return true;
    */
}

template<typename PointT>
void NDTOccupancyMap<PointT>::debugToVRML(const char* fname, pcl::PointCloud<PointT> &pc)
{

    FILE* fout = fopen(fname, "w");

    fprintf(fout,"#VRML V2.0 utf8\n");
    this->writeToVRML(fout);
    perception_oru::writeToVRML(fout,pc,Eigen::Vector3d(1,0,0));

    fprintf(fout,"Shape {\n\tgeometry IndexedLineSet {\n\tcoord Coordinate {\n\t point [\n\t");

    int n_lines = 0;
    PointT centerCell;
    for(size_t i=0; i<pc.points.size(); i++)
    {
        NDTCell<PointT>* link;
        if(this->getCellForPoint(pc.points[i], link))
        {
            if(link == NULL) continue;
            centerCell = link->getCenter();
            if(link->hasGaussian_)
            {
                centerCell.x = link->getMean()(0);
                centerCell.y = link->getMean()(1);
                centerCell.z = link->getMean()(2);
            }
            fprintf(fout,"%lf %lf %lf\n\t%lf %lf %lf\n\t",
                    pc.points[i].x, pc.points[i].y,pc.points[i].z,
                    centerCell.x, centerCell.y, centerCell.z);
            n_lines++;
        }
    }

    fprintf(fout, "]\n\t}\n\tcoordIndex [\n\t");
    for(int i = 0; i<n_lines; i++)
    {
        fprintf(fout,"%d, %d, -1\n\t",2*i, 2*i+1);
    }
    fprintf(fout, "]\n}\n}\n");

    fclose(fout);
}

template<typename PointT>
std::vector<NDTCell<PointT>*> NDTOccupancyMap<PointT>::pseudoTransformNDT(Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T)
{

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

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

/**
* Returns dynamic cells from the map
 */
template<typename PointT>
std::vector<perception_oru::NDTCell<PointT>*>  NDTOccupancyMap<PointT>::getDynamicCells(unsigned int Timescale, float threshold)
{
    std::vector<NDTCell<PointT>*> ret;
    typename SpatialIndex<PointT>::CellVectorItr it = index_->begin();
    while (it != index_->end())
    {
        NDTCell<PointT> *cell = dynamic_cast<NDTCell<PointT>*> (*it);
        if(cell!=NULL)
        {

            if(cell->getDynamicLikelihood(Timescale)>threshold)
            {
                NDTCell<PointT>* nd = (NDTCell<PointT>*)cell->copy();
                ret.push_back(nd);
            }
        }
        else
        {
        }
        it++;
    }
    return ret;
}

template<typename PointT>
//std::vector<lslgeneric::NDTCell<pcl::PointXYZ>*> NDTOccupancyMap<PointT>::getAllCells(){
std::vector<perception_oru::NDTCell<PointT>*> NDTOccupancyMap<PointT>::getAllCells()
{

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

template<typename PointT>
int NDTOccupancyMap<PointT>::numberOfActiveCells()
{
    int ret = 0;
    if(index_ == NULL) return ret;
    typename SpatialIndex<PointT>::CellVectorItr it = index_->begin();
    while (it != index_->end())
    {
        NDTCell<PointT> *cell = dynamic_cast<NDTCell<PointT>*> (*it);
        if(cell!=NULL)
        {
            if(cell->hasGaussian_)
            {
                ret++;
            }
        }
        it++;
    }
    return ret;
}

}


#include "route_solver/log.hpp"
#include "route_solver/structs/data_structs.hpp"
#include "route_solver/solve/winding_number.hpp"

#include <boost/format.hpp>
#include <boost/lexical_cast.hpp>

#include <algorithm>

namespace rs {

using boost::lexical_cast;
using boost::format;

bool isPointInZone(const point2d& pt, const std::vector<point2d>& workzone)
{
	return wn_PnPoly(pt, workzone) != 0;
}

namespace {

bool isPointInCirle(const point2d& pt, const point2d& c, double r)
{
	double x1 = pt.x();
	double y1 = pt.y();
	double x2 = c.x();
	double y2 = c.y();
	double len = (double) sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
	return len < r;
}

}

bool Zone::isTaskInZone(const Task& task) const
{
	bool first_point = false;
	bool second_point = false;

	if (m_zoneType == Zone::Circle)
	{
		first_point = isPointInCirle(task.getStart().point, *m_circleCenter, m_circleRadius);
		second_point = isPointInCirle(task.getEnd().point, *m_circleCenter, m_circleRadius);
	}
	else if (m_zoneType == Zone::Polygon)
	{
		first_point = isPointInZone(task.getStart().point, m_points);
		second_point = isPointInZone(task.getEnd().point, m_points);
	}
	else
	{
		BOOST_THROW_EXCEPTION(std::runtime_error("Unknow zone type"));
	}

	return first_point && second_point;
}

std::uint32_t Zone::getExternalId() const
{
	return m_externalId;
}

void Zone::setExternalId(const std::uint32_t& externalId)
{
	m_externalId = externalId;
}

void Zone::setPolygonPoints(std::vector<point2d>&& points)
{
	m_points = std::move(points);
	m_zoneType = Zone::Polygon;
}

void Zone::setCircle(double circleRadius, const point2d& circleCenter)
{
	m_circleCenter = circleCenter;
	m_circleRadius = circleRadius;
	m_zoneType = Zone::Circle;
}

Zone::Zone(std::uint32_t id)
	: m_id(id)
	, m_externalId(0)
	, m_circleRadius(0)
	, m_zoneType(ZoneType::Circle)
{
}

std::uint32_t Zone::getId() const
{
	return m_id;
}

Zone::ZoneType Zone::getZoneType() const
{
	return m_zoneType;
}

const std::vector<point2d>& Zone::getPoints() const
{
	return m_points;
}

Vehicle::Vehicle(std::uint32_t id)
	: m_id(id)
	, m_externalId(0)
	, m_tripMaxTasks(std::numeric_limits<std::uint32_t>::max())
	, m_tripDuration(std::numeric_limits<double>::max())
	, m_tripLength(std::numeric_limits<double>::max())
	, m_capacity(std::numeric_limits<float>::max())
	, m_length(std::numeric_limits<float>::max())
	, m_height(std::numeric_limits<float>::max())
	, m_width(std::numeric_limits<float>::max())
	, m_costOfUsing(1.0)
	, m_costPerKM(false)
{}

std::uint32_t Vehicle::getId() const
{
	return m_id;
}

float Vehicle::getCapacity() const
{
	return m_capacity;
}

float Vehicle::getLength() const
{
	return m_length;
}

float Vehicle::getHeight() const
{
	return m_height;
}

float Vehicle::getWidth() const
{
	return m_width;
}

float Vehicle::getMaxVolume() const
{
	return m_width * m_height * m_length;
}

float Vehicle::getTripDuration() const
{
	return m_tripDuration;
}

float Vehicle::getTripLength() const
{
	return m_tripLength;
}

const std::vector<uint32_t>& Vehicle::getWorkZonesExternalIds() const
{
	return m_workZones;
}

float Vehicle::getCostOfUsing() const
{
	return m_costOfUsing;
}

std::uint16_t Vehicle::getTripMaxTasks() const
{
	return m_tripMaxTasks;
}

const std::vector<std::uint32_t>& Vehicle::getRestrictedZonesExternalIds() const
{
	return m_restrictedZones;
}

Task::Task(uint32_t id)
	: m_id(id)
	, m_externalId(0)
	, m_vip(false)
	, m_capacity(0)
	, m_volume(0)
	, m_maxsize(0)
{
}

std::uint32_t Task::getId() const
{
	return m_id;
}

float Task::getCapacity() const
{
	return m_capacity;
}

float Task::getVolume() const
{
	return m_volume;
}

float Task::getMaxSize() const
{
	return m_maxsize;
}

bool Task::getVip() const
{
	return m_vip;
}

const std::vector<uint16_t>& Task::getTypes() const
{
	return m_types;
}

const TaskPoint& Task::getStart() const
{
	return m_start;
}

const TaskPoint& Task::getEnd() const
{
	return m_end;
}

std::uint32_t Task::getExternalId() const
{
	return m_externalId;
}

void Task::setExternalId(const std::uint32_t& externalId)
{
	m_externalId = externalId;
}

void Task::setVip(bool vip)
{
	m_vip = vip;
}

void Task::setCapacity(float capacity)
{
	m_capacity = capacity;
}

void Task::setVolume(float volume)
{
	m_volume = volume;
}

void Task::setMaxsize(float maxsize)
{
	m_maxsize = maxsize;
}

void Task::setStart(TaskPoint&& start)
{
	m_start = std::move(start);
}

void Task::setEnd(TaskPoint&& end)
{
	m_end = std::move(end);
}

void Task::setTypes(std::vector<std::uint16_t>&& types)
{
	m_types = std::move(types);
}

//TODO: chcek this https://www.codeproject.com/questions/626899/converting-latitude-and-longitude-to-an-x-y-coordi

/*
R = 6371;
from https://www.codeproject.com/questions/626899/converting-latitude-and-longitude-to-an-x-y-coordi
x = R * cos(lat) * cos(lon)
y = R * cos(lat) * sin(lon)
z = R *sin(lat)
*/

point2d::point2d(double lat, double lon)
	: lat(lat), lon(lon)
{
}

double point2d::x() const
{
	return lon;
}

double point2d::y() const
{
	return lat;
}

bool rs::Vehicle::canTakeTask(const rs::Task& task) const
{
	for (const auto& t : task.getTypes())
	{
		if (!std::binary_search(m_types.begin(), m_types.end(), t))
		{
			return false;
		}
	}

	if (m_capacity < task.getCapacity())
	{
		return false;
	}

	if (m_width * m_length * m_height < task.getVolume())
	{
		return false;
	}

	if (m_width < task.getMaxSize() && m_height < task.getMaxSize())
	{
		return false;
	}

	return true;
}

const std::vector<uint16_t>& Vehicle::getTypes() const
{
	return m_types;
}

std::uint32_t Vehicle::getExternalId() const
{
	return m_externalId;
}

bool Vehicle::costPerKM() const
{
	return m_costPerKM;
}

void Vehicle::setExternalId(uint32_t externalId)
{
	m_externalId = externalId;
}

void Vehicle::setId(uint32_t id)
{
	m_id = id;
}

void Vehicle::setCapacity(float capacity)
{
	m_capacity = capacity;
}

void Vehicle::setLength(float length)
{
	m_length = length;
}

void Vehicle::setHeight(float height)
{
	m_height = height;
}

void Vehicle::setWidth(float width)
{
	m_width = width;
}

void Vehicle::setTripDuration(double tripDuration)
{
	m_tripDuration = tripDuration;
}

void Vehicle::setTripLength(double tripLength)
{
	m_tripLength = tripLength;
}

void Vehicle::setTypes(const std::vector<uint16_t>& types)
{
	m_types = types;
}

std::vector<std::uint32_t> Vehicle::getWorkZones() const
{
	return m_workZones;
}

void Vehicle::setWorkZones(std::vector<std::uint32_t>&& workZones)
{
	m_workZones = std::move(workZones);
}

std::vector<std::uint32_t> Vehicle::getRestrictedZones() const
{
	return m_restrictedZones;
}

void Vehicle::setRestrictedZones(std::vector<std::uint32_t>&& restrictedZones)
{
	m_restrictedZones = std::move(restrictedZones);
}

void Vehicle::setCostOfUsing(float costOfUsing)
{
	m_costOfUsing = costOfUsing;
}

bool Vehicle::getCostPerKM() const
{
	return m_costPerKM;
}

void Vehicle::setCostPerKM(bool costPerKM)
{
	m_costPerKM = costPerKM;
}

void Vehicle::setTripMaxTasks(uint32_t tripMaxTasks)
{
	m_tripMaxTasks = tripMaxTasks;
}

TaskPoint::TaskPoint(point2d&& point, std::size_t ts, std::size_t te, std::size_t dur)
	: point(std::move(point))
	, ts(ts)
	, te(te)
	, dur(dur)
{
}

}


/**
 * Copyright (c) 2009 Carnegie Mellon University.
 *     All rights reserved.
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing,
 *  software distributed under the License is distributed on an "AS
 *  IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
 *  express or implied.  See the License for the specific language
 *  governing permissions and limitations under the License.
 *
 * For more about this software visit:
 *
 *      http://www.graphlab.ml.cmu.edu
 *
 */

/*
 *  \author akyrola, akyrola@cs.cmu.edu Implementation of Co-EM
 *  Algorithm (Jones, 2005) for Named Entity Recognition.  This
 *  application reads a file of noun phrases (NPs) and contexts (CTs)
 *  and co-occurence counts between them. In addition, a list of
 *  positive and negative seeds is provided. Co-EM algorithm is run
 *  until convergence (or for a predefined number of iterations) to
 *  compute a probability of each NP and CT belonging to an entity
 *  class (such as "city", "politician", "animal"). For further
 *  information, see description in our paper:
 *  http://www.select.cs.cmu.edu/publications/scripts/papers.cgi?Low+al:uai10graphlab
 *
 *  This application uses a data format used by Tom Mitchell's (CMU)
 *  group. It should be straightforward to modify it to support your
 *  preferred format.
 *
 *  Application is unnecessary complicated because data sets are
 *  typicall large and several optimizations, which unfortunatelly
 *  worsen readability, have been implemented.
 *
 *  Thanks for Justin Betteridge for the algorithm definition and for
 *  providing test data!
 */

#include <cmath>
#include <cstdio>
#include <string>
#include <vector>
#include <map>
#include <dirent.h>

#include <boost/random.hpp>

#include <graphlab.hpp>
#include <graphlab/macros_def.hpp>

using namespace std;
using namespace graphlab;

// Last bit of flag determines vertex type
#define TYPEMASK  (1<<2)
#define NP_MASK  (0)
#define CTX_MASK  (1<<2)
#define SEEDMASK  0x1

// A dirty hack to encode negative and positive
// seed probabilities without using extra memory.
float NEGSEEDPROB = -1e-30f ;
float POSSEEDPROB = 1.000001f;

float INITIAL_NP_VALUE = 0.0f;

#define TEXT_LENGTH 64

bool ROUNDROBIN = false;

// tfidc is a modified weight formula for Co-EM (see Justin
// Betteridge's "CoEM results" page)
#define TFIDF(coocc, num_neighbors, vtype_total) (log(1+coocc)*log(vtype_total*1.0/num_neighbors))

// Vertex and Edge data definitions.
struct vertex_data {
  unsigned short flags;
  int nbcount;
  char text[TEXT_LENGTH];
  float normalizer;
  float p[200];

  vertex_data() {
    memset(p, 0, sizeof(float)*200);
    normalizer = 0.0f;
    nbcount = 0;
    flags = 0;
  }

  // TODO: save p!
  void save(graphlab::oarchive& archive) const {
    archive << flags << nbcount  << normalizer;
    serialize(archive, text, sizeof(char)*TEXT_LENGTH);
    serialize(archive, p, sizeof(float)*200);
  }

  void load(graphlab::iarchive& archive) {
    archive >> flags >> nbcount  >> normalizer;
    deserialize(archive, text, sizeof(char)*TEXT_LENGTH);
    deserialize(archive, p, sizeof(float)*200);
  }
};

struct edge_data {
  unsigned short cooccurence_count;

  void save(graphlab::oarchive& archive) const {
    archive << cooccurence_count;
  }

  void load(graphlab::iarchive& archive) {
    archive >> cooccurence_count;
  }
};

// Graph type defintiions
typedef graphlab::graph<vertex_data, edge_data> coem_graph;
typedef coem_graph graph_type;
typedef graph_type::vertex_id_type vertex_id_type;
typedef graph_type::edge_id_type edge_id_type;
typedef graph_type::edge_list_type edge_list_type;
typedef graphlab::types<graph_type> gl_types;

// Accuracy
float TARGET_PRECISION = 1e-5;

// Files
std::string npsfile;
std::string contextfile;
std::string matrixfile;
std::string seedsdir;
std::string negseedsdir;

// Category names (entity categories)
std::vector<std::string> categories;

// Subsampling (to get smaller problems)
double subsamplingRatio = 1.0;
bool prune_edges;

// Hack for speeding up
std::vector<vertex_id_type> ** vertex_lookups;
glshared_const<float> PARAM_M;
glshared_const<size_t> NUM_NPS;
glshared_const<size_t> NUM_CTX;
glshared_const<size_t> NUM_CATS;

// Forward declarations
void loadCategories();
std::map<std::string, vertex_id_type>  loadVertices(gl_types::core& core, short typemask, FILE * f);
void prepare(gl_types::core& core);
void load(gl_types::core& core);
void loadEdges(gl_types::core& core, FILE * fcont_to_nps,
               std::map<std::string, vertex_id_type>& nps_map,
               std::map<std::string, vertex_id_type>& ctx_map);
void setseeds(gl_types::core& core, std::map<std::string, vertex_id_type>& nps_map);
void loadAndConvertToBin(gl_types::core& core, std::string binfile);
void output_results(gl_types::core& core);

/// ====== UPDATE FUNCTION ======= ///
void coem_update_function(gl_types::iscope& scope,
                          gl_types::icallback& scheduler) {

  /* A optimization. Use thred-local lookup-table for faster computation. */
  std::vector<vertex_id_type> vlookup = *vertex_lookups[thread::thread_id()];
  vlookup.clear();

  /* Get shared vars. These are just constants. */
  float param_m = PARAM_M.get();
  size_t num_nps = NUM_NPS.get();
  size_t num_ctxs = NUM_CTX.get();
  size_t num_cats = NUM_CATS.get();

  /* Get vertex data */
  vertex_data& vdata =   scope.vertex_data();

  /* We use directed edges as indirected, so either this vertex has only incoming
     or outgoing vertices.  */
  bool use_outgoing = (scope.in_edge_ids().size()==0);
  edge_list_type edge_ids =
    (use_outgoing ? scope.out_edge_ids() : scope.in_edge_ids());

  if (edge_ids.size() == 0) {
    //printf("Warning : dangling node %d %s\n", scope.vertex(), vdata.text);
    return;
  }

  bool is_np = (vdata.flags & TYPEMASK) == NP_MASK;
  bool was_first_run = false;

  unsigned int vtype_total = (is_np ? num_nps : num_ctxs);
  unsigned int vtype_other_total = (is_np ? num_ctxs : num_nps);

  /* First check is my normalizer precomputed. If not, do it. */
  if (vdata.normalizer == 0.0) {
    float norm = param_m * vtype_other_total;
    foreach(edge_id_type eid, edge_ids) {
      const edge_data& edata = scope.const_edge_data(eid);
      const vertex_data& nb_vdata =
        scope.const_neighbor_vertex_data(use_outgoing ?
                                         scope.target(eid) :
                                         scope.source(eid));
      norm += TFIDF(edata.cooccurence_count, nb_vdata.nbcount, vtype_total);
    }
    vdata.normalizer = norm;
    was_first_run = true; // Used to cause update to all neighbors
                          // regardless of residual
    if (scope.vertex()%20000 == 0)
      printf("%d Computed normalizer: %lf (version: tfidf fixed)\n", scope.vertex(), norm);
  }

  /***** COMPUTE NEW VALUE *****/
  float tmp[200];  // assume max 200 cats
  assert(num_cats<200);
  for(int i=0; i<200; i++) tmp[i] = param_m;

  // Compute weighted averages of neighbors' beliefs.
  foreach(edge_id_type eid, edge_ids) {
    const edge_data& edata = scope.const_edge_data(eid);
    vertex_id_type nbvid = use_outgoing ? scope.target(eid) : scope.source(eid);
    const vertex_data& nb_vdata = scope.const_neighbor_vertex_data(nbvid);
    double tfidf = TFIDF(edata.cooccurence_count, nb_vdata.nbcount, vtype_total);

    for(unsigned int cat_id=0; cat_id<num_cats; cat_id++) {
      tmp[cat_id] += nb_vdata.p[cat_id] *  tfidf;
    }
    vlookup.push_back(nbvid);
  }
  // Normalize and write data
  float residual = 0.0;
  for(unsigned int cat_id=0; cat_id<num_cats; cat_id++) {
    tmp[cat_id] /= vdata.normalizer;

    /* I am seed for this category? */
    if (!((vdata.p[cat_id] == POSSEEDPROB) ||
          vdata.p[cat_id] == NEGSEEDPROB)) {
      residual += std::fabs(tmp[cat_id] - vdata.p[cat_id]);
      vdata.p[cat_id] = tmp[cat_id];
    }  else {
      // I am seed - do not change
      if (vdata.p[cat_id] == POSSEEDPROB) {
        assert((vdata.flags & SEEDMASK) != 0);
      }
    }
  }

  // Round robin scheduler does not use dynamic scheduling, so we can
  // skip it.
  if (!ROUNDROBIN) {
    int sz = edge_ids.size();
    // Write data to edges and schedule if threshold reached
    double randomNum = graphlab::random::rand01();
    for(int l = 0; l<sz; l++) {
      vertex_id_type nbvid = vlookup[l];
      gl_types::update_task task(nbvid, coem_update_function);
      edge_id_type eid = edge_ids[l];
      const edge_data& edata = scope.const_edge_data(eid);

      const vertex_data nb_vdata = scope.const_neighbor_vertex_data(nbvid);
      float neighbor_residual =
        (nb_vdata.normalizer == 0.0f ? 1.0 :
         residual *
         TFIDF(edata.cooccurence_count,
               vdata.nbcount, vtype_other_total) /
         nb_vdata.normalizer);
      // Stochastically schedule neighbor if change is less than
      // predetermined threshold.
      if ((neighbor_residual/TARGET_PRECISION >= randomNum) ||
          was_first_run) {
        scheduler.add_task(task, neighbor_residual);
      }
    }
  }
}

int ncats = 0;

/**
 *  MAIN FUNCTION
 */
int main(int argc,  char ** argv) {

  /**** GRAPHLAB INITIALIZATION *****/
  std::string root = "./";

  // Setup the command line parser
  graphlab::command_line_options   clopts("Run the CoEM algorithm.");

  clopts.attach_option("data_root", &root, root,
                       "Root for data.");
  clopts.attach_option("subsampling_ratio", &subsamplingRatio, subsamplingRatio,
                       "Subsampling ratio.");
  clopts.attach_option("target_precision", &TARGET_PRECISION, 0.0f,
                       "Termination threshold.");

  // Create a graphlab core
  gl_types::core core;

  if(!clopts.parse(argc, argv)) {
    std::cout << "Error in parsing input." << std::endl;
    return EXIT_FAILURE;
  }

  // Set the engine options
  core.set_engine_options(clopts);

  // TODO: do not hard-code
  npsfile     = root + "/cat_nps.txt";
  contextfile = root + "/cat_contexts.txt";
  matrixfile  = root + "/cat_pairs_cont-idx.txt";
  seedsdir    = root + "/seeds/";
  negseedsdir = root + "/seeds-neg/";

  /**** GRAPH LOADING ****/
  timer t;
  t.start();
  load(core);
  printf("Loading data took %lf secs\n", t.current_time());

  /** Hack **/
  vertex_lookups = (std::vector<vertex_id_type> **)
    malloc(sizeof(std::vector<vertex_id_type> *) * clopts.get_ncpus());
  for(unsigned int i=0; i< clopts.get_ncpus(); i++) {
    vertex_lookups[i] = new std::vector<vertex_id_type>();
    vertex_lookups[i]->reserve(1e6);
  }

  // Prepare graph, i.e do some precomputation
  prepare(core);

  /* Special handling for round_robin */
  if (clopts.get_scheduler_type() == "round_robin") {
    std::cout << "Round robin enabled!" << std::endl;
    core.add_task_to_all(coem_update_function, 1.0);
    ROUNDROBIN = true;
  } else {
    core.engine().set_task_budget(core.graph().num_vertices()*3);
    std::cout << "Set task budget: " << core.graph().num_vertices()*3 << std::endl;
  }

  // Run GraphLab!
  double runtime = core.start();
  std::cout << "Finished in " << runtime << " seconds." << std::endl;
  std::cout << "Going to output results... " << std::endl;

  // Write the results
  output_results(core);
}

// Bit dirty... who cares (maybe Joey :) ). Should use bind()...
int sort_cat_id = 0;
bool cmp(vertex_data * a, vertex_data * b) {
  return a->p[sort_cat_id] > b->p[sort_cat_id]; // Descending order
}

//
// Writes top 50 noun phrases for each entity category.
//
void output_results(gl_types::core& core) {
  int cat_id = 0;
  int n = core.graph().num_vertices();

  foreach(std::string catname, categories) {
    std::string filename = catname + "_results.txt";
    FILE * f = fopen(filename.c_str(), "w");

    std::vector<vertex_data *> v = std::vector<vertex_data *>(n);
    int k = 0;
    for(int vid=0; vid<n; vid++) {
      vertex_data& vdata = core.graph().vertex_data(vid);
      if ((vdata.flags & TYPEMASK) == NP_MASK &&
          (vdata.p[cat_id] != POSSEEDPROB)) {
        v[k++] = &core.graph().vertex_data(vid);
      }
    }
    v.resize(k);
    sort_cat_id = cat_id;
    std::cout << "Sorting for category:: " << catname << std::endl;
    sort(v.begin(), v.end(), cmp);
    // Output top
    for(int i=0; i<50; i++) {
      vertex_data vdata = *v[i];
      fprintf(f, "%d: %s  %1.5f\n", i+1, vdata.text, (float) vdata.p[cat_id]);
    }

    fclose(f);
    cat_id++;
  }
}

//
// Graph Loading functionality. Not documented!
//

void load(gl_types::core& core) {
  // Load categories
  loadCategories();

  // Check if already converted to binary
  std::string binfile = npsfile + "_graphlabbin.dat";

  if (subsamplingRatio != 1.0) {
    char s[255];
    sprintf(s, "%s_%1.2f", binfile.c_str(), subsamplingRatio);
    binfile = std::string(s);
  }
  std::cout << "Binfile: " << binfile << std::endl;

  FILE * f = fopen(binfile.c_str(), "r");
  if (f == NULL) {
    std::cout << "Going to convert to => " << binfile << std::endl;
    loadAndConvertToBin(core, binfile);
  } else {
    std::cout << "Loading from binary " << binfile << std::endl;
    core.graph().load(binfile);
  }
}

void prepare(gl_types::core& core) {
  // Set register shared data
  /* M is used for "smoothing" */
  float m = 0.01;
  PARAM_M.set(m);

  /*** Count NPs and CONTEXTs, add SEEDS as initial tasks */
  size_t n = core.graph().num_vertices();

  std::cout << "Seed mask " << SEEDMASK << std::endl;

  size_t num_contexts = 0;
  size_t num_nps = 0;
  size_t negseeds = 0, posseeds = 0;
  int ntasks = 0;
  for(size_t i = 0; i<n; i++) {
    vertex_data& vdata = core.graph().vertex_data(i);

    /* Is seed? */
    if ((vdata.flags & SEEDMASK) != 0) {
      for(unsigned int cat_id=0; cat_id<categories.size(); cat_id++) {
        if (vdata.p[cat_id] == POSSEEDPROB) {
          // 	graphlab->get_scheduler().add_task(update_task(i, coem_update_function), 1.0);
          //	ntasks++;
          posseeds++;
        }
      }

    }

    /* Count classes */
    if ((vdata.flags & TYPEMASK) == NP_MASK) {
      if (i%10000 == 0) std::cout << "NP: " << std::string(vdata.text) << std::endl;
      num_nps++;

      /* Set initial values */
      for(unsigned int cat_id=0; cat_id<categories.size(); cat_id++) {
        if (vdata.p[cat_id] == 0.0f) {
          vdata.p[cat_id] = INITIAL_NP_VALUE;
        }
        else if (vdata.p[cat_id] == NEGSEEDPROB) {
          //	graphlab->get_scheduler().add_task(update_task(i, coem_update_function), 1.0);
          //	ntasks++;
          negseeds++;
        }
      }

    } else  if ((vdata.flags & TYPEMASK) == CTX_MASK) {
      if (i%10000 == 0) std::cout << "CTX: " << std::string(vdata.text) << std::endl;
      num_contexts++;
      // Start with contexts

      if (!ROUNDROBIN) {
      	core.add_task(gl_types::update_task(i, coem_update_function), 1.0);
      }
      ntasks++;
    } else assert(false);

    /* Count neighbors */
    vdata.nbcount = std::max(core.graph().in_edge_ids(i).size(), core.graph().out_edge_ids(i).size());
    if (i%10000 == 0) std::cout << "Num neighbors: " << vdata.nbcount << std::endl;

  }
  std::cout << "Positive seeds: " << posseeds << std::endl;
  std::cout << "Negative seeds: " << negseeds << std::endl;
  std::cout << "Num of initial tasks: " << ntasks << std::endl;
  std::cout << "Contexts: " << num_contexts << " nps: " << num_nps << std::endl;

  NUM_CTX.set(num_contexts);
  NUM_NPS.set(num_nps);

  int numcats = categories.size();
  NUM_CATS.set(numcats);
}

// Removes \n from the end of line
void FIXLINE(char * s) {
  int len = strlen(s)-1;
  if(s[len] == '\n') s[len] = 0;
}

std::map<std::string, vertex_id_type>  loadVertices(gl_types::core& core, short typemask, FILE * f) {
  /* Allocation size depends on number of categories */
  int vsize = sizeof(vertex_data) + categories.size()*sizeof(float);
  map<std::string, vertex_id_type> map;
  char s[255];
  char delims[] = "\t";
  char *t = NULL;

  std::cout << "Vsize: " << vsize << " typemask:" << typemask <<  std::endl;

  while(fgets(s, 255, f) != NULL) {
    // Remove new line
    FIXLINE(s);
    t = strtok(s,delims);

    // Create vertex
    vertex_data vdata = vertex_data();
    vdata.flags = typemask;
    int len = strlen(t);
    // rather ugly.....
    memcpy(vdata.text, t, (TEXT_LENGTH < len+1 ? TEXT_LENGTH : len+1));

    // Read count (Not used for anything?)
    //     t = strtok(NULL,delims);
    //  sscanf(t, "%d", &vdata.count);
    double randomNum = graphlab::random::rand01();

    if (randomNum <= subsamplingRatio) {
      vertex_id_type vid = core.graph().add_vertex(vdata);
      if (vid%50000 == 0) printf("Vertex: %d  %s\n", vid, vdata.text);
      map.insert(make_pair(std::string(vdata.text), vid));
    }
  }
  return map;
}

void loadCategories() {
  /* Iterate files in seeds dir */
  DIR *dp;
  struct dirent *ep;
  dp = opendir (seedsdir.c_str());
  if (dp == NULL) {
    std::cout << "ERROR: cannot open directory for seeds: " << seedsdir << std::endl;
    std::cout << "Remember to set variable --data_root" << std::endl;
    assert(dp != NULL);
  }
  while(  (ep = readdir (dp))  ) {
    std::string s(ep->d_name);
    if (!((s == ".")|| (s == ".."))) {
      categories.push_back(s);
      std::cout << "Category [" << s << "]" << std::endl;
    }
  }
  closedir(dp);
}

void setseeds(gl_types::core& core, std::map<std::string, vertex_id_type>& nps_map) {
  short catid = 0;
  size_t num_of_seeds=0;
  foreach(std::string cat, categories) {
    std::cout << "Loading seeds for category: " << cat << std::endl;

    std::string seedfilename = seedsdir + "/" + cat;
    FILE * seedf = fopen(seedfilename.c_str(), "r");
    char cs[255];
    while(fgets(cs, 255, seedf) != NULL) {
      FIXLINE(cs);
      std::string s(cs);
      map<string,vertex_id_type>::iterator iter = nps_map.find(s);
      if (iter == nps_map.end()) {
        continue;
      }
      vertex_data& vdata = core.graph().vertex_data(iter->second);
      vdata.flags = vdata.flags | SEEDMASK;
      vdata.p[catid] = POSSEEDPROB;
      num_of_seeds++;
    }
    fclose(seedf);

    // Negative seeds - copypastecode, bad
    std::string negseedfilename = negseedsdir + "/" + cat;
    FILE * negseedf = fopen(negseedfilename.c_str(), "r");
    while(fgets(cs, 255, negseedf) != NULL) {
      FIXLINE(cs);
      std::string s(cs);
      map<string,vertex_id_type>::iterator iter = nps_map.find(s);
      if (iter == nps_map.end()) {
        //std::cout << "Seed not found: [" << s << "]" << std::endl;
        continue;
      }
      vertex_data& vdata = core.graph().vertex_data(iter->second);
      vdata.p[catid] = NEGSEEDPROB;
    }
    fclose(negseedf);
    catid++;
  }
  std::cout << "Num of seeds (pos): " << num_of_seeds << std::endl;
}

void loadEdges(gl_types::core& core, FILE * fcont_to_nps,
               std::map<std::string, vertex_id_type>& nps_map,
               std::map<std::string, vertex_id_type>& ctx_map) {
  size_t MAXBUF = 5*1000000;
  char  * s = (char*) malloc(MAXBUF); 	// 10 meg buffer :)
  char delims[] = "\t";
  char *t = NULL, *t2;

  std::cout << "Start to load edges... "<< std::endl;

  /* Get file size*/
  fseek(fcont_to_nps, 0L, SEEK_END);
  long sz = ftell(fcont_to_nps);
  fseek(fcont_to_nps, 0L, SEEK_SET);

  unsigned int i = 0, randj=0;
  unsigned int prunecount = 0;
  std::vector<std::string> * nplist = new std::vector<std::string>();

  while(fgets(s, MAXBUF, fcont_to_nps) != NULL) {
    if (strlen(s) >= MAXBUF-2) {
      // Too long!
      printf("WARNING Skipping too long line!! %s \n", strtok(s, delims));
      // Read while end of line reached
      while(fgets(s, MAXBUF, fcont_to_nps) != NULL) {
        // Ugly hack
        if(s[strlen(s)-1] == '\n') break;
      }
      continue;
    }
    FIXLINE(s);
    t = strtok(s, delims);

    // Truncate
    if (strlen(t) > TEXT_LENGTH) {
      t[TEXT_LENGTH+1] = 0;
    }
    if (t == NULL) {
      std::cout << "ERROR t was null!!! " << std::endl;
      continue;
    }
    std::string ctxname(t);
    map<string,vertex_id_type>::iterator iter = ctx_map.find(ctxname);
    if (iter == ctx_map.end()) {
      if (subsamplingRatio < 1.0) {
        // Not found (subsampling in effect)
        continue;
      } else {
        // Create vertex
        vertex_data vdata = vertex_data();
        vdata.flags = CTX_MASK;
        int len = strlen(ctxname.c_str());
        // rather ugly.....
        memcpy(vdata.text, ctxname.c_str(),
               (TEXT_LENGTH < len+1 ? TEXT_LENGTH : len+1));
        vertex_id_type vid = core.graph().add_vertex(vdata);
        if (vid%50000 == 0) printf("NEW Vertex: %d  %s\n", vid, vdata.text);
        ctx_map.insert(make_pair(ctxname, vid));
        iter = ctx_map.find(ctxname);
      }
    }
    vertex_id_type ctx_vid = iter->second;

    //  Each line contains the NP pairs for one pattern, with
    //  format <pattern>[\t<NP> || <NP> -#- <count>[...]]\n
    nplist->clear();
    while ((t = strtok(NULL, delims)) != NULL) {
      nplist->push_back(std::string(t));
    }

    // Parse each np entry on the line
    foreach(std::string npentry, *nplist) {
      t2 = strtok((char*) npentry.c_str(), " ");
      if (t2 == NULL) {
        std::cout << "ERROR t2 was null!!! " << std::endl;
        break;
      }
      std::string npname(t2);
      unsigned int cooccurence_count;
      while(true) {
        t2 = strtok(NULL, " ");
        std::string word(t2);
        if (word == "-#-") {
          t2 = strtok(NULL, " ");
          // This is coocc count
          sscanf(t2, "%u", &cooccurence_count);
          break;
        } else {
          npname += " " + word;
        }
      }

      if (prune_edges && cooccurence_count == 1) {
        // HACK
        if ((ctx_vid+(randj++)) % 3 == 1) {
          prunecount++;
          continue;
        }
      }

      map<string,vertex_id_type>::iterator iter = nps_map.find(npname);

      if (iter == nps_map.end()) {
        if (subsamplingRatio < 1.0) {
          continue;
        } else {
          // Create vertex
          vertex_data vdata = vertex_data();
          vdata.flags = NP_MASK;
          int len = strlen(npname.c_str());
          // rather ugly.....
          memcpy(vdata.text, npname.c_str(),
                 (TEXT_LENGTH < len+1 ? TEXT_LENGTH : len+1));
          vertex_id_type vid = core.graph().add_vertex(vdata);
          if (vid%50000 == 0) printf("NEW Vertex: %d  %s\n", vid, vdata.text);
          nps_map.insert(make_pair(npname, vid));
          iter = nps_map.find(npname);
        }
      }
      vertex_id_type np_vid = iter->second;

      // Create edge data
      edge_data edata;
      edata.cooccurence_count = cooccurence_count;

      if (i++ % 100000 == 0) {
        std::cout << "For ctx [" << ctxname
                  << "<" << ctx_vid << ">] linked to np: [" << npname <<
          "<" << np_vid << ">] with weight " << cooccurence_count << std::endl;
        long filepos = ftell(fcont_to_nps);
        printf("Edge #%d (%2.1f perc) pruned:%u\n", i,
               filepos*1.0/sz*100, prunecount);
      }

      core.graph().add_edge(ctx_vid, np_vid, edata);
    }
  }
  delete(nplist);
  free(s);
}

// Load data and convert to binary format for faster loading.
void loadAndConvertToBin(gl_types::core& core, std::string binfile) {
  /* Open files */
  FILE * fnps = fopen(npsfile.c_str(), "r");
  assert(fnps != NULL);
  FILE * fctx = fopen(contextfile.c_str(), "r");
  assert(fctx != NULL);
  FILE * fmatrix = fopen(matrixfile.c_str(), "r");
  assert(fmatrix != NULL);

  /* Load NPS */
  std::map<std::string, vertex_id_type> nps_map = loadVertices(core, NP_MASK, fnps);
  /* Load CONTEXTS */
  std::map<std::string, vertex_id_type> ctx_map = loadVertices(core, CTX_MASK, fctx);

  std::cout << "NPS map " << nps_map.size() << " entries" << std::endl;
  std::cout << "CTX map " << ctx_map.size() << " entries" << std::endl;

  /* Set seeds */
  setseeds(core, nps_map);

  /* Load edges */
  loadEdges(core, fmatrix, nps_map, ctx_map);

  std::cout << "(After) NPS map " << nps_map.size() << " entries" << std::endl;
  std::cout << "(AFter) CTX map " << ctx_map.size() << " entries" << std::endl;

  std::cout << "Saving graph to disk.... file:"<< binfile << std::endl;

  /* Save */
  core.graph().finalize();
  core.graph().save(binfile);

  fclose(fnps);
  fclose(fctx);
  fclose(fmatrix);

  exit(EXIT_SUCCESS);
}


#include "PCE.h"
#include "GeomCommonFunctions.h"
#include <Gui/Application.h>
#include <BRepBuilderAPI_GTransform.hxx>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <tchar.h>
#include <iostream>

using namespace PartDesignGui;

//WL
////////////////////////////////////////////////////////////////////////////////////////////
WLCollection::WL::WL(int index_, string style_, double x_, double y_, double z_, string u_, double kerf) :
	index(index_), style(style_), volume(x_* y_* z_)
{
	std::vector<double> xyz{ x_,y_,z_ };
	std::sort(xyz.begin(), xyz.end());

	size = Vector3d(xyz[2], xyz[1], xyz[0]);

	const boost::uuids::string_generator gen;
	uid = gen(u_);

	offset_points.emplace_back(-kerf * 0.5, -kerf * 0.5);
	offset_points.emplace_back(size[0] + kerf * 0.5, -kerf * 0.5);
	offset_points.emplace_back(size[0] + kerf * 0.5, size[1] + kerf * 0.5);
	offset_points.emplace_back(-kerf * 0.5, size[1] + kerf * 0.5);

	origin_points.emplace_back(0.0, 0.0);
	origin_points.emplace_back(size[0], 0.0);
	origin_points.emplace_back(size[0], size[1]);
	origin_points.emplace_back(0.0, size[1]);

	surf_normals.emplace_back(Vector3d(0.0, -1.0, 0.0));
	surf_normals.emplace_back(Vector3d(1.0, 0.0, 0.0));
	surf_normals.emplace_back(Vector3d(0.0, 1.0, 0.0));
	surf_normals.emplace_back(Vector3d(-1.0, 0.0, 0.0));

	Vector3d1 bound_wood{
	Vector3d(-kerf,-kerf,0.0),
	Vector3d(size[0] + kerf,-kerf,0.0),
	Vector3d(size[0] + kerf,size[1] + kerf,0.0),
	Vector3d(-kerf,size[1] + kerf,0.0)
	};

	boundary_3d.emplace_back(bound_wood);

	Vector3d1 bound_lumber{
	Vector3d(-kerf,0.0,-kerf),
	Vector3d(size[0] + kerf,0.0,-kerf),
	Vector3d(size[0] + kerf,0.0,size[2] + kerf),
	Vector3d(-kerf,0.0,size[2] + kerf)
	};
	boundary_3d.emplace_back(bound_lumber);
}

std::vector<PCEEdge> WLCollection::WL::ExtractEdges()
{
	std::vector<PCEEdge> edges_;

	edges.clear();
	for (int i = 0; i < offset_points.size(); i++)
	{
		const int index_0 = i;
		const int index_1 = (i + 1) % offset_points.size();

		const Vector2d origin_s = origin_points[index_0];
		const Vector2d origin_e = origin_points[index_1];
		const Vector2d offset_s = offset_points[index_0];
		const Vector2d offset_e = offset_points[index_1];

		PCECUT cut;
		cut.origin_s = origin_s;
		cut.origin_e = origin_e;
		cut.offset_s = offset_s;
		cut.offset_e = offset_e;
		cut.cutting_surface_normal = surf_normals[i];
		cut.u_cutting_line_s = Vector3d(offset_s[0], offset_s[1], size[2]);
		cut.u_cutting_line_e = Vector3d(offset_e[0], offset_e[1], size[2]);
		cut.l_cutting_line_s = Vector3d(offset_s[0], offset_s[1], 0.0);
		cut.l_cutting_line_e = Vector3d(offset_e[0], offset_e[1], 0.0);

		cut.l_u_vector = Vector3d(0.0, 0.0, size[2]);

		edges_.emplace_back(PCEEdge(edges_.size(), -1, cut));
		edges.emplace_back(edges_.back().index);
	}
	return edges_;
}

////////////////////////////////////////////////////////////////////////////////////////////
WLCollection::WLCollection(int index_) : index(index_)
{
}

void WLCollection::SortWLS()
{
	const auto Comp = [](WL & wl0, WL & wl1)
	{
		return wl0.volume > wl1.volume;
	};
	std::sort(wls.begin(), wls.end(), Comp);
	for (auto i = 0; i < wls.size(); i++) wls[i].index = i;
}

void WLCollection::Clear()
{
	std::vector<WL>().swap(wls);
	//std::vector<int>().swap(shapes);
	//std::vector<int>().swap(input_shapes);
}

std::vector<PCETransform> WLCollection::DetectSize
(const HMODULE& hModule,
	const double& kerf,
	const double part_match_error,
	const shared_ptr<PCEShape>& ptr_shape,
	const bool WLCFree)
{
	const auto CompTransform = [](const PCETransform & transform_0, const PCETransform & transform_1)
	{
		return transform_0.wl_rates.front().volume_rate > transform_1.wl_rates.front().volume_rate;
	};

	const auto CompWlrate = [](const PCETransform::WLRATE & wl_rate_0, const PCETransform::WLRATE & wl_rate_1)
	{
		if (wl_rate_0.perfect_matching > wl_rate_1.perfect_matching)
		{
			return true;
		}
		else
		{
			if (wl_rate_0.perfect_matching == wl_rate_1.perfect_matching)
				return wl_rate_0.volume_rate > wl_rate_1.volume_rate;
			else
				return false;
		}
	};

	auto GetRP = [](const glm::dmat4 &PM, const Vector3d2& in_surfs)
	{
		Vector3d2 r_ps;
		for (const auto surf : in_surfs)
		{
			Vector3d1 ps;
			for (const auto p : surf)
				ps.emplace_back(Vector3d(PM * glm::vec4(p,1.0)));
			r_ps.emplace_back(ps);
		}
		return r_ps;
	};

	auto GetRN = [](const glm::dmat4& RM, const Vector3d1 & normals)
	{
		Vector3d1 r_normals;
		for (const auto normal : normals)
			r_normals.emplace_back(Vector3d(RM * glm::vec4(normal, 1.0)));
		return r_normals;
	};

	auto GetDistance = [](const HMODULE & hModule_, const Vector3d2 & ps_0, const Vector3d2 & ps_1)
	{
		auto cgal_3d_distance_point_polygon = (CGAL_3D_Distance_Point_Polygon)GetProcAddress(hModule_, "CGAL_3D_Distance_Point_Polygon");

		Vector3d2 ps_0_ = ps_0;
		Vector3d2 ps_1_ = ps_1;

		double total_distance = 0.0;
		int nb = 0;
		for (auto& ps : ps_0_)
			for (auto& p : ps)
			{
				double min_dis = 100000000000.0;
				for (auto& poly : ps_1_)
					min_dis = min(min_dis, cgal_3d_distance_point_polygon(poly, p));
				total_distance += min_dis;
				nb++;
			}
		total_distance = total_distance / (double)nb;

		return total_distance;
	};

	auto OutputRectangle = [](std::string path, const PCETransform & transform, const HMODULE & hModule, const double& kerf)
	{
		auto output = (CGAL_Export_Path_Segment)GetProcAddress(hModule, "CGAL_Export_Path_Segment");
		const auto& points = transform.surfs;

		std::ofstream file(path);
		for (int i = 0; i < points.size(); i++)
			for (int j = 0; j < points[i].size(); j++)
				file << "v " << points[i][j][0] << " " << points[i][j][1] << " " << points[i][j][2] << std::endl;

		int nb = 1;
		for (int i = 0; i < points.size(); i++)
		{
			file << "f ";
			for (int j = 0; j < points[i].size(); j++)
			{
				file << Math::Functs::IntString(nb) << " ";
				nb++;
			}
			file << "" << std::endl;
		}
		file.clear();
		file.close();
	};

	std::vector<PCETransform> transforms;
	if (part_match_error < 0.0) return transforms;
	Vector3d1 rotations= Math::Functs::EmumerateRotations();

	//angle 0.0
	for (const auto& rotation : rotations)
	{
		glm::dmat4 PM =
			Math::Functs::TranslationMatrix(ptr_shape->center) *
			Math::Functs::RotationMatrix(Vector3d(0, 0, 1), -rotation[2]) *
			Math::Functs::RotationMatrix(Vector3d(0, 1, 0), -rotation[1]) *
			Math::Functs::RotationMatrix(Vector3d(1, 0, 0), -rotation[0]) *
			Math::Functs::TranslationMatrix(-ptr_shape->center);

		glm::dmat4 RM =
			Math::Functs::RotationMatrix(Vector3d(0, 0, 1), -rotation[2]) *
			Math::Functs::RotationMatrix(Vector3d(0, 1, 0), -rotation[1]) *
			Math::Functs::RotationMatrix(Vector3d(1, 0, 0), -rotation[0]);

		auto surfs = GetRP(PM, ptr_shape->surfs);
		auto surf_normals = GetRN(RM, ptr_shape->surf_normals);

		Vector3d maximal_corner, minimal_corner;
		Math::Functs::GetBoundingBox(surfs, minimal_corner, maximal_corner);
		Vector3d shape_size = maximal_corner - minimal_corner;

		for (auto& surf : surfs)
			for (auto& p : surf)
				p = p - minimal_corner;

		PM = Math::Functs::TranslationMatrix(-minimal_corner) * PM;

		std::vector<PCETransform::WLRATE> wl_rates;

		for (auto& wl : wls)
		{
			double ratio = -1.0;
			int perfect_matching = 0;

			if (WLCFree)
			{
				ratio = shape_size[0] * shape_size[1] * shape_size[2] / wl.volume;

				if (Math::Functs::IsAlmostZero_Double(wl.size[0] - shape_size[0], part_match_error)) perfect_matching++;
				if (Math::Functs::IsAlmostZero_Double(wl.size[1] - shape_size[1], part_match_error)) perfect_matching++;
				if (Math::Functs::IsAlmostZero_Double(wl.size[2] - shape_size[2], part_match_error)) perfect_matching++;
			}
			else
			{
				if (wl.size[0] + part_match_error >= shape_size[0] && wl.size[1] + part_match_error >= shape_size[1] && wl.size[2] + part_match_error >= shape_size[2])
				{
					ratio = shape_size[0] * shape_size[1] * shape_size[2] / wl.volume;

					if (Math::Functs::IsAlmostZero_Double(wl.size[0] - shape_size[0], part_match_error)) perfect_matching++;
					if (Math::Functs::IsAlmostZero_Double(wl.size[1] - shape_size[1], part_match_error)) perfect_matching++;
					if (Math::Functs::IsAlmostZero_Double(wl.size[2] - shape_size[2], part_match_error)) perfect_matching++;
					else
						ratio = -1.0;
				}
			}

			if (ratio >= 0)
			{
				bool b = false;

				for (auto& trf : transforms)
				{
					if (GetDistance(hModule, surfs, trf.surfs) < part_match_error)
					{
						b = true;
						if (!trf.CheckWL(wl.index))
							trf.wl_rates.emplace_back(PCETransform::WLRATE(wl,ratio, perfect_matching));
						break;
					}
				}

				if (!b)
				{
					transforms.emplace_back(PCETransform(surfs, surf_normals, ptr_shape->center, rotation, PM, RM, minimal_corner));
					transforms.back().wl_rates.emplace_back(PCETransform::WLRATE(wl,ratio, perfect_matching));
				}

			}
		}
	}

	for (auto& transform : transforms) std::sort(transform.wl_rates.begin(), transform.wl_rates.end(), CompWlrate);

	std::sort(transforms.begin(), transforms.end(), CompTransform);

	for (int iter = 0; iter < transforms.size(); iter++)
		transforms[iter].index = iter;

	std::vector<PCETransform> return_transforms;
	for (auto& transform : transforms)
	{
		transform.BuildBoundingBox(kerf, hModule);
		if (transform.BuildEnd(kerf, hModule))
			return_transforms.emplace_back(transform);
	}

	for (int i = 0; i < return_transforms.size(); i++)
	{
		return_transforms[i].index = i;
		return_transforms[i].pce_shape = ptr_shape;
	}

	return return_transforms;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////
//EGraphClass
//////////////////////////////////////////////////////////////////////////////////////////////////////////

EGraphClass::EGraphClass(int index_,const std::vector<int>& ec_shapes_) :
	index(index_), str(Math::Functs::IntString(ec_shapes_,true,",")), ec_shapes(ec_shapes_), head_index(0)
{
	if (ec_shapes_.empty())
	{
		std::cerr << "if (ec_shapes_.empty())" << std::endl;
		system("pause");
	}

	valid = false;
	compiledNodesB=false,
	ShapeEncode();
}

void EGraphClass::ShapeEncode()
{
	if (ec_shapes.empty())
	{
		std::cerr << "if (ec_shapes.empty())" << std::endl;
		system("pause");
	}
	encode_shapes=PCEEncode::GetEncode(ec_shapes);
};

void EGraphClass::PushComp(std::vector<int> comp_)
{
	if (!(comp_.size() == 1 && comp_[0] == index))
	{
		string str_ = Math::Functs::IntString(comp_,true,",");

		for (auto edges : egraph_edges)
		{
			string str = Math::Functs::IntString(edges,true,",");
			if (str_ == str)return;
		}
		egraph_edges.emplace_back(comp_);
	}
}

void EGraphClass::Pruning(std::vector<shared_ptr<PCEArrange>>& egraph_arranges,
	const int& maximal_number, const bool& random_pruning)
{
	if (arranges.empty())return;

	std::vector<std::tuple<int, int, double>> cns;
	for (int i = 0; i < arranges.size(); i++)
	{
		const auto& arrange = egraph_arranges[arranges[i]];
		cns.emplace_back(arranges[i], arrange->cutting_number, arrange->volume_rate);
	}

	if (random_pruning)
		std::random_shuffle(cns.begin(), cns.end());
	else
	{
		std::sort(cns.begin(), cns.end(), [](const std::tuple<int, int, double>& cn0, const std::tuple<int, int, double>& cn1)
			{return std::get<1>(cn0) < std::get<1>(cn1) || (std::get<1>(cn0) == std::get<1>(cn1) && std::get<2>(cn0) > std::get<2>(cn1));});
	}

	std::vector<int>().swap(arranges);

	for (int i = 0; i < maximal_number && i < cns.size() && std::get<1>(cns[i]) == std::get<1>(cns.front()); i++)
	{
		int arrange_index = std::get<0>(cns[i]);

		const auto& arrange = egraph_arranges[arrange_index];
		bool b = true;
		for (auto& index_ : arranges)
		{
			const auto& arrange_ = egraph_arranges[index_];
			if (arrange->encode_packing_0 == arrange_->encode_packing_0 ||
				arrange->encode_packing_0 == arrange_->encode_packing_1 ||
				arrange->encode_packing_1 == arrange_->encode_packing_0 ||
				arrange->encode_packing_1 == arrange_->encode_packing_1)
			{
				b = false;
				break;
			}
		}
		if (b) arranges.emplace_back(arrange_index);
	}
}

void EGraphClass::Pruning(std::vector<PCEArrange> & egraph_arranges,
	const int& maximal_number, const bool& random_pruning)
{
	struct CN
	{
	public:
		int index;
		int cutting_number;
		double volume_rate;
		CN()
		{
			index = -1;
			cutting_number = -1;
		}
		CN(int index_, int cutting_number_, double volume_rate_)
		{
			index = index_;
			cutting_number = cutting_number_;
			volume_rate = volume_rate_;
		}
	};

	auto Comp = [](const CN & cn0, const CN & cn1)
	{
		if (cn0.cutting_number < cn1.cutting_number)
			return true;
		else
			if (cn0.cutting_number == cn1.cutting_number && cn0.volume_rate > cn1.volume_rate)
				return true;
		return false;
	};

	std::vector<CN> cns;
	for (int i = 0; i < arranges.size(); i++)
	{
		const auto& arrange = egraph_arranges[arranges[i]];
		cns.emplace_back(CN(arranges[i], arrange.cutting_number, arrange.volume_rate));
	}

	if (random_pruning)
		std::random_shuffle(cns.begin(), cns.end());
	else
		sort(cns.begin(), cns.end(), Comp);

	std::vector<int>().swap(arranges);

	for (int i = 0; i < maximal_number && i < cns.size(); i++)
	{
		const auto& arrange = egraph_arranges[cns[i].index];
		bool b = true;
		for (auto& index_ : arranges)
		{
			const auto& arrange_ = egraph_arranges[index_];
			if (arrange.encode_packing_0 == arrange_.encode_packing_0||
				arrange.encode_packing_0 == arrange_.encode_packing_1||
				arrange.encode_packing_1 == arrange_.encode_packing_0 ||
				arrange.encode_packing_1 == arrange_.encode_packing_1)
			{
				b = false;
				break;
			}
		}
		if (b) arranges.emplace_back(cns[i].index);
	}
}

void EGraphClass::Clear()
{
	arranges.clear();
	egraph_edges.clear();
}


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

*/

#ifndef __VISION_STRUCTURE_STATIC_STUFF_FONCTORBA_HPP__
#define __VISION_STRUCTURE_STATIC_STUFF_FONCTORBA_HPP__

#include "computing.hpp"

#include <boost/mpl/for_each.hpp>
#include <boost/mpl/push_back.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/fusion/include/at_key.hpp>

#include <libv/lma/global.hpp>
#include <libv/lma/ttt/traits/wrap.hpp>
#include <libv/lma/ttt/traits/naming.hpp>
#include <libv/lma/ttt/traits/unroll1.hpp>
#include <libv/lma/ttt/traits/rm_all.hpp>
#include <libv/lma/lm/trait/accessor.hpp>

#include <boost/fusion/include/find.hpp>
#include <boost/fusion/include/is_sequence.hpp>
#include <tuple>

#include <libv/lma/lm/ba/meta_prod.hpp>
#include <libv/lma/lm/container/container.hpp>

namespace lma
{
  template<class A, class B, class Flt, class _, class I, class J> typename Table<A,B,Flt,_>::Matrix& to_block(Table<A,B,Flt,_>& table, I i, J j)
  {
    return table(i,j);
  }

  template<class A, class B, class Flt, class I, class J> typename Table<A,B,Flt,Diagonal>::Matrix& to_block(Table<A,B,Flt,Diagonal>& table, I i, J)
  {
    return table(i);
  }

  // Helping accessors
  template<class Key, class Map>
  typename br::value_at_key<Map,bf::pair<Key,Key>>::type::Matrix&
   at_h(Map& h, const ttt::Indice<Key>& indice)
  {
    return to_block(bf::at_key<bf::pair<Key,Key>>(h),indice,0);
  }

  template<class Key1, class Key2, class Map>
  typename br::value_at_key<Map,bf::pair<Key1,Key2>>::type::Matrix&
   at_h(Map& h, const ttt::Indice<Key1>& indice, const ttt::Indice<bf::pair<Key2,Key1>>& isparse)
  {
    return to_block(bf::at_key<bf::pair<Key1,Key2>>(h),indice,isparse);
  }

  template<class Key, class Map>
  typename br::value_at_key<Map,Key>::type::Matrix& at_jte(Map& jte, const ttt::Indice<Key>& indice)
  {
    return bf::at_key<Key>(jte)(indice);
  }

  // compute JtJ at block level
  template<class Result, class JA, class JB> inline void jtj(Result& result, const JA& Ja, const JB& Jb)
  {
    result += transpose(Ja) * Jb;
  }

  // compute Jte at block level
  template<class Result, class J, class E, class Tag> inline void jte(Result& result, const J& j, const E& erreur, ttt::wrap<Tag> const&)
  {
    result -= transpose(j) * make_view(erreur,ttt::wrap<Tag>());
  }

  template<size_t K, class Tie> struct JaTJb
  {
    Tie& tie;
    JaTJb(Tie& tie_):tie(tie_){}

    template<size_t I> void operator()(Int<I> const&)
    {
      auto& map_indice = bf::at_c<0>(tie);
      auto& jacob = bf::at_c<1>(tie);
      auto& bundle = bf::at_c<3>(tie);
      auto& iobs = bf::at_c<4>(tie);
      auto& hessien= bf::at_c<5>(tie);

      const auto& indice1 = bf::at_c<K>(map_indice);
      const auto& Ja = bf::at_c<K>(jacob).second;
      const auto& Jb = bf::at_c<I>(jacob).second;
      const auto& isparse = bf::at_c< I - (K + 1) >(bundle.spi2.template sparse_indices<K>(iobs));

      // sparse_indices structure with 4 paramters A B C D :
      //  ---------------
      //  |  AB  AC  AD
      //  |  BC  BD
      //  |  CD

      // sparse_indices access :
      // K\  L =   0   1   2   with L = I - (K + 1)
      //       ---------------
      // K = 0 |   AB  AC  AD
      // K = 1 |   BC  BD
      // K = 2 |   CD

      jtj(at_h(hessien,indice1,isparse),Ja,Jb);
    }
  };

  template<size_t S, class Tie, class MatrixTag> struct JtJ
  {
    Tie& tie;
    JtJ(Tie& tie_):tie(tie_){}
    template<size_t I> void operator()(Int<I> const &)
    {
      auto& map_indice   = bf::at_c<0>(tie);
      auto& jacob        = bf::at_c<1>(tie);
      auto& residu       = bf::at_c<2>(tie);
      auto& hessien      = bf::at_c<5>(tie);
      auto& residus      = bf::at_c<6>(tie);

      const auto& J      = bf::at_c<I>(jacob).second;
      const auto& indice = bf::at_c<I>(map_indice);

      jtj(at_h(hessien,indice),J,J);
      jte(at_jte(residus,indice),J,residu,ttt::wrap<MatrixTag>());
      //JaTJb (only trig sup) :
      ttt::unroll<I+1,S>(JaTJb<I,Tie>(tie));
    }
  };

  template<class H> struct AssignSame
  {
    const H& h_;
    AssignSame(const H& h__):h_(h__){}

    template<class _> void operator()(const _&) const {}

    template<class Key, class Cont, template<class,class> class Pair> void operator()(Pair<Key,Cont>& obj) const
      {
        auto& h = bf::at_key<Key>(h_);
        auto& s = obj.second;

        _assign_(s,h);
      }

    template<class S_, class H_>
    void _assign_(S_& s,const H_& h) const
    {
//       std::cout << " assign table table : " << s.name() << std::endl;
      if (s.size()==0)
      {
        s = h;
      }
      else
      {
        for(auto i = h.first() ; i < h.size() ; ++i)
          for(auto j = h.first(i) ; j < h.size(i) ; ++j)
          {
            s(i,j) = h(i,j);
          }
      }
    }

    template<class A, class Flt>
    void _assign_(Table<A,A,Flt,Symetric>& s,const Table<A,A,Flt,Diagonal>& h) const
    {
//       std::cout << " assign symetric diagonal : " << s.name() << std::endl;
      for(auto i = h.first() ; i < h.size() ; ++i)
        s(i,0) = h(i);
    }

    template<class A, class Flt>
    void _assign_(Table<A,A,Flt,Symetric>& s,const Table<A,A,Flt,Symetric>& h) const
    {
//       std::cout << " assign symetric symetric : " << s.name() << std::endl;
      for(auto i = h.first() ; i < h.size() ; ++i)
        for(auto j = h.first(i) ; j < h.size(i) ; ++j)
//           if (i <= s.indice(i,j))
        {
          s(i,s.indice.get(i,h.indice(i,j))) = h(i,j);
        }
    }

    template<class A, class Flt>
    void _assign_(Table<A,A,Flt,Diagonal>& s,const Table<A,A,Flt,Diagonal>& h) const
    {
//       std::cout << " assign diagonal diagonal : " << s.name() << std::endl;
      s.copy_mat(h);
//       s = h;
    }
  };

  struct OpAssignSame
  {
    template<class A, class B> void operator()(A& a, const B& b) const
    {
      a.copy_mat(b);
    }
  };

  struct Inv
  {
    template<class T> void operator()(T& t) const
    {
      t.inv();
    }
  };

  struct Correct
  {
    template<class Bdl, class Delta> void operator()(Bdl& ref, const Delta& refdelta) const
    {
      assert(ref.size()==refdelta.size());
      for(auto i = ref.first() ; i < ref.size() ; ++i)
      {
        detail::internal_apply_increment(ref.reference(i),get_ptr(refdelta(i)));
      }
    }
  };

  struct LambdaDiag
  {
    double lambda;
    LambdaDiag(double lambda_):lambda(lambda_) {}
    template<class T> void operator()(T& table) const
    {
      table.augment_diag(lambda);
    }
  };

  template<class Tie, class Op> struct TieOp
  {
    Tie tie;
    Op op;
    TieOp(Tie tie_, Op op_):tie(tie_),op(op_) {}

    template<class T> void operator()(ttt::wrap<T>) {}

    template<class Key1, class Key2> void operator()
    (
      ttt::wrap<Product<Key1,Key2>>,
      typename boost::disable_if<
                                  mpl::not_<
                                            br::has_key<
                                                          typename ttt::rm_all<
                                                                                typename std::tuple_element<0,Tie>::type
                                                                              >::type,
                                                          typename Product<Key1,Key2>::type
                                                        >
                                          >
                                >::type* =0// trig sup éparse de S non géré dans S = Ws * WsT
    )
    {
      op(
          bf::at_key<typename Product<Key1,Key2>::type>(std::get<0>(tie)),
          bf::at_key<typename TransToPair<Key1>::type>(std::get<1>(tie)),
          bf::at_key<typename TransToPair<Key2>::type>(std::get<2>(tie))
        );
    }

    template<class Key1> void operator()(ttt::wrap<Unary<Key1>>)
    {
      op(bf::at_key<Key1>(std::get<0>(tie)));
    }

    template<class Key1, class Key2> void operator()(ttt::wrap<Binary<Key1,Key2>>)
    {
      op(
            bf::at_key<Key1>(std::get<0>(tie)),
            bf::at_key<Key2>(std::get<1>(tie))
        );
    }
  };

  template<class Keys, class Op, class Tie> void for_each(Tie tie, Op op)
  {
    mpl::for_each<typename Keys::type,wrap_>(TieOp<Tie,Op>(tie,op));
  }

}// eon

#endif


// SNCOperators.cpp - Code for the SNC operator equations.
// See SNCOperators.hpp for details.
//
// Copyright (c) 2016 Timothy Zhu.
// Licensed under the MIT License. See LICENSE file for details.
//

#include <cassert>
#include <cmath>
#include <cstdlib>
#include <stdint.h>
#include <set>
#include <vector>
#include <limits>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include "ProcessedTrace.hpp"
#include "time.hpp"
#include "search.hpp"
#include "NC.hpp"
#include "MGF.hpp"
#include "SNCOperators.hpp"

using namespace std;

const uint64_t MMBPArrival::intervalWidth = ConvertSecondsToTime(1);

// Check if there is a dependency between bound and this. A dependency exists if both bound and this share the same flow id in their dependency sets.
bool DependencyParams::checkDependence(const DependencyParams* bound) const
{
    const set<FlowId>& A = getDependencies();
    const set<FlowId>& B = bound->getDependencies();
    set<FlowId>::const_iterator itA = A.begin();
    set<FlowId>::const_iterator itB = B.begin();
    while ((itA != A.end()) && (itB != B.end())) {
        if (*itA == *itB) {
            return true;
        } else if (*itA < *itB) {
            itA++;
        } else {
            itB++;
        }
    }
    return false;
}

// Top-level function for analyzing a trace and initializing the MMBP parameters based on the analysis.
void MMBPArrival::init(ProcessedTrace* pTrace)
{
    // Split trace into intervals of length intervalWidth
    vector<double> intervals;
    countArrivalIntervals(pTrace, intervals);
    // Assign a state to each interval using the LAMBDA algorithm
    vector<unsigned int> states;
    vector<double> lambdas;
    unsigned int numStates = determineStatesLAMBDA(intervals, states, lambdas, 2.0);
    // Initialize the transition matrix between states
    initTransitionMatrix(numStates, states);
    // Initialize MGFs for each state
    initMGFs(pTrace, numStates, states, lambdas);
}

// Count the number of arrivals in each interval in the trace pTrace and store the results in intervals.
void MMBPArrival::countArrivalIntervals(ProcessedTrace* pTrace, vector<double>& intervals) const
{
    intervals.clear();
    int count = 0;
    uint64_t nextIntervalTime = intervalWidth;
    ProcessedTraceEntry traceEntry;
    pTrace->reset();
    while (pTrace->nextEntry(traceEntry)) {
        while (traceEntry.arrivalTime >= nextIntervalTime) {
            intervals.push_back(count);
            count = 0;
            nextIntervalTime += intervalWidth;
        }
        count++;
    }
    intervals.push_back(count);
}

// Helper function for performing the LAMBDA algorithm.
double MMBPArrival::LAMBDAAlgorithm(double a, void* params)
{
    LAMBDAParams_t* p = static_cast<LAMBDAParams_t*>(params);
    double low = p->low;
    double high = p->high;
    vector<double>* pLambdas = p->pLambdas;
    double lambda = sqrt(high + a * a / 4.0) - (a / 2.0);
    lambda = lambda * lambda;
    for (unsigned int state = pLambdas->size() - 1; state > 0; state--) {
        if (lambda < low) {
            return -numeric_limits<double>::infinity();
        }
        (*pLambdas)[state] = lambda;
        lambda = sqrt(lambda) - a;
        lambda = lambda * lambda;
    }
    (*pLambdas)[0] = lambda;
    return lambda - a * sqrt(lambda);
}

// Assign a state to each interval in the trace using the LAMBDA algorithm. Returns number of states used, up to maxNumStates.
unsigned int MMBPArrival::determineStatesLAMBDA(const vector<double>& intervals, vector<unsigned int>& states, vector<double>& lambdas, double a) const
{
    // Find largest and smallest interval size
    LAMBDAParams_t params;
    double& low = params.low;
    double& high = params.high;
    params.pLambdas = &lambdas;
    low = intervals[0];
    high = intervals[0];
    for (unsigned int intervalIndex = 1; intervalIndex < intervals.size(); intervalIndex++) {
        if (intervals[intervalIndex] < low) {
            low = intervals[intervalIndex];
        }
        if (intervals[intervalIndex] > high) {
            high = intervals[intervalIndex];
        }
    }
    // Determine lambdas with LAMBDA algorithm
    unsigned int numStates = maxNumStates;
    lambdas.resize(numStates, 0);
    if (isfinite(LAMBDAAlgorithm(a, &params))) {
        // Maximum number of states used, search for best confidence interval for the max number of states to cover the range
        a = binarySearchReverse(0, high, low, 0.01, LAMBDAAlgorithm, &params);
        LAMBDAAlgorithm(a, &params);
    } else {
        // Less than the max number of states needed for a given confidence interval, remove unnecessary states
        unsigned int unusedStates = 0;
        for (unsigned int state = 0; state < lambdas.size(); state++) {
            if (lambdas[state] > 0) {
                lambdas[state - unusedStates] = lambdas[state];
            } else {
                unusedStates++;
            }
        }
        numStates -= unusedStates;
        lambdas.resize(numStates);
    }
    // Assign states to intervals
    states.resize(intervals.size(), 0);
    for (unsigned int intervalIndex = 0; intervalIndex < intervals.size(); intervalIndex++) {
        states[intervalIndex] = 0;
        for (unsigned int state = lambdas.size() - 1; state > 0; state--) {
            double lambda = lambdas[state];
            if (intervals[intervalIndex] > (lambda - a * sqrt(lambda))) {
                states[intervalIndex] = state;
                break;
            }
        }
    }
    return numStates;
}

// Initialize the transition matrix between states.
void MMBPArrival::initTransitionMatrix(unsigned int numStates, const vector<unsigned int>& states)
{
    // Initialize transition matrix size
    _transitionMatrix.resize(numStates);
    for (unsigned int state = 0; state < numStates; state++) {
        _transitionMatrix[state].resize(numStates);
    }
    // Calculate transition matrix
    vector<uint64_t> stateDurations(numStates, 0);
    unsigned int fromState = states[0];
    stateDurations[fromState] += intervalWidth;
    for (unsigned int stateIndex = 1; stateIndex < states.size(); stateIndex++) {
        unsigned int toState = states[stateIndex];
        stateDurations[toState] += intervalWidth;
        _transitionMatrix[fromState][toState] += 1;
        fromState = toState;
    }
    for (unsigned int fromState = 0; fromState < numStates; fromState++) {
        double stateSteps = floor(ConvertTimeToSeconds(stateDurations[fromState]) / stepSize);
        if (stateSteps == 0) {
            stateSteps = 1;
        }
        double probTransition = 0;
        _transitionMatrix[fromState][fromState] = 0;
        for (unsigned int toState = 0; toState < numStates; toState++) {
            _transitionMatrix[fromState][toState] /= stateSteps;
            probTransition += _transitionMatrix[fromState][toState];
        }
        _transitionMatrix[fromState][fromState] = 1.0 - probTransition;
    }
}

// Helper function for creating the MGF for a MMBP state.
MGF* MMBPArrival::createMMBPStateMGF()
{
    return new MGFExponential();
}

// Initialize each MMBP state with its associated arrival rate and request size distribution, which is represented by a moment generating function (MGF).
void MMBPArrival::initMGFs(ProcessedTrace* pTrace, unsigned int numStates, const vector<unsigned int>& states, const vector<double>& lambdas)
{
    // Initialize MGFs
    _MGFs.resize(numStates);
    for (unsigned int state = 0; state < numStates; state++) {
        _MGFs[state] = createMMBPStateMGF();
    }
    // Estimate MGFs based on trace
    uint64_t nextIntervalTime = intervalWidth;
    unsigned int stateIndex = 0;
    ProcessedTraceEntry traceEntry;
    pTrace->reset();
    while (pTrace->nextEntry(traceEntry)) {
        while (traceEntry.arrivalTime >= nextIntervalTime) {
            stateIndex++;
            nextIntervalTime += intervalWidth;
        }
        unsigned int state = states[stateIndex];
        _MGFs[state]->addSampleRequest(traceEntry);
    }
    // Set prob of generating a request for each MGF
    for (unsigned int state = 0; state < numStates; state++) {
        _MGFs[state]->setProbRequest(lambdas[state] * stepSize / ConvertTimeToSeconds(intervalWidth));
    }
}

// Calculate the spectral radius of the matrix: Diag(_MGFs(theta)) * _transitionMatrix,
// where Diag(_MGFs(theta)) is the diagonal matrix from evaluating each entry of _MGFs for a given theta value.
double MMBPArrival::calcSpectralRadius(double theta) const
{
    if (_MGFs.size() == 2) {
        // Hand-solved solution for 2 states
        double MGF0 = _MGFs[0]->calcMGF(theta);
        double MGF1 = _MGFs[1]->calcMGF(theta);
        double L1 = (_transitionMatrix[0][0] * MGF0 + _transitionMatrix[1][1] * MGF1 + sqrt((_transitionMatrix[0][0] * MGF0 - _transitionMatrix[1][1] * MGF1) * (_transitionMatrix[0][0] * MGF0 - _transitionMatrix[1][1] * MGF1) + 4.0 * _transitionMatrix[0][1] * _transitionMatrix[1][0] * MGF0 * MGF1)) / 2.0;
        double L2 = (_transitionMatrix[0][0] * MGF0 + _transitionMatrix[1][1] * MGF1 - sqrt((_transitionMatrix[0][0] * MGF0 - _transitionMatrix[1][1] * MGF1) * (_transitionMatrix[0][0] * MGF0 - _transitionMatrix[1][1] * MGF1) + 4.0 * _transitionMatrix[0][1] * _transitionMatrix[1][0] * MGF0 * MGF1)) / 2.0;
        return max(fabs(L1), fabs(L2));
    } else {
        // Generic solution for n states
        Eigen::MatrixXd m(_MGFs.size(), _MGFs.size());
        for (unsigned int fromState = 0; fromState < _transitionMatrix.size(); fromState++) {
            double stateMGF = _MGFs[fromState]->calcMGF(theta);
            if (!isfinite(stateMGF)) {
                return numeric_limits<double>::infinity();
            }
            for (unsigned int toState = 0; toState < _transitionMatrix[fromState].size(); toState++) {
                m(fromState, toState) = stateMGF * _transitionMatrix[fromState][toState];
            }
        }
        return m.eigenvalues().cwiseAbs().maxCoeff();
    }
}

// Equations for the MMBPArrival operator, representing an arrival process of a flow as analyzed by its trace.
void MMBPArrival::calcBound(double theta, double* sigma, double* rho) const
{
    *sigma = 0;
    *rho = log(calcSpectralRadius(theta)) / theta;
}

// Equations for the ConstantService operator, representing a constant service process with rate c.
void ConstantService::calcBound(double theta, double* sigma, double* rho) const
{
    *sigma = 0;
    *rho = -_c;
}

// Equations for the AggregateArrival operator, representing the aggregation of two arrival processes A and B.
void AggregateArrival::calcBound(double theta, double* sigma, double* rho) const
{
    double sigmaA, rhoA;
    double sigmaB, rhoB;
    _A->calcBound(getP() * theta, &sigmaA, &rhoA);
    _B->calcBound(getQ() * theta, &sigmaB, &rhoB);
    *sigma = sigmaA + sigmaB;
    *rho = rhoA + rhoB;
}

// Equations for the ConvolutionService operator, representing the convolution of two service processes S and T.
void ConvolutionService::calcBound(double theta, double* sigma, double* rho) const
{
    double sigmaS, rhoS;
    double sigmaT, rhoT;
    _S->calcBound(getP() * theta, &sigmaS, &rhoS);
    _T->calcBound(getQ() * theta, &sigmaT, &rhoT);
    // Handle the rhoS == rhoT case
    if (rhoS == rhoT) {
        rhoS *= 0.99;
    }
    *sigma = sigmaS + sigmaT - log(1.0 - exp(-theta * abs(rhoS - rhoT))) / theta;
    *rho = max(rhoS, rhoT);
}

// Equations for the OutputArrival operator, representing the departure process of an arrival process A after leaving a queue with service process S.
void OutputArrival::calcBound(double theta, double* sigma, double* rho) const
{
    double sigmaA, rhoA;
    double sigmaS, rhoS;
    _A->calcBound(getP() * theta, &sigmaA, &rhoA);
    _S->calcBound(getQ() * theta, &sigmaS, &rhoS);
    *sigma = sigmaA + sigmaS - log(1.0 - exp(theta * (rhoA + rhoS))) / theta;
    *rho = rhoA;
}

// Equations for the LeftoverService operator, representing the remaining service process that is leftover once a queue with service process S has accounted for the behavior of an arrival process A.
void LeftoverService::calcBound(double theta, double* sigma, double* rho) const
{
    double sigmaA, rhoA;
    double sigmaS, rhoS;
    _A->calcBound(getP() * theta, &sigmaA, &rhoA);
    _S->calcBound(getQ() * theta, &sigmaS, &rhoS);
    *sigma = sigmaA + sigmaS;
    *rho = rhoA + rhoS;
}

// Calculate the latency bound for a given theta value. Each invocation with a positive theta produces a valid (but possibly sub-optimal) upper bound on latency.
double LatencyBound::calcLatency(double theta) const
{
    double sigmaA, rhoA;
    double sigmaS, rhoS;
    _A->calcBound(getP() * theta, &sigmaA, &rhoA);
    _S->calcBound(getQ() * theta, &sigmaS, &rhoS);
    double latency = (log(_epsilon * (1.0 - exp(theta * (rhoA + rhoS)))) / theta - (sigmaA + sigmaS)) / rhoS;
    return latency * stepSize;
}

// Optimize over the space of positive theta values to search for the theta value that produces the best (i.e., tightest) latency bound.
double LatencyBound::calcTheta() const
{
    const double MIN_THETA = 1e-9;
    const double INITIAL_THETA = 1000.0;
    const double STEP_SIZE_DECREASE_FACTOR = 10.0;
    const double INITIAL_STEP_SIZE = INITIAL_THETA / STEP_SIZE_DECREASE_FACTOR;
    void* params = const_cast<LatencyBound*>(this);
    double theta = INITIAL_THETA;
    for (double stepSize = INITIAL_STEP_SIZE; stepSize >= MIN_THETA; stepSize /= STEP_SIZE_DECREASE_FACTOR) {
        theta = minSearch(max(theta - (STEP_SIZE_DECREASE_FACTOR * stepSize), MIN_THETA), theta + (STEP_SIZE_DECREASE_FACTOR * stepSize), stepSize, calcLatency, params);
    }
    return theta;
}

// Static helper function used by calcTheta.
double LatencyBound::calcLatency(double theta, void* params)
{
    const LatencyBound* p = static_cast<const LatencyBound*>(params);
    return p->calcLatency(theta);
}

// Calculate the latency bound using an optimized theta value.
double LatencyBound::calcLatency() const
{
    return calcLatency(calcTheta());
}

// Optimize the Hoelder dependency parameters. More research is needed to improve speed and accuracy.
double LatencyBound::dependencyOptimization() const
{
    const vector<DependencyParams*>& bounds = getDependentBounds();
    const unsigned int SEARCH_RANGE_DECREASE_COUNT = 25;
    const double SEARCH_RANGE_DECREASE_FACTOR = 1.2;
    const unsigned int ITERATION_COUNT = bounds.size() * 10;
    double minLatency = calcLatency();
    // Early exit if no dependencyParams to optimize
    if (bounds.size() == 0) {
        return minLatency;
    }
    // Optimize for the best Hoelder p value
    vector<double> bestP(bounds.size());
    for (unsigned int index = 0; index < bounds.size(); index++) {
        bestP[index] = bounds[index]->getP();
    }
    srand(1); // use fixed seed to avoid different latency calculations across multiple calls
    for (unsigned int i = 0; i < SEARCH_RANGE_DECREASE_COUNT; i++) {
        // Randomly search within search space
        for (unsigned int iter = 0; iter < ITERATION_COUNT; iter++) {
            // Set p/q
            for (unsigned int index = 0; index < bounds.size(); index++) {
                DependencyParams* bound = bounds[index];
                double searchRangeP = bound->getUpperP() - bound->getLowerP();
                double searchRangeQ = bound->getUpperQ() - bound->getLowerQ();
                double r = static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
                r *= searchRangeP + searchRangeQ;
                if (r <= searchRangeP) {
                    bound->setP(bound->getLowerP() + r);
                } else {
                    r -= searchRangeP;
                    bound->setQ(bound->getLowerQ() + r);
                }
            }
            // Check for better latency
            double latency = calcLatency();
            if (latency < minLatency) {
                minLatency = latency;
                for (unsigned int index = 0; index < bounds.size(); index++) {
                    bestP[index] = bounds[index]->getP();
                }
            }
        }
        // Update search space
        for (unsigned int index = 0; index < bounds.size(); index++) {
            DependencyParams* bound = bounds[index];
            bound->setP(bestP[index]);
            double searchRangeP = bound->getUpperP() - bound->getLowerP();
            searchRangeP /= SEARCH_RANGE_DECREASE_FACTOR;
            double searchRangeQ = bound->getUpperQ() - bound->getLowerQ();
            searchRangeQ /= SEARCH_RANGE_DECREASE_FACTOR;
            bound->setLowerP(max(bound->getP() - (searchRangeP / 2.0), 1.001));
            bound->setUpperP(bound->getLowerP() + searchRangeP);
            bound->setLowerQ(max(bound->getQ() - (searchRangeQ / 2.0), 1.001));
            bound->setUpperQ(bound->getLowerQ() + searchRangeQ);
        }
    }
    return minLatency;
}