#include "visual_utility/VisualUtilityEstimator.h"

#include <ros/ros.h>
#include <rosbag/view.h>
#include <limits>
#include <math.h>
#include <boost/scoped_ptr.hpp>
#include <boost/foreach.hpp>
#include <boost/functional/hash.hpp>
#include <boost/unordered_map.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <algorithm>
#include "visual_utility/cvutils-inl.h"
#include "cv_utils/DisplayImages.h"
#include "cv_utils/IntegralHistogram-Inl.h"
#include "visual_utility/Parameter.h"
#include "visual_utility/VisualUtilityEstimation.h"
#include "base/StringHash.h"
#include "visual_utility/VisualUtilityROSParams.h"
#include "hog_detector/integral_hog_detector_inl.h"

using namespace cv;
using namespace boost;
using namespace std;

namespace cv {
inline std::size_t hash_value(const cv::Rect& key) {
  size_t seed = 7823;
  hash_combine(seed, key.x);
  hash_combine(seed, key.y);
  hash_combine(seed, key.height);
  hash_combine(seed, key.width);
  return seed;
}

inline std::size_t hash_value(const cv::Size& key) {
  size_t seed = 54326;
  hash_combine(seed, key.height);
  hash_combine(seed, key.width);
  return seed;
}
} // end namespace

namespace visual_utility {

// ---------- Start VisualUtilityEstimator -----------

VisualUtilityEstimator::~VisualUtilityEstimator() {}

const Mat* VisualUtilityEstimator::GetLastTransform() const {
  return NULL;
}

void VisualUtilityEstimator::CalculateVisualUtility(
  const string& filename,
  const vector<Rect>& rois,
  double time,
  std::vector<ROIScore>* vuOut) {
  // Open up the file
  Mat cvImage = imread(filename);

  // Calculate the visual utility
  CalculateVisualUtility(cvImage, rois, time, vuOut);
}

void VisualUtilityEstimator::CalculateVisualUtility(
  const cv::Mat& image,
  const vector<Rect>& rois,
  double time,
  std::vector<ROIScore>* vuOut) {

  ROS_ASSERT(vuOut);
  lastRuntime_.reset(NULL);

  // Start by initializing anything that's needed with the image
  ros::WallTime startTime = ros::WallTime::now();
  InitBoxCalculator(image, time);
  ros::WallDuration initTime = ros::WallTime::now() - startTime;

  ROS_INFO_STREAM("Processing time for initialization: " << initTime.toSec());

  // Now collect all the rois of interest for each box size
  unordered_map<Size, shared_ptr<PointScores> > pointLists;
  vuOut->resize(rois.size());
  int i = 0;
  for(vector<Rect>::const_iterator regionI = rois.begin();
      regionI != rois.end(); ++regionI, ++i) {
    Size winSize(regionI->width, regionI->height);
    unordered_map<Size, shared_ptr<PointScores> >::iterator pointsPtr =
      pointLists.find(winSize);
    if (pointsPtr == pointLists.end()) {
      pointsPtr = pointLists.insert(pair<Size, shared_ptr<PointScores> >(
        winSize,
        shared_ptr<PointScores>(new PointScores()))).first;
    }
    (*vuOut)[i].second = *regionI;
    pointsPtr->second->push_back(pair<double*, Point>(&((*vuOut)[i].first),
                                                      Point(regionI->x,
                                                            regionI->y)));
  }

  // Now for each box size, calculate the visual utility scores across
  // the image.
  startTime = ros::WallTime::now();
  for(unordered_map<Size, shared_ptr<PointScores> >::const_iterator
        pointsI = pointLists.begin();
      pointsI != pointLists.end(); ++pointsI) {
    CalculateVisualUtilityBoxAcrossImage(pointsI->first.width,
                                         pointsI->first.height,
                                         *pointsI->second);
  }

  ros::WallDuration calculationTime = ros::WallTime::now() - startTime;
  ROS_INFO_STREAM("Processing time for calculation: " << calculationTime);
  if (lastRuntime_.get() == NULL) {
    lastRuntime_.reset(new double((initTime + calculationTime).toSec()));
  }
}

// Helper function for the following CalculateVisualUtility that
// creates a list of points to evaluate at based on a grid and a mask.
void VisualUtilityEstimator::GetLocationsForEvaluation(
  const Mat& image,
  int minX, int minY,
  int strideX, int strideY,
  double width, double height,
  const Mat& mask,
  Mat& scores,
  PointScores* locations) {
  ROS_ASSERT(mask.empty() || mask.type() == CV_8U);
  ROS_ASSERT(scores.type() == CV_64F);
  ROS_ASSERT(locations);
  locations->clear();

  for (int i = 0, curX = minX; curX < image.cols-width; ++i) {
    for (int j = 0, curY = minY; curY < image.rows-height; ++j) {
      if (mask.empty() || mask.at<uint8_t>(i,j)) {
        locations->push_back(pair<double*, Point>(scores.ptr<double>(i,j),
                                                  Point(curX,curY)));
      }
      curY += strideY;
    }
    curX += strideX;
  }
}

void VisualUtilityEstimator::GetGridHeightsAndWidths(const Mat& image,
                                                     int minX, int minY,
                                                     int minW, int minH,
                                                     double strideW,
                                                     double strideH,
                                                     bool fixAspect,
                                                     vector<double>* widths,
                                                     vector<double>* heights){
  ROS_ASSERT(widths);
  ROS_ASSERT(heights);

  if (fixAspect) {
    strideH = strideW;
  }

  for (double curW = minW; curW < image.cols-minX; curW *= strideW) {
    widths->push_back(cvRound(curW));
  }

  for (double curH = minH; curH < image.rows-minY; curH *= strideH) {
    heights->push_back(cvRound(curH));
  }

  // If the aspect ratio is fixed, truncate the list of sizes to the
  // smaller one.
  if (fixAspect) {
    if (heights->size() > widths->size()) {
      heights->resize(widths->size());
    } else {
      widths->resize(heights->size());
    }
  }

}

void VisualUtilityEstimator::InitializeScoreGrid(
  const Mat& image,
  int minX, int minY,
  int strideX, int strideY,
  bool fixAspect,
  const vector<double>& widths,
  const vector<double>& heights,
  Mat* scoreGrid) {
  ROS_ASSERT(scoreGrid);

  vector<int> scoreSize;
  scoreSize.push_back(widths.size());
  if (!fixAspect) scoreSize.push_back(heights.size());
  scoreSize.push_back(
    std::max<int>(1, (image.cols - minX - widths[0]) / strideX));
  scoreSize.push_back(
    std::max<int>(1, (image.rows - minY - heights[0]) / strideY));
  *scoreGrid = cv::Mat(scoreSize.size(), &scoreSize[0], CV_64F,
                   -numeric_limits<double>::infinity());
}

cv::Mat VisualUtilityEstimator::CalculateVisualUtility(
  const cv::Mat& image,
  int minX, int minY,
  int minW, int minH,
  int strideX, int strideY,
  double strideW, double strideH,
  bool fixAspect,
  double time,
  const cv::Mat& mask) {
  Mat scores;

  lastRuntime_.reset(NULL);

  // Start by initializing anything that's needed with the image
  ros::WallTime startTime = ros::WallTime::now();
  InitBoxCalculator(image, time);
  ros::WallDuration initTime = ros::WallTime::now() - startTime;

  vector<double> widths;
  vector<double> heights;
  GetGridHeightsAndWidths(image, minX, minY, minW, minH, strideW, strideH,
                          fixAspect, &widths, &heights);

  // Build the output score matrix
  InitializeScoreGrid(image, minX, minY, strideX, strideY, fixAspect,
                      widths, heights,
                      &scores);
  ROS_ASSERT(mask.empty() ||
             (mask.dims == scores.dims &&
              mask.size == scores.size));

  // Now walk through the different size of boxes
  PointScores locations;
  ros::WallDuration calculationTime(0);
  for (unsigned int widthI = 0u; widthI < widths.size(); ++widthI) {
    if (fixAspect) {
      Mat scorePlane(2, &scores.size[1], CV_64F,
                     scores.ptr(widthI), &scores.step[1]);
      GetLocationsForEvaluation(image,
                                minX, minY, strideX, strideY,
                                widths[widthI],
                                heights[widthI],
                                (mask.empty() ? mask :
                                 Mat(2, &mask.size[1], mask.type(),
                                     const_cast<uchar*>(mask.ptr(widthI)),
                                     &mask.step[1])),
                                scorePlane,
                                &locations);

      startTime = ros::WallTime::now();
      CalculateVisualUtilityBoxAcrossImage(widths[widthI],
                                           heights[widthI],
                                           locations);
      calculationTime += ros::WallTime::now() - startTime;

    } else {
      for (unsigned int heightI = 0u; heightI < heights.size(); ++heightI) {
        Mat scorePlane(2, &scores.size[2], CV_64F,
                       scores.ptr(widthI, heightI),
                       &scores.step[2]);
        GetLocationsForEvaluation(image,
                                  minX, minY, strideX, strideY,
                                  widths[widthI],
                                  heights[heightI],
                                  (mask.empty() ? mask :
                                   Mat(2, &mask.size[2], mask.type(),
                                       const_cast<uchar*>(mask.ptr(widthI,
                                                                  heightI)),
                                       &mask.step[2])),
                                  scorePlane,
                                  &locations);

        startTime = ros::WallTime::now();
        CalculateVisualUtilityBoxAcrossImage(widths[widthI],
                                             heights[heightI],
                                             locations);
        calculationTime += ros::WallTime::now() - startTime;
      }
    }
  }

  if (lastRuntime_.get() == NULL) {
    lastRuntime_.reset(new double((initTime + calculationTime).toSec()));
  }

  return scores;
}

bool VisualUtilityEstimator::InitBoxCalculator(const cv::Mat& image,
                                               double time) {
  ROS_FATAL("InitBoxCalculator is not implemented");
  ROS_ASSERT(false);
  return false;
}

void VisualUtilityEstimator::CalculateVisualUtilityBoxAcrossImage(
    double width, double height,
    const PointScores& locations) {

  for (PointScores::const_iterator pointI = locations.begin();
       pointI != locations.end();
       ++pointI) {
    *pointI->first = CalculateVisualUtilityOfBox(pointI->second.x,
                                                 pointI->second.y,
                                                 cvRound(height),
                                                 cvRound(width));
  }

}

double VisualUtilityEstimator::CalculateVisualUtilityOfBox(int x, int y,
                                                           int h, int w) {
  ROS_FATAL("Calculating the visual utility of a box is not implemented. "
            "Try wrapping it in one of the wrapper visual "
            "utility estimators like RelativeEntropyVUWrapper");
  ROS_ASSERT(false);
  return 0; // To make compiler happy
}

//-------- Start LABMotionVUEstimator --------------

LABMotionVUEstimator::~LABMotionVUEstimator() {}

const Mat* LABMotionVUEstimator::GetLastTransform() const {
  if (lastTransform_.empty()) {
    return NULL;
  }
  return &lastTransform_;
}

//#define GREY_OVERRIDE 0

cv::Mat_<double> LABMotionVUEstimator::CalculateVisualUtility(
  const Mat& image, double time) {

  if (image.channels() != 3) {
    ROS_ERROR("Image is not in color. We need a BGR image");
    return cv::Mat_<double>();
  }

  if (image.depth() != CV_8U) {
    ROS_FATAL("For speed reasons, the image must be still in its natural "
              "8-bit format");
    return cv::Mat_<double>();
  }

  // Get the LAB and greyscale images
  Mat_<uchar> tmpGreyImage;
  Mat_<double> curGreyImage;
  cvtColor(image, tmpGreyImage, CV_BGR2GRAY);
  tmpGreyImage.convertTo(curGreyImage, CV_64FC1, 1.0/255);

  // The L value in the image is scaled to be 0-255 instead of 0-100,
  // so we need to rescale it
  Mat_<Vec3b> tmpLabImage;
  Mat_<Vec3d> curLabImage;
  cvtColor(image, tmpLabImage, CV_BGR2Lab);
  tmpLabImage.assignTo(curLabImage, CV_64FC3);
  vector<Mat> channels;
  split(curLabImage, channels);
  channels[0] *= 100./255.;
  merge(channels, curLabImage);

  if (lastLabImage_.empty()) {
    // This is the first image added
    lastLabImage_ = curLabImage;
    lastGreyImage_ = curGreyImage;
    return cv::Mat_<double>();
  }

  // Calculate the affine transform to convert the last image to the
  // current one
  Mat lastTransform_ = transformEstimator_.EstimateTransform(lastGreyImage_,
                                                             curGreyImage);

  // Now warp the old image to the current one
#ifdef GREY_OVERRIDE
  Mat_<double> warpedImage;
  if (!lastTransform_.empty()) {
    warpedImage =  transformEstimator_.ApplyTransform(
      lastGreyImage_,
      lastTransform_,
      geo::BORDER_TRANSPARENT,
      0.0,
      curGreyImage);
  } else {
    warpedImage = lastGreyImage_;
  }
#else
  Mat_<Vec3d> warpedImage ;
  if (!lastTransform_.empty()) {
    warpedImage = transformEstimator_.ApplyTransform(
      lastLabImage_,
      lastTransform_,
      geo::BORDER_TRANSPARENT,
      Vec3d(0,0,0),
      curLabImage);
  } else {
    warpedImage = lastLabImage_;
  }
#endif

  // Next calculate the distance between the two images
  Mat_<float> dist(image.rows, image.cols, 0.0);

#ifdef GREY_OVERRIDE
  dist = abs(curGreyImage - warpedImage);
#else
  Mat_<Vec3d> diff = curLabImage - warpedImage;
  diff = diff.mul(diff);

  vector<Mat> diffChannels;
  split(diff, diffChannels);
  for (unsigned int i = 0u; i < diffChannels.size(); i++) {
    add(dist, diffChannels[i], dist, noArray(), CV_32F);
    //dist += diffChannels[i];
  }
  sqrt(dist, dist);
#endif

  lastLabImage_ = curLabImage;
  lastGreyImage_ = curGreyImage;

  // Do erode and dilate (aka openeing)
  morphologyEx(dist, dist, MORPH_OPEN,
               Mat_<int>::ones(openingSize_, openingSize_));

  return dist;
}

// Functor for calculating the sums needed for the alpha calculation
struct SumForAlphaFunctor {
  SumForAlphaFunctor(int& _nValidPixels,
                     double& _paretoSum,
                     double _logMode)
    : nValidPixels(_nValidPixels), paretoSum(_paretoSum), logMode(_logMode){}

  inline void operator()(const double val) {
    const double lVal = log(val);
    if (lVal >= logMode) {
      nValidPixels++;
      paretoSum += lVal - logMode;
    }
  }

  int& nValidPixels;
  double& paretoSum;
  const double logMode;

};

// Functor for building up the histogram
struct FillHistogramFunctor {
  FillHistogramFunctor(vector<double>& hist, double minVal, double maxVal)
    : hist_(hist), minVal_(minVal), maxVal_(maxVal),
      histFactor((hist.size()-1)/(maxVal-minVal)) {}

  inline void operator()(double val) {
    if (val >= minVal_ && val <= maxVal_) {
      hist_[static_cast<int>(histFactor*(val-minVal_))] += val;
    }
  }

  vector<double>& hist_;
  const double minVal_;
  const double maxVal_;
  const double histFactor;
};

double LABMotionVUEstimator::CalculateParetoThreshold(
  const Mat_<double>& dist) {
  // Fist find the range of distance values
  double maxVal;
  double minVal;
  minMaxLoc(dist, &minVal, &maxVal);

  if (minVal == maxVal) {
    return std::numeric_limits<double>::infinity();
  }

  // Calculate the histogram
  const int HIST_SIZE = 256;
  vector<double> hist(HIST_SIZE+1, 0.0);
  double histFactor = ((double)HIST_SIZE)/(maxVal - minVal);
  double bucketSize = 1./histFactor;
  cvutils::ApplyToEachElement<FillHistogramFunctor, double>(dist,
    FillHistogramFunctor(hist, minVal, maxVal));

  // Calculate the mode since we'll ignore points below that
  int modeBucket=0;
  double modeVal = 0;
  for (unsigned int i = 0u; i < hist.size(); ++i) {
    if (hist[i] > modeVal) {
      modeVal = hist[i];
      modeBucket = i;
    }
  }
  double mode = ((double)modeBucket)/histFactor + minVal + bucketSize/2;
  double logMode = log(mode);

  // Now calculate the alpha parameter
  int nValidPixels = 0;
  double paretoSum = 0;
  cvutils::ApplyToEachElement<SumForAlphaFunctor, double>(dist,
    SumForAlphaFunctor(nValidPixels, paretoSum, logMode));
  double alpha = nValidPixels / paretoSum;

  // Alternate way to calcuate alpha using the buckets
  /*int nValidPixels2 = 0;
  double paretoSum2 = 0;
  for (int i = 0; i < HIST_SIZE; ++i) {
    const double bucketCenter = ((double)i)/histFactor + minVal +
      bucketSize/2;
    if (bucketCenter >= mode) {
      nValidPixels2 += hist[i];
      paretoSum2 += hist[i]*(log(bucketCenter) - logMode);
    }
  }
  double alpha2 = nValidPixels2 / paretoSum2;

  ROS_WARN_STREAM("Alpha1: " << alpha << " Alpha2: " << alpha2);*/

  // Finally calculate the threshold to where we have paretoThresh_
  // fraction of the pixels
  return exp((alpha*logMode-log(paretoThreshold_)) / alpha);
}

// ------------ Start SpectralSaliency ------------
SpectralSaliency::~SpectralSaliency() {}

Mat_<double> SpectralSaliency::CalculateVisualUtility(
  const Mat& image, double time) {
  Mat_<double> retval;

  // Get a floating point grey version of the image
  /*Mat tmpGreyImage;
  if (image.channels() != 1) {
    cvtColor(image, tmpGreyImage, CV_BGR2GRAY);
  } else {
    tmpGreyImage = image;
  }
  Mat_<double> greyImage;
  tmpGreyImage.convertTo(greyImage, CV_64FC1, 1.0/255);
  Mat_<Vec<double, 2> > greyComplex;
  vector<Mat> greyGroup;
  greyGroup.push_back(greyImage);
  greyGroup.push_back(Mat_<double>::zeros(image.rows, image.cols));
  merge(greyGroup, greyComplex);

  Mat_<Vec<double, 2> > fftMat(image.rows, image.cols);
  dft(greyComplex, fftMat, DFT_COMPLEX_OUTPUT);

  vector<Mat> splitFftMat;
  split(fftMat, splitFftMat);

  Mat_<double> phaseMat;
  Mat_<double> logAmplitude;
  cartToPolar(splitFftMat[0], splitFftMat[1], logAmplitude, phaseMat);
  log(logAmplitude, logAmplitude);

  Mat_<double> spectralResidual;
  boxFilter(logAmplitude, spectralResidual, spectralResidual.depth(),
            Size(3, 3));
  spectralResidual = logAmplitude - spectralResidual;

  polarToCart(spectralResidual, phaseMat, splitFftMat[0], splitFftMat[1]);
  merge(splitFftMat, fftMat);

  Mat_<Vec<double, 2> > complexSaliency;
  dft(fftMat, complexSaliency, DFT_COMPLEX_OUTPUT | DFT_INVERSE);
  vector<Mat> splitSaliency;
  split(complexSaliency, splitSaliency);

  retval = splitSaliency[0].mul(splitSaliency[0]) +
    splitSaliency[1].mul(splitSaliency[1]);*/

  // The 32-bit floating point version to speed things up
  Mat tmpGreyImage;
  if (image.channels() != 1) {
    cvtColor(image, tmpGreyImage, CV_BGR2GRAY);
  } else {
    tmpGreyImage = image;
  }
  Mat_<float> greyImage;
  tmpGreyImage.convertTo(greyImage, CV_32FC1, 1.0/255);
  Mat_<Vec<float, 2> > greyComplex;
  vector<Mat> greyGroup;
  greyGroup.push_back(greyImage);
  greyGroup.push_back(Mat_<float>::zeros(image.rows, image.cols));
  merge(greyGroup, greyComplex);

  Mat_<Vec<float, 2> > fftMat(image.rows, image.cols);
  dft(greyComplex, fftMat, DFT_COMPLEX_OUTPUT);

  vector<Mat> splitFftMat;
  split(fftMat, splitFftMat);

  Mat_<float> phaseMat;
  Mat_<float> logAmplitude;
  cartToPolar(splitFftMat[0], splitFftMat[1], logAmplitude, phaseMat);
  log(logAmplitude, logAmplitude);

  Mat_<float> spectralResidual;
  boxFilter(logAmplitude, spectralResidual, spectralResidual.depth(),
            Size(3, 3));
  spectralResidual = logAmplitude - spectralResidual;

  polarToCart(spectralResidual, phaseMat, splitFftMat[0], splitFftMat[1]);
  merge(splitFftMat, fftMat);

  Mat_<Vec<float, 2> > complexSaliency;
  dft(fftMat, complexSaliency, DFT_COMPLEX_OUTPUT | DFT_INVERSE);
  vector<Mat> splitSaliency;
  split(complexSaliency, splitSaliency);

  retval = splitSaliency[0].mul(splitSaliency[0]) +
    splitSaliency[1].mul(splitSaliency[1]);

  return retval;
}

// ------- Start RelativeEntropyVUWrapper

RelativeEntropyVUWrapper::~RelativeEntropyVUWrapper() {}

bool RelativeEntropyVUWrapper::InitBoxCalculator(const cv::Mat& image,
                                                 double time) {
  // First get the visual utility of the entire image
  Mat_<double> baseVu = baseEstimator_->CalculateVisualUtility(image, time);

  // Now get the integral image so we can compute more quickly
  integralVu_.reset(new Mat_<double>());
  integral(baseVu, *integralVu_, CV_64F);

  return true;
}

double RelativeEntropyVUWrapper::CalculateVisualUtilityOfBox(int x, int y,
                                                             int h, int w) {
  const Mat_<double>& intImage = *integralVu_;

  double imArea = (intImage.rows-1)*(intImage.cols-1);
  double imSum = intImage[intImage.rows-1][intImage.cols-1];
  double pB = w*h / imArea;
  double rB = intImage[y][x] + intImage[y+h][x+w] -
    intImage[y][x+w] - intImage[y+h][x];
  double entropy = 0;
  if (imSum < 1e-10 || rB < 1e-10 || pB < 1e-10) {
    // We basically have a zero value, so force the entropy to be zero
  } else {
    rB /= imSum;
    if (rB > pB) {
      entropy = rB * log(rB/pB) + (1-rB) * log((1-rB)/(1-pB));
    } else {
      entropy = - pB * log(pB/rB) - (1-pB) * log((1-pB)/(1-rB));
    }
  }

  if (isnan(entropy)) {
    ROS_WARN_STREAM("Somehow we have the entropy being a NaN."
                    << " pB = " << pB
                    << " rB = " << rB
                    << " imSum = " << imSum);
  }

  return entropy;
}

// --------- Start LaplacianVU -------------
LaplacianVU::~LaplacianVU() {}

Mat_<double> LaplacianVU::CalculateVisualUtility(const cv::Mat& image,
                                                 double time) {
  Mat_<double> retval;

  Mat tmpGreyImage;
  if (image.channels() != 1) {
    cvtColor(image, tmpGreyImage, CV_BGR2GRAY);
  } else {
    tmpGreyImage = image;
  }
  Mat_<double> greyImage;
  tmpGreyImage.convertTo(greyImage, CV_64FC1, 1.0/255);

  Laplacian(greyImage, retval, CV_64FC1, ksize_);
  retval = abs(retval);

  return retval;
}

// --------- Start AverageCenterSurround ---------
CenterSurroundHistogram::~CenterSurroundHistogram() {}

CenterSurroundHistogram::CenterSurroundHistogram(
  const std::vector<double>& surroundScales,
  const std::string& distType)
  : surroundScales_(surroundScales) {
  if (distType == "chisq") {
    distType_ = CV_COMP_CHISQR;
  } else if (distType == "correl") {
    distType_ = CV_COMP_CORREL;
  } else if (distType == "intersect") {
    distType_ = CV_COMP_INTERSECT;
  } else if (distType == "bhattacharyya") {
    distType_ = CV_COMP_BHATTACHARYYA;
  } else {
    ROS_ERROR_STREAM("Invalid distance type: " << distType
                     << " defaulting to Chi Squared");
    distType_ = CV_COMP_CHISQR;
  }
}

Mat_<double> CenterSurroundHistogram::CalculateVisualUtility(const Mat& image,
                                                             double time) {
  ROS_FATAL("The visual utility for every pixel is not defined for this "
            "estimator.");
  return Mat_<double>();
}

bool CenterSurroundHistogram::InitBoxCalculator(const cv::Mat& image,
                                                double time) {
  // Get the image intensity since that's the feature we're going to use
  Mat tmpGreyImage;
  if (image.channels() != 1) {
    cvtColor(image, tmpGreyImage, CV_BGR2GRAY);
  } else {
    tmpGreyImage = image;
  }
  Mat_<uint8_t> greyImage(tmpGreyImage);

  // Calculate the integral histogram for the image
  integralHist_.reset(cv_utils::IntegralHistogram<int>::Calculate<uint8_t>(
    greyImage, 64, &std::pair<uint8_t, uint8_t>(0, 255)));

  return true;
}

double CenterSurroundHistogram::CalculateVisualUtilityOfBox(int x, int y,
                                                            int h, int w) {
  double bestScore = 0;
  for (vector<double>::const_iterator scaleI = surroundScales_.begin();
       scaleI != surroundScales_.end(); ++scaleI) {
    const double centerW = w * (*scaleI);
    const double centerH = h * (*scaleI);
    const int centerX1 = cvRound(x + (w - centerW) / 2.0);
    const int centerY1 = cvRound(y + (h - centerH) / 2.0);
    Mat_<float> centerHist =
      integralHist_->GetHistInRegion(Rect(centerX1, centerY1,
                                          cvRound(centerW), cvRound(centerH)));

    const double surW = centerW * M_SQRT2;
    const double surH = centerH * M_SQRT2;
    const int surX1 = cvRound(x + (w - surW) / 2.0);
    const int surY1 = cvRound(y + (h - surH) / 2.0);
    Mat_<float> surHist =
      integralHist_->GetHistInRegion(Rect(surX1, surY1,
                                          cvRound(surW), cvRound(surH)));
    surHist -= centerHist;

    // Normalize the histograms
    const int centerArea = cvRound(centerH) * cvRound(centerW);
    centerHist /= centerArea;
    surHist /= (cvRound(surW) * cvRound(surH) - centerArea);

    double score = compareHist(centerHist, surHist, distType_);
    if (score > bestScore) {
      bestScore = score;
    }
  }
  return bestScore;
}

// --------- Start Objectness ------------
Objectness::~Objectness() {}

Objectness::Objectness() : impl_(true) {
  if (!impl_.Init()) {
    ROS_FATAL_STREAM("Couldn't not initialize the objectness wrapper");
  }
}

void Objectness::CalculateVisualUtility(const cv::Mat& image,
                                        const std::vector<cv::Rect>& rois,
                                        double time,
                                        std::vector<ROIScore>* vuOut) {
  lastRuntime_.reset(new double(0));
  impl_.CalculateObjectness(image, rois, vuOut, lastRuntime_.get());
}

cv::Mat Objectness::CalculateVisualUtility(const cv::Mat& image,
                                           int minX, int minY,
                                           int minW, int minH,
                                           int strideX, int strideY,
                                           double strideW, double strideH,
                                           bool fixAspect,
                                           double time,
                                           const cv::Mat& mask) {
  Mat scores;

  vector<double> widths;
  vector<double> heights;
  GetGridHeightsAndWidths(image, minX, minY, minW, minH, strideW, strideH,
                          fixAspect, &widths, &heights);

  // Build the output score matrix
  InitializeScoreGrid(image, minX, minY, strideX, strideY, fixAspect,
                      widths, heights, &scores);
  ROS_ASSERT(mask.empty() ||
             (mask.dims == scores.dims &&
              mask.size == scores.size));

  // Build up the list of boxes to evaluate
  vector<Rect> rois;
  for (unsigned int widthI = 0u; widthI < widths.size(); ++widthI) {
    for (int curX = minX; curX < image.cols-widths[widthI];) {
      for (int curY = minY; curY < image.rows;) {
        if (fixAspect) {
          if (curY + heights[widthI] <= image.rows) {
            rois.push_back(Rect(curX, curY, widths[widthI], heights[widthI]));
          }
        } else {
          for (unsigned int heightI = 0u;
               heightI < heights.size() &&
                 curY + heights[heightI] <= image.rows;
               ++heightI) {
            rois.push_back(Rect(curX, curY, widths[widthI],
                                heights[heightI]));
          }
        }
        curY += strideY;
      }
      curX += strideX;
    }
  }

  // Do the evaluation
  vector<ROIScore> scoreList;
  CalculateVisualUtility(image, rois, time, &scoreList);

  // Now load the results into the output grid
  int gridIdx[4]; // (s, x, y) or (w, h, x, y)
  for (vector<ROIScore>::const_iterator scoreI = scoreList.begin();
       scoreI != scoreList.end(); ++scoreI) {
    gridIdx[0] = (log(scoreI->second.width) - log(minW)) / log(strideW);
    gridIdx[scores.dims-2] = (scoreI->second.x - minX) / strideX;
    gridIdx[scores.dims-1] = (scoreI->second.y - minY) / strideY;
    if (!fixAspect) {
      gridIdx[1] = (log(scoreI->second.height) - log(minH)) / log(strideH);
    }
    ROS_ASSERT(gridIdx[0] >= 0 && gridIdx[1] >= 0 && gridIdx[2] >= 0 &&
               gridIdx[0] < scores.size[0] &&
               gridIdx[1] < scores.size[1] &&
               gridIdx[2] < scores.size[2]);
    scores.at<double>(gridIdx) = scoreI->first;
  }

  return scores;
}

Mat_<double> Objectness::CalculateVisualUtility(const Mat& image,
                                                double time) {
  ROS_FATAL("Visual utility for a pixel is not defined");
  return Mat_<double>::zeros(image.rows, image.cols);
}

// ---------- Start ROSBag ---------------
ROSBag::ResultKey::ResultKey(const std::string& _filename,
                             const cv::Rect _rect)
  : filenameHash(hash<string>()(_filename)), rect(_rect) {}
size_t ROSBag::ResultKeyHash::operator()(const ResultKey& s) const {
  size_t result = s.filenameHash;
  hash_combine(result, hash_value(s.rect));
  return result;
}

ROSBag::ROSBag(const string& filename) {
  rosbag::Bag bag;
  bag.open(filename, rosbag::bagmode::Read);

  CreateBaseEstimator(bag);

  LoadBagResults(bag);

  bag.close();
}

ROSBag::~ROSBag() {}

void ROSBag::CreateBaseEstimator(const rosbag::Bag& bag) {

  // Load all the parameters into ros params
  ros::NodeHandle handle("~ROSBag");
  rosbag::View view(bag, rosbag::TopicQuery("parameters"));
  BOOST_FOREACH(rosbag::MessageInstance const msg, view) {
    Parameter::ConstPtr param = msg.instantiate<Parameter>();
    if (param != NULL) {
      handle.setParam(param->name.data, param->value.data);
    }
  }

  // Create the visual utility estimator
  transformEstimator_.reset(CreateTransformEstimator(handle));
  baseEstimator_.reset(CreateVisualUtilityEstimator(handle,
                                                    *transformEstimator_));

  // Delete all the parameters
  rosbag::View view2(bag, rosbag::TopicQuery("parameters"));
  BOOST_FOREACH(rosbag::MessageInstance const msg, view2) {
    Parameter::ConstPtr param = msg.instantiate<Parameter>();
    if (param != NULL) {
      handle.deleteParam(param->name.data);
    }
  }
}

void ROSBag::LoadBagResults(const rosbag::Bag& bag) {
  rosbag::View view(bag, rosbag::TopicQuery("results"));
  BOOST_FOREACH(rosbag::MessageInstance const msg, view) {
    VisualUtilityEstimation::ConstPtr result =
      msg.instantiate<VisualUtilityEstimation>();
    if (result != NULL) {
      if (result->regions.size() != result->scores.size()) {
        ROS_ERROR_STREAM("The number of regions and scores don't match. "
                         "Aborting");
        continue;
      }
      for (unsigned int i = 0u; i < result->regions.size(); ++i) {
        const sensor_msgs::RegionOfInterest& region = result->regions[i];
        lut_[ResultKey(result->image, Rect(region.x_offset, region.y_offset,
                                           region.width, region.height))] =
          result->scores[i];
      }
    }
  }
}

void ROSBag::CalculateVisualUtility(const cv::Mat& image,
                                    const std::vector<cv::Rect>& rois,
                                    double time,
                                    std::vector<ROIScore>* vuOut) {
  if (baseEstimator_.get()) {
    baseEstimator_->CalculateVisualUtility(image, rois, time, vuOut);
  }
}

cv::Mat_<double> ROSBag::CalculateVisualUtility(const cv::Mat& image,
                                                double time) {
  if (baseEstimator_.get()) {
    return baseEstimator_->CalculateVisualUtility(image, time);
  }
  return Mat_<double>::zeros(image.rows, image.cols);
}

void ROSBag::CalculateVisualUtility(const std::string& filename,
                                    const std::vector<cv::Rect>& rois,
                                    double time,
                                    std::vector<ROIScore>* vuOut) {
  ROS_ASSERT(vuOut);

  // Go through all the rois and look them up in the lookup table.
  for (vector<Rect>::const_iterator roi = rois.begin(); roi != rois.end();
       ++roi) {
    hash_map<ResultKey, float, ResultKeyHash>::const_iterator entry =
      lut_.find(ResultKey(filename, *roi));
    if (entry != lut_.end()) {
      vuOut->push_back(ROIScore(entry->second, *roi));
    } else {
      vuOut->push_back(ROIScore(0.0, *roi));
    }
  }

  if (vuOut->size() != rois.size()) {
    ROS_ERROR_STREAM("We could not find entries in the LUT for all of the "
                     "rois. "
                     << rois.size() - vuOut->size()
                     << " entries were missing for file: "
                     << filename);
  }
}

// ---------- Start HOGDetector ---------------
HOGDetector::~HOGDetector() {}

HOGDetector::HOGDetector(const std::string& modelFile,
                         bool useDefaultPeopleDetector,
                         Size winStride,
                         bool doCache) {
  if (!impl_.InitModel(modelFile, useDefaultPeopleDetector,
                       -numeric_limits<double>::infinity(), // threshold
                       false, // doNMS
                       winStride,
                       doCache)) {
    ROS_FATAL_STREAM("Could not initialize HOG detector");
  }
}

void HOGDetector::CalculateVisualUtility(const cv::Mat& image,
                                         const std::vector<cv::Rect>& rois,
                                         double time,
                                         std::vector<ROIScore>* vuOut) {
  ROS_ASSERT(vuOut);

  vector<double> scores;
  vector<Rect> foundLocations;
  lastRuntime_.reset(new double(0));
  impl_.DetectObjects(image, rois, &foundLocations, &scores,
                       lastRuntime_.get());

  ROS_ASSERT(scores.size() == foundLocations.size());

  for (unsigned int i = 0u; i < foundLocations.size(); ++i) {
    vuOut->push_back(ROIScore(scores[i], foundLocations[i]));
  }
}

Mat_<double> HOGDetector::CalculateVisualUtility(const Mat& image,
                                                 double time) {
  ROS_FATAL("The visual utility for every pixel is not defined for this "
            "estimator.");
  return Mat_<double>();
}

// ---------- Start CascadeDetector ---------------
CascadeDetector::~CascadeDetector() {}
CascadeDetector::CascadeDetector(const std::string& modelFile){
  if (!impl_.Init(modelFile)) {
    ROS_FATAL_STREAM("Could not load the classifier defined in " << modelFile);
  }
}

Mat_<double> CascadeDetector::CalculateVisualUtility(const Mat& image,
                                                 double time) {
  ROS_FATAL("The visual utility for every pixel is not defined for this "
            "estimator.");
  return Mat_<double>();
}

void CascadeDetector::CalculateVisualUtility(
  const cv::Mat& image,
  const std::vector<cv::Rect>& rois,
  double time,
  std::vector<ROIScore>* vuOut) {

  ROS_ASSERT(vuOut);

  if (image.empty()) {
    return;
  }

  vector<int> scores;
  vector<Rect> foundLocations;
  lastRuntime_.reset(new double(0));
  impl_.DetectObjects(image, rois, &foundLocations, &scores,
                      lastRuntime_.get());

  ROS_ASSERT(scores.size() == rois.size());
  for (unsigned int i = 0u; i < scores.size(); ++i) {
    vuOut->push_back(ROIScore(scores[i], rois[i]));
  }
}

// ----------- Start ScaledDetectorWrapper -----------
ScaledDetectorWrapper::~ScaledDetectorWrapper() {}

bool ScaledDetectorWrapper::InitBoxCalculator(const cv::Mat& image,
                                              double time) {
  ros::WallTime startTime = ros::WallTime::now();

  // Resize the image and send it to the base estimator
  Mat scaledImage;
  cv::resize(image, scaledImage, cv::Size(), scaleFactor_, scaleFactor_);

  resizeTime_ = ros::WallTime::now() - startTime;

  return baseEstimator_->InitBoxCalculator(scaledImage, time);
}

double ScaledDetectorWrapper::CalculateVisualUtilityOfBox(int x, int y,
                                                          int h, int w) {
  // Scale the coordinates of the box and send to the base estimaor
  return baseEstimator_->CalculateVisualUtilityOfBox(
    cvRound(x * scaleFactor_),
    cvRound(y * scaleFactor_),
    cvRound(h * scaleFactor_),
    cvRound(w * scaleFactor_));
}

void ScaledDetectorWrapper::CalculateVisualUtility(
  const cv::Mat& image,
  const std::vector<cv::Rect>& rois,
  double time,
  std::vector<ROIScore>* vuOut) {
  ROS_ASSERT(vuOut);

  // Scale the input rois
  vector<Rect> scaledRois;
  unordered_map<Rect, const Rect*> roiMap;
  for (vector<Rect>::const_iterator roiI = rois.begin();
       roiI != rois.end();
       ++roiI) {
    scaledRois.push_back(Rect(cvRound(roiI->x * scaleFactor_),
                              cvRound(roiI->y * scaleFactor_),
                              cvRound(roiI->width * scaleFactor_),
                              cvRound(roiI->height * scaleFactor_)));
    roiMap[scaledRois.back()] = &(*roiI);
  }

  ros::WallTime startTime = ros::WallTime::now();

  // Resize the image
  Mat scaledImage;
  cv::resize(image, scaledImage, cv::Size(), scaleFactor_, scaleFactor_);
  resizeTime_ = ros::WallTime::now() - startTime;

  // Calculate the visual utility
  vector<ROIScore> scaledScores;
  startTime = ros::WallTime::now();
  baseEstimator_->CalculateVisualUtility(scaledImage,
                                         scaledRois,
                                         time,
                                         &scaledScores);
  ros::WallDuration baseTime = ros::WallTime::now() - startTime;

  // Scale the output rois
  for (vector<ROIScore>::const_iterator scoreI = scaledScores.begin();
       scoreI != scaledScores.end();
       ++scoreI) {
    vuOut->push_back(ROIScore(scoreI->first,
                              *roiMap[scoreI->second]));
  }

  // Do the timing properly
  lastRuntime_.reset(new double(0));
  *lastRuntime_ += resizeTime_.toSec();
  if (baseEstimator_->GetLastRuntime()) {
    *lastRuntime_ += *baseEstimator_->GetLastRuntime();
  } else {
    *lastRuntime_ += baseTime.toSec();
  }

}

cv::Mat_<double> ScaledDetectorWrapper::CalculateVisualUtility(
  const cv::Mat& image,
  double time) {
  ros::WallTime startTime = ros::WallTime::now();

  // Rescale the image
  Mat scaledImage;
  cv::resize(image, scaledImage, cv::Size(), scaleFactor_, scaleFactor_);

  ros::WallDuration resizeTime = ros::WallTime::now() - startTime;
  startTime = ros::WallTime::now();

  // Calculate the visual utility on the scaled image
  Mat_<double> scaledVU = baseEstimator_->CalculateVisualUtility(
    scaledImage, time);

  Mat_<double> retval;
  cv::resize(scaledVU, retval, image.size());

  ros::WallDuration baseTime = ros::WallTime::now() - startTime;

  // Store the timing data properly
  lastRuntime_.reset(new double(0));
  *lastRuntime_ += resizeTime.toSec();
  if (baseEstimator_->GetLastRuntime()) {
    *lastRuntime_ += *baseEstimator_->GetLastRuntime();
  } else {
    *lastRuntime_ += baseTime.toSec();
  }
  return retval;
}

// ----------- Start IntegralHOGDetector -----------
IntegralHOGDetector::IntegralHOGDetector(const std::string& modelFile,
                                         const cv::Size& winStride)
  : impl_(modelFile, winStride), hist_(), histSum_() {}

IntegralHOGDetector::~IntegralHOGDetector() {}

bool IntegralHOGDetector::InitBoxCalculator(const cv::Mat& image,
                                            double time) {
  hist_.reset(impl_.ComputeGradientIntegralHistograms(image, &histSum_));

  return true;
}

double IntegralHOGDetector::CalculateVisualUtilityOfBox(int x, int y, int h,
                                                        int w) {
  return impl_.ComputeScore(*hist_, histSum_, Rect(x, y, w, h));
}

cv::Mat_<double> IntegralHOGDetector::CalculateVisualUtility(
  const cv::Mat& image,
  double time) {
  ROS_FATAL("Per pixel evaluation is not implemented for this estimator");
  ROS_ASSERT(false);
  return cv::Mat_<double>();
}

// ----------- Start IntegralHOGCascade -----------
IntegralHOGCascade::IntegralHOGCascade(const std::string& modelFile,
                                        const cv::Size& winStride)
  : impl_(modelFile, winStride), hist_(), histSum_() {}

IntegralHOGCascade::~IntegralHOGCascade() {}

bool IntegralHOGCascade::InitBoxCalculator(const cv::Mat& image,
                                           double time) {
  hist_.reset(impl_.ComputeGradientIntegralHistograms(image, &histSum_));

  return true;
}

double IntegralHOGCascade::CalculateVisualUtilityOfBox(int x, int y, int h,
                                                       int w) {
  return impl_.ComputeScore(*hist_, histSum_, Rect(x, y, w, h));
}

cv::Mat_<double> IntegralHOGCascade::CalculateVisualUtility(
  const cv::Mat& image,
  double time) {
  ROS_FATAL("Per pixel evaluation is not implemented for this estimator");
  ROS_ASSERT(false);
  return cv::Mat_<double>();
}

} // namespace


//
//  Copyright (C) 2018 Boran Adas, Google Summer of Code
//
//   @@ All Rights Reserved @@
//  This file is part of the RDKit.
//  The contents are covered by the terms of the BSD license
//  which is included in the file license.txt, found at the root
//  of the RDKit source tree.
//

#include <GraphMol/RDKitBase.h>
#include <GraphMol/Fingerprints/FingerprintUtil.h>
#include <GraphMol/Subgraphs/Subgraphs.h>
#include <RDGeneral/hash/hash.hpp>
#include <boost/dynamic_bitset.hpp>

#include <GraphMol/RDKitBase.h>
#include <GraphMol/SmilesParse/SmilesParse.h>
#include <GraphMol/Substruct/SubstructMatch.h>
#include <boost/dynamic_bitset.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_comparison.hpp>
#include <algorithm>
#include <RDGeneral/BoostStartInclude.h>
#include <boost/flyweight.hpp>
#include <boost/flyweight/key_value.hpp>
#include <boost/flyweight/no_tracking.hpp>
#include <RDGeneral/BoostEndInclude.h>

#include <GraphMol/QueryOps.h>
#include <DataStructs/ExplicitBitVect.h>
#include <DataStructs/BitOps.h>
#include <GraphMol/Subgraphs/SubgraphUtils.h>
#include <RDGeneral/Invariant.h>
#include <RDGeneral/BoostStartInclude.h>
#include <boost/random.hpp>
#include <cstdint>
#include <RDGeneral/BoostEndInclude.h>
#include <climits>
#include <RDGeneral/types.h>

namespace RDKit {
namespace AtomPairs {
unsigned int numPiElectrons(const Atom *atom) {
  PRECONDITION(atom, "no atom");
  unsigned int res = 0;
  if (atom->getIsAromatic()) {
    res = 1;
  } else if (atom->getHybridization() != Atom::SP3) {
    auto val = static_cast<unsigned int>(atom->getExplicitValence());
    unsigned int physical_bonds = atom->getNumExplicitHs();
    const auto &mol = atom->getOwningMol();
    for (const auto &bndi :
         boost::make_iterator_range(mol.getAtomBonds(atom))) {
      if (mol[bndi]->getValenceContrib(atom) != 0.0) {
        ++physical_bonds;
      }
    }
    CHECK_INVARIANT(val >= physical_bonds,
                    "explicit valence exceeds atom degree");
    res = val - physical_bonds;
  }
  return res;
}

std::uint32_t getAtomCode(const Atom *atom, unsigned int branchSubtract,
                          bool includeChirality) {
  PRECONDITION(atom, "no atom");
  std::uint32_t code;

  unsigned int numBranches = 0;
  if (atom->getDegree() > branchSubtract) {
    numBranches = atom->getDegree() - branchSubtract;
  }

  code = numBranches % maxNumBranches;
  unsigned int nPi = numPiElectrons(atom) % maxNumPi;
  code |= nPi << numBranchBits;

  unsigned int typeIdx = 0;
  unsigned int nTypes = 1 << numTypeBits;
  while (typeIdx < nTypes) {
    if (atomNumberTypes[typeIdx] ==
        static_cast<unsigned int>(atom->getAtomicNum())) {
      break;
    } else if (atomNumberTypes[typeIdx] >
               static_cast<unsigned int>(atom->getAtomicNum())) {
      typeIdx = nTypes;
      break;
    }
    ++typeIdx;
  }
  if (typeIdx == nTypes) {
    --typeIdx;
  }
  code |= typeIdx << (numBranchBits + numPiBits);
  if (includeChirality) {
    std::string cipCode;
    if (atom->getPropIfPresent(common_properties::_CIPCode, cipCode)) {
      std::uint32_t offset = numBranchBits + numPiBits + numTypeBits;
      if (cipCode == "R") {
        code |= 1 << offset;
      } else if (cipCode == "S") {
        code |= 2 << offset;
      }
    }
  }
  POSTCONDITION(code < static_cast<std::uint32_t>(
                           1 << (codeSize + (includeChirality ? 2 : 0))),
                "code exceeds number of bits");
  return code;
};

std::uint32_t getAtomPairCode(std::uint32_t codeI, std::uint32_t codeJ,
                              unsigned int dist, bool includeChirality) {
  PRECONDITION(dist < maxPathLen, "dist too long");
  std::uint32_t res = dist;
  res |= std::min(codeI, codeJ) << numPathBits;
  res |= std::max(codeI, codeJ)
         << (numPathBits + codeSize + (includeChirality ? numChiralBits : 0));
  return res;
}

std::uint64_t getTopologicalTorsionCode(
    const std::vector<std::uint32_t> &pathCodes, bool includeChirality) {
  bool reverseIt = false;
  unsigned int i = 0;
  unsigned int j = pathCodes.size() - 1;
  while (i < j) {
    if (pathCodes[i] > pathCodes[j]) {
      reverseIt = true;
      break;
    } else if (pathCodes[i] < pathCodes[j]) {
      break;
    }
    ++i;
    --j;
  }

  int shiftSize = codeSize + (includeChirality ? numChiralBits : 0);
  std::uint64_t res = 0;
  if (reverseIt) {
    for (unsigned int i = 0; i < pathCodes.size(); ++i) {
      res |= static_cast<std::uint64_t>(pathCodes[pathCodes.size() - i - 1])
             << (shiftSize * i);
    }
  } else {
    for (unsigned int i = 0; i < pathCodes.size(); ++i) {
      res |= static_cast<std::uint64_t>(pathCodes[i]) << (shiftSize * i);
    }
  }
  return res;
}

std::uint32_t getTopologicalTorsionHash(
    const std::vector<std::uint32_t> &pathCodes) {
  bool reverseIt = false;
  unsigned int i = 0;
  unsigned int j = pathCodes.size() - 1;
  while (i < j) {
    if (pathCodes[i] > pathCodes[j]) {
      reverseIt = true;
      break;
    } else if (pathCodes[i] < pathCodes[j]) {
      break;
    }
    ++i;
    --j;
  }

  std::uint32_t res = 0;
  if (reverseIt) {
    for (unsigned int i = 0; i < pathCodes.size(); ++i) {
      gboost::hash_combine(res, pathCodes[pathCodes.size() - i - 1]);
    }
  } else {
    for (unsigned int pathCode : pathCodes) {
      gboost::hash_combine(res, pathCode);
    }
  }
  return res;
}
}  // namespace AtomPairs

namespace MorganFingerprints {

// Definitions for feature points adapted from:
// Gobbi and Poppinger, Biotech. Bioeng. _61_ 47-54 (1998)
const char *smartsPatterns[6] = {
    "[$([N;!H0;v3,v4&+1]),\
$([O,S;H1;+0]),\
n&H1&+0]",                                                  // Donor
    "[$([O,S;H1;v2;!$(*-*=[O,N,P,S])]),\
$([O,S;H0;v2]),\
$([O,S;-]),\
$([N;v3;!$(N-*=[O,N,P,S])]),\
n&H0&+0,\
$([o,s;+0;!$([o,s]:n);!$([o,s]:c:n)])]",                    // Acceptor
    "[a]",                                                  // Aromatic
    "[F,Cl,Br,I]",                                          // Halogen
    "[#7;+,\
$([N;H2&+0][$([C,a]);!$([C,a](=O))]),\
$([N;H1&+0]([$([C,a]);!$([C,a](=O))])[$([C,a]);!$([C,a](=O))]),\
$([N;H0&+0]([C;!$(C(=O))])([C;!$(C(=O))])[C;!$(C(=O))])]",  // Basic
    "[$([C,S](=[O,S,P])-[O;H1,-1])]"                        // Acidic
};

const RDKit::ROMol *ss_matcher::getMatcher() const { return m_matcher.get(); }

ss_matcher::ss_matcher(){};
ss_matcher::ss_matcher(const std::string &pattern) {
  RDKit::RWMol *p = RDKit::SmartsToMol(pattern);
  TEST_ASSERT(p);
  m_matcher.reset(p);
};

typedef boost::flyweight<boost::flyweights::key_value<std::string, ss_matcher>,
                         boost::flyweights::no_tracking>
    pattern_flyweight;

std::vector<std::string> defaultFeatureSmarts(smartsPatterns,
                                              smartsPatterns + 6);
typedef boost::flyweight<boost::flyweights::key_value<std::string, ss_matcher>,
                         boost::flyweights::no_tracking>
    pattern_flyweight;
void getFeatureInvariants(const ROMol &mol, std::vector<uint32_t> &invars,
                          std::vector<const ROMol *> *patterns) {
  unsigned int nAtoms = mol.getNumAtoms();
  PRECONDITION(invars.size() >= nAtoms, "vector too small");

  std::vector<const ROMol *> featureMatchers;
  if (!patterns) {
    featureMatchers.reserve(defaultFeatureSmarts.size());
    for (std::vector<std::string>::const_iterator smaIt =
             defaultFeatureSmarts.begin();
         smaIt != defaultFeatureSmarts.end(); ++smaIt) {
      const ROMol *matcher = pattern_flyweight(*smaIt).get().getMatcher();
      CHECK_INVARIANT(matcher, "bad smarts");
      featureMatchers.push_back(matcher);
    }
    patterns = &featureMatchers;
  }
  std::fill(invars.begin(), invars.end(), 0);
  for (unsigned int i = 0; i < patterns->size(); ++i) {
    unsigned int mask = 1 << i;
    std::vector<MatchVectType> matchVect;
    // to maintain thread safety, we have to copy the pattern
    // molecules:
    SubstructMatch(mol, ROMol(*(*patterns)[i], true), matchVect);
    for (std::vector<MatchVectType>::const_iterator mvIt = matchVect.begin();
         mvIt != matchVect.end(); ++mvIt) {
      for (const auto &mIt : *mvIt) {
        invars[mIt.second] |= mask;
      }
    }
  }
}  // end of getFeatureInvariants()

void getConnectivityInvariants(const ROMol &mol, std::vector<uint32_t> &invars,
                               bool includeRingMembership) {
  unsigned int nAtoms = mol.getNumAtoms();
  PRECONDITION(invars.size() >= nAtoms, "vector too small");
  gboost::hash<std::vector<uint32_t>> vectHasher;
  for (unsigned int i = 0; i < nAtoms; ++i) {
    Atom const *atom = mol.getAtomWithIdx(i);
    std::vector<uint32_t> components;
    components.push_back(atom->getAtomicNum());
    components.push_back(atom->getTotalDegree());
    components.push_back(atom->getTotalNumHs());
    components.push_back(atom->getFormalCharge());
    int deltaMass = static_cast<int>(
        atom->getMass() -
        PeriodicTable::getTable()->getAtomicWeight(atom->getAtomicNum()));
    components.push_back(deltaMass);

    if (includeRingMembership &&
        atom->getOwningMol().getRingInfo()->numAtomRings(atom->getIdx())) {
      components.push_back(1);
    }
    invars[i] = vectHasher(components);
  }
}  // end of getConnectivityInvariants()

}  // namespace MorganFingerprints

namespace RDKitFPUtils {

void buildDefaultRDKitFingerprintAtomInvariants(
    const ROMol &mol, std::vector<std::uint32_t> &lAtomInvariants) {
  lAtomInvariants.clear();
  lAtomInvariants.reserve(mol.getNumAtoms());
  for (ROMol::ConstAtomIterator atomIt = mol.beginAtoms();
       atomIt != mol.endAtoms(); ++atomIt) {
    unsigned int aHash = ((*atomIt)->getAtomicNum() % 128) << 1 |
                         static_cast<unsigned int>((*atomIt)->getIsAromatic());
    lAtomInvariants.push_back(aHash);
  }
}

void enumerateAllPaths(const ROMol &mol, INT_PATH_LIST_MAP &allPaths,
                       const std::vector<std::uint32_t> *fromAtoms,
                       bool branchedPaths, bool useHs, unsigned int minPath,
                       unsigned int maxPath) {
  if (!fromAtoms) {
    if (branchedPaths) {
      allPaths = findAllSubgraphsOfLengthsMtoN(mol, minPath, maxPath, useHs);
    } else {
      allPaths = findAllPathsOfLengthsMtoN(mol, minPath, maxPath, true, useHs);
    }
  } else {
    for (auto aidx : *fromAtoms) {
      INT_PATH_LIST_MAP tPaths;
      if (branchedPaths) {
        tPaths =
            findAllSubgraphsOfLengthsMtoN(mol, minPath, maxPath, useHs, aidx);
      } else {
        tPaths =
            findAllPathsOfLengthsMtoN(mol, minPath, maxPath, true, useHs, aidx);
      }
      for (INT_PATH_LIST_MAP::const_iterator tpit = tPaths.begin();
           tpit != tPaths.end(); ++tpit) {
#ifdef VERBOSE_FINGERPRINTING
        std::cerr << "paths from " << aidx << " size: " << tpit->first
                  << std::endl;
        for (auto path : tpit->second) {
          std::cerr << " path: ";
          std::copy(path.begin(), path.end(),
                    std::ostream_iterator<int>(std::cerr, ", "));
          std::cerr << std::endl;
        }
#endif

        allPaths[tpit->first].insert(allPaths[tpit->first].begin(),
                                     tpit->second.begin(), tpit->second.end());
      }
    }
  }
}

void identifyQueryBonds(const ROMol &mol, std::vector<const Bond *> &bondCache,
                        std::vector<short> &isQueryBond) {
  bondCache.resize(mol.getNumBonds());
  ROMol::EDGE_ITER firstB, lastB;
  boost::tie(firstB, lastB) = mol.getEdges();
  while (firstB != lastB) {
    const Bond *bond = mol[*firstB];
    isQueryBond[bond->getIdx()] = 0x0;
    bondCache[bond->getIdx()] = bond;
    if (isComplexQuery(bond)) {
      isQueryBond[bond->getIdx()] = 0x1;
    }
    if (isComplexQuery(bond->getBeginAtom())) {
      isQueryBond[bond->getIdx()] |= 0x2;
    }
    if (isComplexQuery(bond->getEndAtom())) {
      isQueryBond[bond->getIdx()] |= 0x4;
    }
    ++firstB;
  }
}

std::vector<unsigned int> generateBondHashes(
    const ROMol &mol, boost::dynamic_bitset<> &atomsInPath,
    const std::vector<const Bond *> &bondCache,
    const std::vector<short> &isQueryBond, const PATH_TYPE &path,
    bool useBondOrder, const std::vector<std::uint32_t> *atomInvariants) {
  PRECONDITION(!atomInvariants || atomInvariants->size() >= mol.getNumAtoms(),
               "bad atomInvariants size");

  std::vector<unsigned int> bondHashes;
  atomsInPath.reset();
  bool queryInPath = false;
  std::vector<unsigned int> atomDegrees(mol.getNumAtoms(), 0);
  for (unsigned int i = 0; i < path.size() && !queryInPath; ++i) {
    const Bond *bi = bondCache[path[i]];
    CHECK_INVARIANT(bi, "bond not in cache");
    atomDegrees[bi->getBeginAtomIdx()]++;
    atomDegrees[bi->getEndAtomIdx()]++;
    atomsInPath.set(bi->getBeginAtomIdx());
    atomsInPath.set(bi->getEndAtomIdx());
    if (isQueryBond[path[i]]) {
      queryInPath = true;
    }
  }
  if (queryInPath) {
    return bondHashes;
  }

  // -----------------
  // calculate the bond hashes:
  std::vector<unsigned int> bondNbrs(path.size(), 0);
  bondHashes.reserve(path.size() + 1);

  for (unsigned int i = 0; i < path.size(); ++i) {
    const Bond *bi = bondCache[path[i]];
#ifdef REPORT_FP_STATS
    if (std::find(atomsToUse.begin(), atomsToUse.end(),
                  bi->getBeginAtomIdx()) == atomsToUse.end()) {
      atomsToUse.push_back(bi->getBeginAtomIdx());
    }
    if (std::find(atomsToUse.begin(), atomsToUse.end(), bi->getEndAtomIdx()) ==
        atomsToUse.end()) {
      atomsToUse.push_back(bi->getEndAtomIdx());
    }
#endif
    for (unsigned int j = i + 1; j < path.size(); ++j) {
      const Bond *bj = bondCache[path[j]];
      if (bi->getBeginAtomIdx() == bj->getBeginAtomIdx() ||
          bi->getBeginAtomIdx() == bj->getEndAtomIdx() ||
          bi->getEndAtomIdx() == bj->getBeginAtomIdx() ||
          bi->getEndAtomIdx() == bj->getEndAtomIdx()) {
        ++bondNbrs[i];
        ++bondNbrs[j];
      }
    }
#ifdef VERBOSE_FINGERPRINTING
    std::cerr << "   bond(" << i << "):" << bondNbrs[i] << std::endl;
#endif
    // we have the count of neighbors for bond bi, compute its hash:
    unsigned int a1Hash = (*atomInvariants)[bi->getBeginAtomIdx()];
    unsigned int a2Hash = (*atomInvariants)[bi->getEndAtomIdx()];
    unsigned int deg1 = atomDegrees[bi->getBeginAtomIdx()];
    unsigned int deg2 = atomDegrees[bi->getEndAtomIdx()];
    if (a1Hash < a2Hash) {
      std::swap(a1Hash, a2Hash);
      std::swap(deg1, deg2);
    } else if (a1Hash == a2Hash && deg1 < deg2) {
      std::swap(deg1, deg2);
    }
    unsigned int bondHash = 1;
    if (useBondOrder) {
      if (bi->getIsAromatic() || bi->getBondType() == Bond::AROMATIC) {
        // makes sure aromatic bonds always hash as aromatic
        bondHash = Bond::AROMATIC;
      } else {
        bondHash = bi->getBondType();
      }
    }
    std::uint32_t ourHash = bondNbrs[i];
    gboost::hash_combine(ourHash, bondHash);
    gboost::hash_combine(ourHash, a1Hash);
    gboost::hash_combine(ourHash, deg1);
    gboost::hash_combine(ourHash, a2Hash);
    gboost::hash_combine(ourHash, deg2);
    bondHashes.push_back(ourHash);
    // std::cerr<<"    "<<bi->getIdx()<<"
    // "<<a1Hash<<"("<<deg1<<")"<<"-"<<a2Hash<<"("<<deg2<<")"<<" "<<bondHash<<"
    // -> "<<ourHash<<std::endl;
  }
  return bondHashes;
}

}  // namespace RDKitFPUtils

}  // namespace RDKit


#include "baldr/tilehierarchy.h"
#include "baldr/graphid.h"
#include "midgard/pointll.h"

#include "test.h"

#include <boost/property_tree/ptree.hpp>

using namespace std;
using namespace valhalla::baldr;
using namespace valhalla::midgard;

namespace {

TEST(TileHierarchy, Parse) {
  EXPECT_EQ(TileHierarchy::levels().size(), 3) << "Incorrect number of hierarchy levels";
  EXPECT_EQ((++TileHierarchy::levels().begin())->second.name, "arterial")
      << "Middle hierarchy should be named arterial";
  EXPECT_EQ(TileHierarchy::levels().begin()->second.level, 0) << "Top hierarchy should have level 0";
  EXPECT_EQ(TileHierarchy::levels().rbegin()->second.tiles.TileSize(), .25f)
      << "Bottom hierarchy should have tile size of .25f";
  EXPECT_EQ(TileHierarchy::levels().find(5), TileHierarchy::levels().end())
      << "There should only be levels 0, 1, 2";
  EXPECT_NE(TileHierarchy::levels().find(2), TileHierarchy::levels().end())
      << "There should be a level 2";
  GraphId id = TileHierarchy::GetGraphId(PointLL(0, 0), 34);

  EXPECT_FALSE(id.Is_Valid()) << "GraphId should be invalid as the level doesn't exist";

  // there are 1440 cols and 720 rows, this spot lands on col 414 and row 522
  id = TileHierarchy::GetGraphId(PointLL(-76.5, 40.5), 2);
  EXPECT_EQ(id.level(), 2);
  EXPECT_EQ(id.tileid(), (522 * 1440) + 414);
  EXPECT_EQ(id.id(), 0);

  EXPECT_EQ(TileHierarchy::levels().begin()->second.importance, RoadClass::kPrimary)
      << "Importance should be set to primary";
  EXPECT_EQ((++TileHierarchy::levels().begin())->second.importance, RoadClass::kTertiary)
      << "Importance should be set to tertiary";
  EXPECT_EQ(TileHierarchy::levels().rbegin()->second.importance, RoadClass::kServiceOther)
      << "Importance should be set to service/other";
}

TEST(TileHierarchy, Tiles) {

  // there are 1440 cols and 720 rows, this spot lands on col 414 and row 522
  AABB2<PointLL> bbox{{-76.49, 40.51}, {-76.48, 40.52}};
  auto ids = TileHierarchy::GetGraphIds(bbox, 2);
  EXPECT_EQ(ids.size(), 1) << "Should have only found one result.";

  auto id = ids[0];
  EXPECT_EQ(id.level(), 2);
  EXPECT_EQ(id.tileid(), (522 * 1440) + 414);
  EXPECT_EQ(id.id(), 0);

  bbox = AABB2<PointLL>{{-76.51, 40.49}, {-76.49, 40.51}};
  ids = TileHierarchy::GetGraphIds(bbox, 2);
  EXPECT_EQ(ids.size(), 4) << "Should have found 4 results.";
}

} // namespace

int main(int argc, char* argv[]) {
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


/*
 * Copyright (C) 2019-2020 LEIDOS.
 *
 * 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.
 */

#include <ros/ros.h>
#include <string>
#include <algorithm>
#include <memory>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <lanelet2_core/geometry/Point.h>
#include <trajectory_utils/trajectory_utils.h>
#include <trajectory_utils/conversions/conversions.h>
#include <sstream>
#include <carma_utils/containers/containers.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <unordered_set>
#include "stop_and_wait_plugin.h"
#include <vector>
#include <cav_msgs/Trajectory.h>
#include <cav_msgs/StopAndWaitManeuver.h>
#include <lanelet2_core/primitives/Lanelet.h>
#include <lanelet2_core/geometry/LineString.h>
#include <carma_wm/CARMAWorldModel.h>
#include <carma_utils/containers/containers.h>
#include <carma_wm/Geometry.h>
#include <cav_msgs/TrajectoryPlanPoint.h>
#include <cav_msgs/TrajectoryPlan.h>
#include <math.h>
#include <std_msgs/Float64.h>

using oss = std::ostringstream;

namespace stop_and_wait_plugin
{
    void StopandWait::initialize()
    {
        nh_.reset(new ros::CARMANodeHandle());
        pnh_.reset(new ros::CARMANodeHandle("~"));
        pnh2_.reset(new ros::CARMANodeHandle("/"));

        trajectory_srv_ = nh_->advertiseService("plan_trajectory",&StopandWait::plan_trajectory_cb, this);

        plugin_discovery_pub_ = nh_->advertise<cav_msgs::Plugin>("plugin_discovery",1);
        jerk_pub_ = nh_->advertise<std_msgs::Float64>("jerk",1);
        plugin_discovery_msg_.name = "StopandWaitPlugin";
        plugin_discovery_msg_.versionId = "v1.0";
        plugin_discovery_msg_.available = true;
        plugin_discovery_msg_.activated = false;
        plugin_discovery_msg_.type = cav_msgs::Plugin::TACTICAL;
        plugin_discovery_msg_.capability = "tactical_plan/plan_trajectory";

        pose_sub_ = nh_->subscribe("current_pose",1, &StopandWait::pose_cb, this);
        twist_sub_ = nh_->subscribe("current_velocity", 1, &StopandWait::twist_cb, this);

        wml_.reset(new carma_wm::WMListener());
        wm_ = wml_->getWorldModel();

        pnh_->param<double>("minimal_trajectory_duration", minimal_trajectory_duration_);
        pnh_->param<double>("max_jerk_limit", max_jerk_limit_);
        pnh_->param<double>("min_timestep",min_timestep_);
        pnh_->param<double>("min_jerk", min_jerk_limit_);
        pnh_->param<double>("/guidance/destination_downtrack_range",destination_downtrack_range_);

        discovery_pub_timer_ = pnh_->createTimer(
            ros::Duration(ros::Rate(10.0)),
            [this](const auto&) {
                plugin_discovery_pub_.publish(plugin_discovery_msg_);
                std_msgs::Float64 jerk_msg;
                jerk_msg.data = jerk_;
                jerk_pub_.publish(jerk_msg);
             });
    }

    void StopandWait::run()
    {
        initialize();
        ros::CARMANodeHandle::spin();
    }

    void StopandWait::pose_cb(const geometry_msgs::PoseStampedConstPtr& msg)
    {
        pose_msg_ = geometry_msgs::PoseStamped(*msg.get());
    }

    void StopandWait::twist_cb(const geometry_msgs::TwistStampedConstPtr& msg)
    {
        current_speed_ = msg->twist.linear.x;
    }

    bool StopandWait::plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest& req, cav_srvs::PlanTrajectoryResponse& resp)
    {
        lanelet::BasicPoint2d veh_pos(pose_msg_.pose.position.x,pose_msg_.pose.position.y);
        ROS_DEBUG_STREAM("curr state x:"<< pose_msg_.pose.position.x << ", y: " << pose_msg_.pose.position.y);
        ROS_DEBUG_STREAM("old state x:"<<  req.vehicle_state.X_pos_global << ", y: " << req.vehicle_state.Y_pos_global);
        double current_downtrack = wm_->routeTrackPos(veh_pos).downtrack;
        ROS_DEBUG_STREAM("Starting stop&wait planning");
        ROS_DEBUG_STREAM("Current_downtrack"<<current_downtrack);

        // Only plan the trajectory for the requested STOP_AND_WAIT maneuver
        std::vector<cav_msgs::Maneuver> maneuver_plan;
        if(req.maneuver_plan.maneuvers[req.maneuver_index_to_plan].type != cav_msgs::Maneuver::STOP_AND_WAIT)
        {
            throw std::invalid_argument ("Stop and Wait Plugin doesn't support this maneuver type");
        }
        maneuver_plan.push_back(req.maneuver_plan.maneuvers[req.maneuver_index_to_plan]);
        resp.related_maneuvers.push_back(req.maneuver_index_to_plan);

        if(current_downtrack < maneuver_plan[0].stop_and_wait_maneuver.start_dist){
            //Do nothing
            return true;
        }

        std::vector<PointSpeedPair> points_and_target_speeds = maneuvers_to_points(maneuver_plan, current_downtrack, wm_, req.vehicle_state);

        auto downsampled_points =
            carma_utils::containers::downsample_vector(points_and_target_speeds,downsample_ratio_);
        ROS_DEBUG_STREAM("downsampled points size:"<<downsampled_points.size());
        //Trajectory plan
        cav_msgs::TrajectoryPlan  trajectory;
        trajectory.header.frame_id = "map";
        trajectory.header.stamp = ros::Time::now();
        trajectory.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()());

        trajectory.trajectory_points = compose_trajectory_from_centerline(downsampled_points,req.vehicle_state);
        ROS_DEBUG_STREAM("Trajectory points size:"<<trajectory.trajectory_points.size());
        trajectory.initial_longitudinal_velocity = req.vehicle_state.longitudinal_vel;
        resp.trajectory_plan = trajectory;
        resp.maneuver_status.push_back(cav_srvs::PlanTrajectory::Response::MANEUVER_IN_PROGRESS);

        return true;
    }

    std::vector<PointSpeedPair> StopandWait::maneuvers_to_points(const std::vector<cav_msgs::Maneuver>& maneuvers,
                                                                      double max_starting_downtrack,
                                                                      const carma_wm::WorldModelConstPtr& wm, const cav_msgs::VehicleState& state)
    {
        std::vector<PointSpeedPair> points_and_target_speeds;
        std::unordered_set<lanelet::Id> visited_lanelets;

        bool first = true;
        for(const auto& maneuver : maneuvers)
        {
            if(maneuver.type != cav_msgs::Maneuver::STOP_AND_WAIT)
            {
                throw std::invalid_argument ("Stop and Wait Maneuver Plugin doesn't support this maneuver type");
            }
            cav_msgs::StopAndWaitManeuver stop_and_wait_maneuver= maneuver.stop_and_wait_maneuver;

            double starting_downtrack = stop_and_wait_maneuver.start_dist;  //starting downtrack recorded in message
            if(first)   //check for first maneuver in vector
            {
                if(starting_downtrack > max_starting_downtrack)
                {
                    starting_downtrack = max_starting_downtrack;
                }
                first = false;

            }

            double ending_downtrack = stop_and_wait_maneuver.end_dist;
            double start_speed = current_speed_;    //Get static value of current speed at start of planning
            //maneuver_time_ = ros::Duration(stop_and_wait_maneuver.end_time - stop_and_wait_maneuver.start_time).toSec();

            maneuver_time_ = (3*(ending_downtrack - starting_downtrack))/(2*start_speed);

            double delta_time, curr_time;
            if(start_speed < epsilon_ )  //If at end_dist return zero speed trajectory
            {
                ///guidance/route/destination_downtrack_range
                auto shortest_path = wm_->getRoute()->shortestPath();

                delta_time = min_timestep_;
                curr_time = 0.0;
                //wait
                lanelet::BasicPoint2d curr_pose (pose_msg_.pose.position.x,pose_msg_.pose.position.y);
                while(curr_time < minimal_trajectory_duration_)
                {
                    PointSpeedPair pair;
                    pair.point = curr_pose;
                    pair.speed = 0.0;
                    points_and_target_speeds.push_back(pair);
                    curr_time += delta_time;

                    points_and_target_speeds.push_back(pair);
                }
            }
            else
            {
                double jerk_req = (2*start_speed)/pow(maneuver_time_,2);

                if(jerk_req > max_jerk_limit_)
                {
                    //unsafe to stop at the required jerk - reset to max_jerk and go beyond the maneuver end_dist
                    jerk_ = max_jerk_limit_;
                    double travel_dist_new = start_speed * maneuver_time_ - (0.167 * jerk_ * pow(maneuver_time_,3));
                    ending_downtrack = travel_dist_new + starting_downtrack;

                    auto shortest_path = wm_->getRoute()->shortestPath();
                    if(ending_downtrack > wm_->getRouteEndTrackPos().downtrack)
                    {
                        ROS_ERROR("Ending distance is beyond known route");
                        throw std::invalid_argument("Ending distance is beyond known route");
                    }
                }
                else {
                    //find distance to the end
                    lanelet::BasicPoint2d curr_pose (pose_msg_.pose.position.x,pose_msg_.pose.position.y);
                    double current_downtrack = wm_->routeTrackPos(curr_pose).downtrack;
                    //stay approximately at crawl speed until within destination downtrack range (defined in route)
                    if(start_speed <= min_crawl_speed_ && current_downtrack < ending_downtrack - destination_downtrack_range_){
                        jerk_ = 0.0;
                    }
                    else{
                        jerk_ = jerk_req;
                    }
                }
                //get all the lanelets in between starting and ending downtrack on shortest path
                auto lanelets = wm_->getLaneletsBetween(starting_downtrack, ending_downtrack, true);
                //record all the lanelets to be added to path
                std::vector<lanelet::ConstLanelet> lanelets_to_add;
                for (auto& l : lanelets)
                {
                    if(visited_lanelets.find(l.id()) == visited_lanelets.end())
                    {
                        lanelets_to_add.push_back(l);
                        visited_lanelets.insert(l.id());
                    }
                }

                lanelet::BasicLineString2d route_geometry = carma_wm::geometry::concatenate_lanelets(lanelets_to_add);
                int nearest_pt_index = getNearestRouteIndex(route_geometry,state);
                // route end point index
                auto temp_state = state;

                // maneuver end dist index
                int ending_downtrack_pt_index = (int)(route_geometry.size() * (ending_downtrack / wm_->getRoute()->length2d()));
                ROS_DEBUG_STREAM("ending_downtrack: " << ending_downtrack);
                ROS_DEBUG_STREAM("ending_downtrack_pt_index" << ending_downtrack_pt_index);
                ROS_DEBUG_STREAM("nearest_pt_index" << nearest_pt_index);

                lanelet::BasicLineString2d future_route_geometry(route_geometry.begin() + nearest_pt_index, route_geometry.begin()+ ending_downtrack_pt_index);

                int points_count = future_route_geometry.size();
                delta_time = maneuver_time_/(points_count-1);

                first = true;
                curr_time = 0.0;

                for(auto p : future_route_geometry)
                {
                    if (first && points_and_target_speeds.empty())
                    {
                        first = false;

                        continue; //Skip the first point to avoid duplicates from previous maneuver
                    }
                    PointSpeedPair pair;
                    pair.point = p;
                    pair.speed = start_speed - (0.5 * jerk_ * pow(curr_time,2));
                    if(pair.speed < 0.01){
                        pair.speed = 0.0;
                    }

                    if(p == future_route_geometry.back())
                    {
                        pair.speed = 0.0;    //force speed to 0 at last point
                    }
                    curr_time +=delta_time;

                    points_and_target_speeds.push_back(pair);
                }
                //Change timestep to 0.1s
                PointSpeedPair last_point = points_and_target_speeds.back();
                if(delta_time < min_timestep_){
                    int downsample_ratio = min_timestep_/delta_time;
                    points_and_target_speeds = carma_utils::containers::downsample_vector(points_and_target_speeds,downsample_ratio);
                }
                points_and_target_speeds.push_back(last_point);            }
            //If planned time is less than min trajectory duration add zero speed points
            while(curr_time < minimal_trajectory_duration_)
            {
                PointSpeedPair pair;
                pair.point = points_and_target_speeds.back().point;
                pair.speed = 0.0;
                curr_time += delta_time;
                points_and_target_speeds.push_back(pair);
            }

        }

        return points_and_target_speeds;
    }

    std::vector<cav_msgs::TrajectoryPlanPoint> StopandWait::compose_trajectory_from_centerline(
    const std::vector<PointSpeedPair>& points, const cav_msgs::VehicleState& state)
    {
        int nearest_pt_index = getNearestPointIndex(points,state);
        std::vector<PointSpeedPair> future_points(points.begin() + nearest_pt_index, points.end()); // Points in front of current vehicle position
        //Get yaw - geometrically
        std::vector<double> yaw_values;
        for(size_t i=0 ;i < future_points.size()-1 ;i++)
        {
            double yaw = atan((future_points[i+1].point.y() - future_points[i].point.y())/ (future_points[i+1].point.x() - future_points[i].point.x()));
            yaw_values.push_back(yaw);
        }
        yaw_values.push_back(0.0); //No rotation from last point

        //get target time from speed
        std::vector<double> target_times;
        std::vector<lanelet::BasicPoint2d> trajectory_locations;
        std::vector<double> trajectory_speeds;
        //split point speed pair
        splitPointSpeedPairs(future_points,&trajectory_locations,&trajectory_speeds);
        std::vector<double> downtracks = carma_wm::geometry::compute_arc_lengths(trajectory_locations);

        //get trajectory time from distance and speed
        speed_to_time(downtracks, trajectory_speeds,target_times, jerk_);
        std::vector <cav_msgs::TrajectoryPlanPoint> traj;
        ros::Time start_time = ros::Time::now();
        cav_msgs::TrajectoryPlanPoint traj_prev;
        traj_prev.x= future_points[0].point.x();
        traj_prev.y = future_points[0].point.y();
        traj_prev.yaw = yaw_values[0];
        traj_prev.target_time = start_time;

        for (size_t i=0; i < future_points.size(); i++)
        {
            cav_msgs::TrajectoryPlanPoint traj_point;
            if(trajectory_speeds[i] > 0.0){
                traj_point.x = future_points[i].point.x();
                traj_point.y = future_points[i].point.y();
                traj_point.yaw = yaw_values[i];
                traj_point.target_time = start_time + ros::Duration(target_times[i]);
            }
            else    //speed_to_time doesn't work for 0.0 speed
            {
                traj_point.x = future_points[i].point.x();
                traj_point.y = future_points[i].point.y();
                traj_point.yaw=traj_prev.yaw;
                traj_point.target_time = traj_prev.target_time + ros::Duration(min_timestep_);

            }
            traj_point.controller_plugin_name = "Pure Pursuit Jerk";
            traj_point.planner_plugin_name =plugin_discovery_msg_.name;
            traj.push_back(traj_point);
            traj_prev = traj_point;
        }
        //If trajectory only contains one 0.0 mph point, add another iteration(valid trajectory needs to have atleast 2 points)
        if(traj.size() == 1 && trajectory_speeds[0] == 0){
            cav_msgs::TrajectoryPlanPoint traj_point;
            traj_point = traj_prev;
            traj_point.target_time = traj_prev.target_time + ros::Duration(min_timestep_);
            traj.push_back(traj_point);
        }

        return traj;
    }

    void StopandWait::speed_to_time(const std::vector<double>& downtrack, const std::vector<double>& speeds,std::vector<double>& times, double jerk) const
    {
        if(downtrack.size() !=speeds.size())
        {
            throw std::invalid_argument("Input vector sizes do not match");
        }
        if (downtrack.empty())
        {
            throw std::invalid_argument("Input vectors are empty");
        }

        times.reserve(downtrack.size());

        //Uses equation
        //d_t = sqrt(2(d_v)/j)
        double prev_speed = speeds[0];
        double prev_time = 0.0;
        double prev_pos = downtrack[0];
        times.push_back(prev_time);
        for(int i=1; i <downtrack.size();i++)
        {
            double cur_speed = speeds[i];
            double delta_v = std::abs(cur_speed - prev_speed);
            double dt = sqrt(2*delta_v/jerk);
            double inst_acc = jerk * dt;
            if(jerk < min_jerk_limit_)    //Below minimum jerk slow down is ignored, treat as constant velocity
            {
                double cur_pos = downtrack[i];
                double delta_x = cur_pos - prev_pos;
                dt = delta_x/cur_speed;
            }
            double cur_time = dt + prev_time;
            times.push_back(cur_time);

            prev_speed = cur_speed;
            prev_time = cur_time;
            prev_pos = downtrack[i];
        }
    }

    int StopandWait::getNearestPointIndex(const std::vector<PointSpeedPair>& points, const cav_msgs::VehicleState& state) const
    {
        lanelet::BasicPoint2d veh_point(state.X_pos_global, state.Y_pos_global);
        double min_distance = std::numeric_limits<double>::max();
        int i = 0;
        int best_index = 0;
        for (const auto& p : points)
        {
            double distance = lanelet::geometry::distance2d(p.point, veh_point);
            if (distance < min_distance)
            {
            best_index = i;
            min_distance = distance;
            }
            i++;
        }

        return best_index;

    }

    int StopandWait::getNearestRouteIndex(lanelet::BasicLineString2d& points, const cav_msgs::VehicleState& state)
    {
        lanelet::BasicPoint2d veh_point(state.X_pos_global, state.Y_pos_global);
                double min_distance = std::numeric_limits<double>::max();
        int i = 0;
        int best_index = 0;
        for (const auto& p : points)
        {
            double distance = lanelet::geometry::distance2d(p,veh_point);
            if (distance < min_distance)
            {
            best_index = i;
            min_distance = distance;
            }
            i++;
        }
        return best_index;

    }

    void StopandWait::splitPointSpeedPairs(const std::vector<PointSpeedPair>& points, std::vector<lanelet::BasicPoint2d>* basic_points,
                        std::vector<double>* speeds) const
    {
        basic_points->reserve(points.size());
        speeds->reserve(points.size());

        for (const auto& p : points)
        {
            basic_points->push_back(p.point);
            speeds->push_back(p.speed);
        }
    }

}


/* Copyright (c) Members of the EGEE Collaboration. 2004.
See http://www.eu-egee.org/partners/ for details on the copyright
holders.

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. */
#include <unistd.h>

#include <cmath>

#include <iostream>
#include <iomanip>
#include <string>
#include <climits>

#include <boost/filesystem/path.hpp>

#include "common/src/utilities/streamdescriptor.h"
#include "glite/wms/common/logger/logstream.h"
#include "glite/wms/common/logger/manipulators.h"
#include "jobcontrol_namespace.h"

#include "SizeFile.h"

USING_COMMON_NAMESPACE;
using namespace std;
namespace fs = boost::filesystem;

RenameLogStreamNS( elog );

JOBCONTROL_NAMESPACE_BEGIN {

namespace logmonitor {

namespace {

int integer_size( size_t size, int basen = 10 )
{
  int         bitnumber = (int) (log((double)(UCHAR_MAX + 1)) / log(2.0)), answer;
  double      partial = size / sizeof(unsigned char), total = (bitnumber * partial), largest, rapporto;

  largest = pow( 2, total ) - 1;
  rapporto = log( largest ) / log( (double) basen );
  answer = (int) rapporto;

  if( ((double) answer) != rapporto ) answer += 1;

  return answer;
}

} // Anonymous namespace

size_t  SizeField::sf_s_long = integer_size( sizeof(long int) ), SizeField::sf_s_unsigned = integer_size( sizeof(unsigned int) );
const string SizeFile::sf_s_defaultHeader( "Condor log size file, DAG enabled.\n"
					   "This header is made to contain the dagId.\n"
					   "In this moment the DAG Id is unknown." );

ostream &operator<<( ostream &os, const SizeField &sf )
{
  os << setfill('0') << setw(SizeField::sf_s_long) << sf.sf_position << ' '
     << setfill('0') << setw(SizeField::sf_s_unsigned) << sf.sf_pending << ' '
     << sf.sf_last << " |";

  return os;
}

istream &operator>>( istream &is, SizeField &sf )
{
  bool            good, last;
  char            c;
  unsigned int    pending;
  long int        position;

  is >> position >> pending >> last >> c;
  good = is.good() && !is.bad() && (c == '|');

  if( good ) {
    sf.sf_position = position;
    sf.sf_pending = pending;
    sf.sf_last = last;
  }

  sf.sf_good = good;

  return is;
}

SizeField::SizeField( void ) : sf_good( false ), sf_last( false ), sf_pending( 0 ), sf_position( 0 )
{}

SizeField::SizeField( long int position, unsigned int pending, bool last ) : sf_good( true ), sf_last( last ),
									     sf_pending( pending ), sf_position( position )
{}

SizeField::~SizeField( void ) {}

SizeField &SizeField::reset( long int position, unsigned int pending, bool last )
{
  this->sf_position = position;
  this->sf_pending = pending;
  this->sf_last = last;
  this->sf_good = true;

  return *this;
}

ostream &operator<<( ostream &os, const SizeHeader &sh )
{
  os << sh.sh_header << "\n...";

  return os;
}

istream &operator>>( istream &is, SizeHeader &sh )
{
  bool            last;
  string          buffer, header;

  do {
    getline( is, buffer );

    last = (buffer == "...");
    if( is.good() && !last ) {
      header.append( buffer );
      header.append( 1, '\n' );
    }
    else if( last )
      header.erase( header.end() - 1 );

  } while( is.good() && !last );

  if( (sh.sh_good = last) )
    sh.sh_header.assign( header );
  else
    sh.sh_header.clear();

  return is;
}

SizeHeader::SizeHeader( const char *header ) : sh_good( header != NULL ), sh_header( header ? header : "" )
{}

SizeHeader::SizeHeader( const string &header ) : sh_good( header.size() != 0 ), sh_header( header )
{}

SizeHeader::~SizeHeader( void ) {}

SizeHeader &SizeHeader::reset( const string &header )
{
  this->sh_good = (header.size() != 0);
  this->sh_header.assign( header );

  return *this;
}

void SizeFile::createDotFile( void )
{
  fs::path      condorfile( this->sf_filename, fs::native );
  string        name( condorfile.leaf() );

  if( !condorfile.empty() ) {

    name.insert( name.begin(), '.' );
    name.append( ".size" );

    fs::path dotfile(condorfile.branch_path() / fs::path(name, fs::native));

    this->sf_filename.assign( dotfile.native_file_string() );
  }
  else this->sf_filename.clear();

  return;
}

void SizeFile::newSizeFile( void )
{
  if (! this->sf_stashed) {
    this->sf_stream.clear(); this->sf_stream.close();
  } else {
    this->sf_stashed = false;
  }
  this->sf_stream.open( this->sf_filename.c_str(), ios::out );

  if( !this->sf_header.good() )
    this->sf_header.reset( sf_s_defaultHeader );

  this->sf_stream << this->sf_header << endl << this->sf_current << endl;

  this->sf_stream.close();
  this->sf_stream.open( this->sf_filename.c_str(), ios::out | ios::in );
  this->sf_stream.seekp( 0, ios::end );

  return;
}

SizeField SizeFile::readField( streamoff position )
{
  SizeField     field;

  // For extra safety - private method
  if (this->sf_stashed) this->reopenFile();

  this->sf_stream.seekg( position );
  this->sf_stream >> field;

  return field;
}

SizeField SizeFile::readLastField( void )
{
  streamoff       position = (SizeField::dimension() + 1);
  SizeField       field;

  // For extra safety - private method
  if (this->sf_stashed) this->reopenFile();

  this->sf_stream.seekg( -position, ios::end );
  this->sf_stream >> field;

  return field;
}

bool SizeFile::checkOldFormat( void )
{
  bool               last, good = false;
  long int           position;
  unsigned int       pending;
  string             dagid, buffer;
  SizeField          field;

  // For extra safety - private method
  if (this->sf_stashed) this->reopenFile();

  this->sf_stream.clear(); this->sf_stream.seekg( 0 );
  this->sf_stream >> position >> pending >> last;

  if( this->sf_stream.good() ) this->sf_stream >> dagid;

  if( this->sf_stream.good() || this->sf_stream.eof() ) {
    this->sf_stream.clear();
    field.reset( position, pending, last );

    if( (good = field.good()) && (dagid.length() != 0) ) {
      buffer.assign( "Restored from old file\nDagId = " );
      buffer.append( dagid );
      buffer.append( "\n###########" );

      this->sf_header.reset( buffer );
    }

    if( good ) this->sf_current = field;
  }

  return good;
}

void SizeFile::openFile( bool create )
{
  int                   field_number;
  streamoff             position, filesize, field_storage_size;
  logger::StatePusher   pusher( elog::cedglog, "SizeFile::openFile()" );

  if( this->sf_filename.size() != 0 ) {
    utilities::create_file( this->sf_filename.c_str() );

    if( create ) {
      this->sf_header.reset( sf_s_defaultHeader );
      this->sf_current.reset( 0, 0, 0 );

      this->newSizeFile();

      if( !this->sf_stream.good() || this->sf_stream.bad() ) {
	elog::cedglog << logger::setlevel( logger::severe ) << "Cannot open size file \"" << this->sf_filename << "\"." << endl;

	this->sf_good = false;
      }
    }
    else {
      this->sf_stream.open( this->sf_filename.c_str(), ios::in | ios::out );

      if( this->sf_stream.good() ) {
	this->sf_stream.seekg( 0, ios::end );
	filesize = this->sf_stream.tellg();

	if( filesize != 0 ) { // File contains something
	  elog::cedglog << logger::setlevel( logger::info ) << "Size file is not empty. Checking header." << endl;

	  this->sf_stream.seekg( 0 ); // Rewind the file !!!
	  this->sf_stream >> this->sf_header;

	  if( this->sf_header.good() && !this->sf_stream.eof() ) { // Good header
	    elog::cedglog << logger::setlevel( logger::debug ) << "The header of the size file seems to be good..." << endl
			  << "Trying to read the last status..." << endl;

	    this->sf_current = this->readLastField();
	    if( this->sf_current.good() && this->sf_stream.good() ) { // Last field successfully read
	      elog::cedglog << logger::setlevel( logger::info ) << "Last field successfully read, winding size file." << endl
			    << logger::setlevel( logger::debug ) << "Position = " << this->sf_current.position() << endl
			    << "Pending jobs = " << this->sf_current.pending() << endl
			    << "Last job submitted = " << this->sf_current.last() << endl;

	      this->sf_stream.seekp( 0, ios::end );
	    }
	    else { // Try to read the last but one field
	      if( !this->sf_stream.good() || this->sf_stream.bad() ) this->sf_stream.clear();

	      field_storage_size = filesize - this->sf_header.size();
	      field_number = field_storage_size / (SizeField::dimension() + 1);

	      if( field_number != 0 ) { // There seems to be enough space to store some fields
		while( field_number > 0 ) { // Try to find the "first" good field
		  this->sf_current = this->readField( this->sf_header.size() + (field_number * (SizeField::dimension() + 1)) );

		  if( this->sf_current.good() && this->sf_stream.good() ) {
		    elog::cedglog << logger::setlevel( logger::info ) << "Badly closed file successfully recovered, winding it." << endl
				  << logger::setlevel( logger::debug ) << "Position = " << this->sf_current.position() << endl
				  << "Pending jobs = " << this->sf_current.pending() << endl
				  << "Last job submitted = " << this->sf_current.last() << endl;

		    position = this->sf_stream.tellg();
		    this->sf_stream.seekp( position ); // Put the write pointer at the last good known field.

		    break;
		  }
		  else if( !this->sf_stream.good() || this->sf_stream.bad() )
		    this->sf_stream.clear();

		  field_number -= 1;
		}

		if( field_number == 0 ) {
		  elog::cedglog << logger::setlevel( logger::error ) << "Cannot find any good size field inside the file." << endl
				<< logger::setlevel( logger::warning ) << "(Re)Starting with a zero sized file." << endl;

		  this->sf_current.reset( 0, 0, 0 );
		  this->newSizeFile();
		}
	      }
	      else { // File seems to be too short
		elog::cedglog << logger::setlevel( logger::error ) << "Size file is too short ("
			      << filesize << "/" << (this->sf_header.size() + SizeField::dimension() + 1) << ")" << endl
			      << logger::setlevel( logger::warning ) << "(Re)Starting with a zero sized file." << endl;

		this->sf_current.reset( 0, 0, 0 );
		this->newSizeFile();
	      }
	    }
	  }
	  else if( this->sf_stream.eof() ) { // Header corrupted or old file...
	    elog::cedglog << logger::setlevel( logger::error ) << "Size file header is not good."
			  << logger::setlevel( logger::info ) << "Trying to understand if the file is in the old format." << endl;

	    if( this->checkOldFormat() ) { // File in the old format !!! Good...
	      elog::cedglog << logger::setlevel( logger::info ) << "The file was in the old format... Well !!!" << endl
			    << logger::setlevel( logger::debug ) << "Position = " << this->sf_current.position() << endl
			    << "Pending jobs = " << this->sf_current.pending() << endl
			    << "Last job submitted = " << this->sf_current.last() << endl
			    << logger::setlevel( logger::info ) << "Reverting to the new format..." << endl;

	      this->newSizeFile();
	    }
	    else {
	      elog::cedglog << logger::setlevel( logger::error ) << "The file wasn't even in the old format." << endl
			    << logger::setlevel( logger::warning ) << "(Re)Starting with a zero sized file." << endl;

	      this->sf_current.reset( 0, 0, 0 );
	      this->newSizeFile();
	    }
	  }
	  else { // Yuck !!! I/O errors...
	    elog::cedglog << logger::setlevel( logger::severe ) << "Input/Output errors while reading size file." << endl;

	    this->sf_good = false;
	  }
	}
	else { // File is empty
	  elog::cedglog << logger::setlevel( logger::info ) << "Size file is empty. Writing new one." << endl;

	  this->sf_header.reset( sf_s_defaultHeader );
	  this->sf_current.reset( 0, 0, 0 );
	  this->sf_stream.seekp( 0 );
	  this->sf_stream << this->sf_header << endl << this->sf_current << endl;
	}
      }
      else { // Yuck !!! I/O errors...
	elog::cedglog << logger::setlevel( logger::severe ) << "Input/Output errors while reading size file." << endl;

	this->sf_good = false;
      }
    }
  }
  else { // Empty file name ???
    elog::cedglog << logger::setlevel( logger::severe ) << "Filename is empty: what I have to do ???" << endl;

    this->sf_good = false;
  }

  this->sf_stashed = false;
  return;
}

void SizeFile::stashFile( void )
{
  // Stash file to save the file descriptor.
  if ( this->sf_stashed ) return;
  sf_stash_pos = this->sf_stream.tellg();
  this->sf_stream.close();
  // SizeFile::good() can continue to be happy.
  this->sf_stashed = true;
}

void SizeFile::reopenFile( void )
{
  if ( ! this->sf_stashed ) return;
  this->sf_stream.open(this->sf_filename.c_str(), ios::in | ios::out);
  if (this->sf_stream.good()) this->sf_stream.seekg(sf_stash_pos);
  this->sf_good = this->sf_stream.good();
  this->sf_stashed = false;
}

void SizeFile::dumpField( void )
{
  // For extra safety - private method
  if (this->sf_stashed) this->reopenFile();

  if( this->sf_stream.good() )
    this->sf_stream << this->sf_current << endl;

  this->sf_good = this->sf_stream.good();

  return;
}

SizeFile::SizeFile( const char *filename, bool create ) : sf_good( true ), sf_stashed( false ), sf_filename( filename ? filename : "" ), sf_stream(),
							  sf_header(), sf_current()
{
  this->createDotFile();
  this->openFile( create );
  // Try to save an open FD
  this->stashFile();
}

void SizeFile::open( const char *filename, bool create )
{
  this->sf_good = true;
  this->sf_filename.assign( filename ? filename : "" );
  this->sf_stream.clear(); this->sf_stream.close();

  this->createDotFile();
  this->openFile( create );
  // Try to save an open FD
  this->stashFile();
}

SizeFile &SizeFile::update_position( long int new_position )
{
  if (this->sf_stashed) this->reopenFile();

  if( this->sf_good ) {
    this->sf_current.position( new_position );
    this->dumpField();
  }

  // Try to save an open FD
  this->stashFile();

  return *this;
}

SizeFile &SizeFile::update_pending( unsigned int new_pending )
{
  if (this->sf_stashed) this->reopenFile();

  if( this->sf_good ) {
    this->sf_current.pending( new_pending );
    this->dumpField();
  }

  // Try to save an open FD
  this->stashFile();

  return *this;
}

SizeFile &SizeFile::update_last( bool new_last )
{
  if (this->sf_stashed) this->reopenFile();

  if( this->sf_good ) {
    this->sf_current.last( new_last );
    this->dumpField();
  }

  // Try to save an open FD
  this->stashFile();

  return *this;
}

SizeFile &SizeFile::update( long int new_position, unsigned int new_pending, bool new_last )
{
  if (this->sf_stashed) this->reopenFile();

  if( this->sf_good ) {
    this->sf_current.reset( new_position, new_pending, new_last );
    this->dumpField();
  }

  // Try to save an open FD
  this->stashFile();

  return *this;
}

SizeFile &SizeFile::set_last( bool new_last )
{
  if( this->sf_good ) this->sf_current.last( new_last );

  return *this;
}

SizeFile &SizeFile::set_completed( void )
{
  if( this->sf_good ) this->sf_current.pending( 0 ).last( true );

  return *this;
}

SizeFile &SizeFile::increment_pending( void )
{
  unsigned int    old;

  if( this->sf_good ) {
    old = this->sf_current.pending() + 1;
    this->sf_current.pending( old );
  }

  return *this;
}

SizeFile &SizeFile::decrement_pending( void )
{
  unsigned int    old;

  if( this->sf_good ) {
    old = this->sf_current.pending();

    if( old > 0 ) {
      old -= 1;
      this->sf_current.pending( old );
    }
    // Something goes wrong but this is not a good reason for stopping
    //else this->sf_good = false;
  }

  return *this;
}

SizeFile &SizeFile::update_header( const std::string &newheader )
{
  int                  fd;
  string::size_type    space;
  string               buffer;

  if (this->sf_stashed) this->reopenFile();

  if( this->sf_good ) {
    if( this->sf_header.header().length() <= newheader.length() ) {
      space = newheader.length() - this->sf_header.header().length();

      buffer.assign( newheader );
      if( space > 1 ) {
	buffer.append( 1, '\n' );
	buffer.append( space - 1, '#' );
      }
      else if( space == 1 ) buffer.append( 1, ' ' );

      this->sf_header.reset( buffer );

      /*
	This is dangerous, too...
	But it should be faster than the next one...
      */

      if( this->sf_stream.good() ) {
	this->sf_stream.seekp( 0 );
	this->sf_stream << this->sf_header;

	this->sf_stream.seekp( 0, ios::end );

	this->sf_good = this->sf_stream.good();
      }
      else this->sf_good = false;
    }
    else {
      /*
	Dangerous operation, must rewind all the file and truncate it...
	Brrrrr...
	This restores back an old problem with I/O interrupted operations.
      */

      this->sf_header.reset( newheader );
      this->sf_stream.seekg( 0 ); this->sf_stream.seekp( 0 );

      fd = utilities::streamdescriptor( this->sf_stream );
      if( ftruncate(fd, 0) ) this->sf_good = false;
      else {
	this->sf_stream << this->sf_header << endl << this->sf_current << endl;

	this->sf_good = this->sf_stream.good();
      }
    }
  }

  // Try to save an open FD
  this->stashFile();

  return *this;
}

} // Namespace logmonitor

} JOBCONTROL_NAMESPACE_END