//=========================================================================
//
// Copyright 2018 Kitware, Inc.
// Author: Guilbert Pierre (spguilbert@gmail.com)
//         Laurenson Nick (nlaurenson5@gmail.com)
// Date: 03-27-2018
//
// 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 "SpinningSensorKeypointExtractor.h"

#include <numeric>

#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace {
//-----------------------------------------------------------------------------
template <typename T>
std::vector<size_t> sortIdx(const std::vector<T> &v)
{
  // initialize original index locations
  std::vector<size_t> idx(v.size());
  std::iota(idx.begin(), idx.end(), 0);

  // sort indexes based on comparing values in v
  std::sort(idx.begin(), idx.end(),
       [&v](size_t i1, size_t i2) {return v[i1] > v[i2];});

  return idx;
}

//-----------------------------------------------------------------------------
class LineFitting
{
public:
  // Fitting using PCA
  bool FitPCA(std::vector<Eigen::Vector3d >& points);

  // Futting using very local line and
  // check if this local line is consistent
  // in a more global neighborhood
  bool FitPCAAndCheckConsistency(std::vector<Eigen::Vector3d >& points);

  // Poor but fast fitting using
  // extremities of the distribution
  void FitFast(std::vector<Eigen::Vector3d >& points);

  // Direction and position
  Eigen::Vector3d Direction;
  Eigen::Vector3d Position;
  Eigen::Matrix3d SemiDist;
  Eigen::Matrix3d I3 = Eigen::Matrix3d::Identity();
  double MaxDistance = 0.02;
  double MaxSinAngle = 0.65;
};

//-----------------------------------------------------------------------------
bool LineFitting::FitPCA(std::vector<Eigen::Vector3d >& points)
{
  // Compute PCA to determine best line approximation
  // of the points distribution
  Eigen::MatrixXd data(points.size(), 3);

  for (unsigned int k = 0; k < points.size(); k++)
  {
    data.row(k) = points[k];
  }
  // Position
  this->Position = data.colwise().mean();
  Eigen::MatrixXd centered = data.rowwise() - this->Position.transpose();
  Eigen::Matrix3d varianceCovariance = centered.transpose() * centered;
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eig(varianceCovariance);

  // Direction
  this->Direction = eig.eigenvectors().col(2).normalized();

  // Semi distance matrix
  // (polar form associated to
  // a bilineare symmetric positive
  // semi-definite matrix)
  this->SemiDist = (this->I3 - this->Direction * this->Direction.transpose());

  bool isLineFittingAccurate = true;

  // if a point of the neighborhood is too far from
  // the fitting line we considere the neighborhood as
  // non flat
  double squaredMaxDistance = std::pow(this->MaxDistance, 2);
  for (unsigned int k = 0; k < points.size(); k++)
  {
    double d = (points[k] - this->Position).transpose() * this->SemiDist * (points[k] - this->Position);
    if (d > squaredMaxDistance)
    {
      isLineFittingAccurate = false;
    }
  }
  return isLineFittingAccurate;
}

//-----------------------------------------------------------------------------
bool LineFitting::FitPCAAndCheckConsistency(std::vector<Eigen::Vector3d >& points)
{
  bool isLineFittingAccurate = true;

  // first check if the neighborhood is straight
  Eigen::Vector3d U, V;
  U = (points[1] - points[0]).normalized();
  for (unsigned int index = 1; index < points.size() - 1; index++)
  {
    V = (points[index + 1] - points[index]).normalized();
    double sinAngle = (U.cross(V)).norm();
    if (sinAngle > this->MaxSinAngle)
    {
      isLineFittingAccurate = false;
    }
  }

  // Then fit with PCA
  isLineFittingAccurate &= this->FitPCA(points);
  return isLineFittingAccurate;
}

//-----------------------------------------------------------------------------
void LineFitting::FitFast(std::vector<Eigen::Vector3d >& points)
{
  // Take the two extrems points of the neighborhood
  // i.e the farest and the closest to the current point
  Eigen::Vector3d U = points[0];
  Eigen::Vector3d V = points[points.size() - 1];

  // direction
  this->Direction = (V - U).normalized();

  // position
  this->Position = U;

  // Semi distance matrix
  // (polar form associated to
  // a bilineare symmetric positive
  // semi-definite matrix)
  this->SemiDist = (this->I3 - this->Direction * this->Direction.transpose());
  this->SemiDist = this->SemiDist.transpose() * this->SemiDist;
}
}

//-----------------------------------------------------------------------------
void SpinningSensorKeypointExtractor::PrepareDataForNextFrame()
{
  // Reset the pcl format pointcloud to store the new frame
  this->pclCurrentFrameByScan.resize(this->NLasers);
  for (unsigned int k = 0; k < this->NLasers; ++k)
  {
    this->pclCurrentFrameByScan[k].reset(new pcl::PointCloud<Point>());
  }

  this->EdgesPoints.reset(new pcl::PointCloud<Point>());
  this->PlanarsPoints.reset(new pcl::PointCloud<Point>());
  this->BlobsPoints.reset(new pcl::PointCloud<Point>());

  this->Angles.clear();
  this->Angles.resize(this->NLasers);
  this->SaillantPoint.clear();
  this->SaillantPoint.resize(this->NLasers);
  this->DepthGap.clear();
  this->DepthGap.resize(this->NLasers);
  this->IntensityGap.clear();
  this->IntensityGap.resize(this->NLasers);
  this->IsPointValid.clear();
  this->IsPointValid.resize(this->NLasers);
  this->Label.clear();
  this->Label.resize(this->NLasers);
}

//-----------------------------------------------------------------------------
void SpinningSensorKeypointExtractor::ConvertAndSortScanLines()
{
  int nbPoints = this->pclCurrentFrame->size();
  double frameStartTime = this->pclCurrentFrame->points[0].time;
  double frameDuration = this->pclCurrentFrame->points[nbPoints-1].time - frameStartTime;

  for (size_t index = 0; index < nbPoints; ++index)
  {
    const Point& oldPoint = this->pclCurrentFrame->points[index];
    int id = this->LaserIdMapping[oldPoint.laserId];
    // modify the point so that:
    // - laserId is corrected with the laserIdMapping
    // - time become a relative advancement time (between 0 and 1)
    Point newPoint(oldPoint);
    newPoint.laserId = id;
    newPoint.time = (oldPoint.time - frameStartTime) / frameDuration;

    // add the current point to its corresponding laser scan
    this->pclCurrentFrameByScan[id]->push_back(newPoint);
  }
}

//-----------------------------------------------------------------------------
void SpinningSensorKeypointExtractor::ComputeKeyPoints(pcl::PointCloud<Point>::Ptr pc, std::vector<size_t> laserIdMapping)
{
  if (this->LaserIdMapping.empty())
  {
    this->NLasers = laserIdMapping.size();
    this->LaserIdMapping = laserIdMapping;
  }
  this->pclCurrentFrame = pc;
  this->PrepareDataForNextFrame();
  this->ConvertAndSortScanLines();
  // Initialize the vectors with the correct length
  for (unsigned int k = 0; k < this->NLasers; ++k)
  {
    size_t nbPoint = this->pclCurrentFrameByScan[k]->size();
    this->IsPointValid[k].resize(nbPoint, 1);
    this->Label[k].resize(nbPoint, 0);
    this->Angles[k].resize(nbPoint, 0);
    this->SaillantPoint[k].resize(nbPoint, 0);
    this->DepthGap[k].resize(nbPoint, 0);
    this->IntensityGap[k].resize(nbPoint, 0);
  }

  // Invalid points with bad criteria
  this->InvalidPointWithBadCriteria();

  // compute keypoints scores
  this->ComputeCurvature();

  // labelize keypoints
  this->SetKeyPointsLabels();
}

//-----------------------------------------------------------------------------
void SpinningSensorKeypointExtractor::ComputeCurvature()
{
  double squaredDistToLineThreshold = std::pow(this->DistToLineThreshold, 2);
  double squaredDepthDistCoeff = 0.25;
  // loop over scans lines
  for (unsigned int scanLine = 0; scanLine < this->NLasers; ++scanLine)
  {
    Point currentPoint, nextPoint, previousPoint;
    Eigen::Vector3d X, centralPoint;
    LineFitting leftLine, rightLine, farNeighborsLine;

    // We will compute the line that fit the neighbors located
    // previously the current. We will do the same for the
    // neighbors located after the current points. We will then
    // compute the angle between these two lines as an approximation
    // of the "sharpness" of the current point.
    std::vector<Eigen::Vector3d> leftNeighbor(this->NeighborWidth);
    std::vector<Eigen::Vector3d> rightNeighbor(this->NeighborWidth);
    std::vector<Eigen::Vector3d> farNeighbors;
    farNeighbors.reserve(3 * this->NeighborWidth);

    // loop over points in the current scan line
    int Npts = this->pclCurrentFrameByScan[scanLine]->size();

    // if the line is almost empty, skip it
    if (Npts < 2 * this->NeighborWidth + 1)
    {
      continue;
    }

    for (int index = this->NeighborWidth; (index + this->NeighborWidth) < Npts; ++index)
    {
      // Skip curvature computation for invalid points
      if (this->IsPointValid[scanLine][index] == 0)
      {
        continue;
      }

      // central point
      currentPoint = this->pclCurrentFrameByScan[scanLine]->points[index];
      centralPoint << currentPoint.x, currentPoint.y, currentPoint.z;

      // compute intensity gap
      nextPoint = this->pclCurrentFrameByScan[scanLine]->points[index + 1];
      previousPoint = this->pclCurrentFrameByScan[scanLine]->points[index - 1];
      this->IntensityGap[scanLine][index] = std::abs(nextPoint.intensity - previousPoint.intensity);

      // Fill right and left neighborhood
      // /!\ The way the neighbors are added
      // to the vectors matters. Especially when
      // computing the saillancy
      for (int j = index - this->NeighborWidth; j < index; ++j)
      {
        currentPoint = this->pclCurrentFrameByScan[scanLine]->points[j];
        leftNeighbor[j -index + this->NeighborWidth] << currentPoint.x, currentPoint.y, currentPoint.z;
      }
      for (int j = index + 1; j <= index + this->NeighborWidth; ++j)
      {
        currentPoint = this->pclCurrentFrameByScan[scanLine]->points[j];
        rightNeighbor[j - index - 1] << currentPoint.x, currentPoint.y, currentPoint.z;
      }

      // Fit line on the neighborhood and
      // Indicate if the left and right side
      // neighborhood of the current point is flat or not
      bool leftFlat = leftLine.FitPCAAndCheckConsistency(leftNeighbor);
      bool rightFlat = rightLine.FitPCAAndCheckConsistency(rightNeighbor);

      // Measurement of the gap
      double dist1 = 0; double dist2 = 0;

      // if both neighborhood are flat we can compute
      // the angle between them as an approximation of the
      // sharpness of the current point
      if (rightFlat && leftFlat)
      {
        // We check that the current point is not too far from its
        // neighborhood lines. This is because we don't want a point
        // to be considered as a angles point if it is due to gap
        dist1 = (centralPoint - leftLine.Position).transpose() * leftLine.SemiDist * (centralPoint - leftLine.Position);
        dist2 = (centralPoint - rightLine.Position).transpose() * rightLine.SemiDist * (centralPoint - rightLine.Position);

        if ((dist1 < squaredDistToLineThreshold) && (dist2 < squaredDistToLineThreshold))
          this->Angles[scanLine][index] = std::abs((leftLine.Direction.cross(rightLine.Direction)).norm()); // sin of angle actually
      }
      // Here one side of the neighborhood is non flat
      // Hence it is not worth to estimate the sharpness.
      // Only the gap will be considered here.
      else if (rightFlat && !leftFlat)
      {
        dist1 = std::numeric_limits<double>::max();
        for (unsigned int neighIndex = 0; neighIndex < leftNeighbor.size(); ++neighIndex)
        {
          dist1 = std::min(dist1,
                  ((leftNeighbor[neighIndex] - rightLine.Position).transpose() * rightLine.SemiDist * (leftNeighbor[neighIndex] - rightLine.Position))(0));
        }
        dist1 = squaredDepthDistCoeff * dist1;
      }
      else if (!rightFlat && leftFlat)
      {
        dist2 = std::numeric_limits<double>::max();
        for (unsigned int neighIndex = 0; neighIndex < leftNeighbor.size(); ++neighIndex)
        {
          dist2 = std::min(dist2,
                  ((rightNeighbor[neighIndex] - leftLine.Position).transpose() * leftLine.SemiDist * (rightNeighbor[neighIndex] - leftLine.Position))(0));
        }
        dist2 = squaredDepthDistCoeff * dist2;
      }
      else
      {
        // Compute saillant point score
        double currDepth = centralPoint.norm();
        unsigned int diffDepth = 0;
        bool canLeftBeAdded = true; bool hasLeftEncounteredDepthGap = false;
        bool canRightBeAdded = true; bool hasRightEncounteredDepthGap = false;

        // The saillant point score is the distance between the current point
        // and the points that have a depth gap with the current point
        farNeighbors.resize(0);
        for (unsigned int neighIndex = 0; neighIndex < leftNeighbor.size(); ++neighIndex)
        {
          // Left neighborhood depth gap computation
          if ((std::abs(leftNeighbor[leftNeighbor.size() - 1 - neighIndex].norm() - currDepth) > 1.5) && canLeftBeAdded)
          {
            hasLeftEncounteredDepthGap = true;
            diffDepth++;
            farNeighbors.emplace_back(leftNeighbor[neighIndex]);
          }
          else
          {
            if (hasLeftEncounteredDepthGap)
            {
              canLeftBeAdded = false;
            }
          }
          // Right neigborhood depth gap computation
          if ((std::abs(rightNeighbor[neighIndex].norm() - currDepth) > 1.5) && canRightBeAdded)
          {
            hasRightEncounteredDepthGap = true;
            diffDepth++;
            farNeighbors.emplace_back(rightNeighbor[neighIndex]);
          }
          else
          {
            if (hasRightEncounteredDepthGap)
            {
              canRightBeAdded = false;
            }
          }
        }

        // If there is enought neighbors with a big depth gap
        // we propose to compute the saillancy of the current
        // as the distance between the line that fits the neighbors
        // with a depth gap and the current point
        if (static_cast<double>(diffDepth) / (2.0 * this->NeighborWidth) > 0.5)
        {
          farNeighborsLine.FitPCA(farNeighbors);
          this->SaillantPoint[scanLine][index] =
            (centralPoint - farNeighborsLine.Position).transpose() * farNeighborsLine.SemiDist * (centralPoint - farNeighborsLine.Position);
        }
      }
      this->DepthGap[scanLine][index] = std::max(dist1, dist2);
    }
  }
}

//-----------------------------------------------------------------------------
void SpinningSensorKeypointExtractor::InvalidPointWithBadCriteria()
{
  // Temporary variables used in the next loop
  Eigen::Vector3d dX, X, Xn, Xp, Xproj, dXproj;
  Eigen::Vector3d Y, Yn, Yp, dY;
  double L, Ln, expectedLength, dLn, dLp;
  Point currentPoint, nextPoint, previousPoint;
  Point temp;

  // loop over scan lines
  for (unsigned int scanLine = 0; scanLine < this->NLasers; ++scanLine)
  {
    int Npts = this->pclCurrentFrameByScan[scanLine]->size();

    // if the line is almost empty, skip it
    if (Npts < 3 * this->NeighborWidth)
    {
      continue;
    }
    // invalidate first and last points
    for (int index = 0; index <= this->NeighborWidth; ++index)
    {
      this->IsPointValid[scanLine][index] = 0;
    }
    for (int index = Npts - 1 - this->NeighborWidth - 1; index < Npts; ++index)
    {
      this->IsPointValid[scanLine][index] = 0;
    }

    // loop over points into the scan line
    for (int index = this->NeighborWidth; index <  Npts - this->NeighborWidth - 1; ++index)
    {
      currentPoint = this->pclCurrentFrameByScan[scanLine]->points[index];
      nextPoint = this->pclCurrentFrameByScan[scanLine]->points[index + 1];
      previousPoint = this->pclCurrentFrameByScan[scanLine]->points[index - 1];
      X << currentPoint.x, currentPoint.y, currentPoint.z;
      Xn << nextPoint.x, nextPoint.y, nextPoint.z;
      Xp << previousPoint.x, previousPoint.y, previousPoint.z;
      dX = Xn - X;
      L = X.norm();
      Ln = Xn.norm();
      dLn = dX.norm();

      // the expected length between two firing of the same laser
      // depend on the distance and the angular resolution of the
      // sensor.
      expectedLength = 2.0 *  std::tan(this->AngleResolution / 2.0) * L;
      double ratioExpectedLength = 10.0;

      // if the length between the two firing
      // is more than n-th the expected length
      // it means that there is a gap. We now must
      // determine if the gap is due to the geometry of
      // the scene or if the gap is due to an occluded area
      if (dLn > ratioExpectedLength * expectedLength)
      {
        // Project the next point onto the
        // sphere of center 0 and radius =
        // norm of the current point. If the
        // gap has disappeared it means that
        // the gap was due to an occlusion
        Xproj = L / Ln * Xn;
        dXproj = Xproj - X;
        // it is a depth gap, invalidate the part which belong
        // to the occluded area (farest)
        // invalid next part
        if (L < Ln)
        {
          for (int i = index + 1; i <= index + this->NeighborWidth; ++i)
          {
            if (i > index + 1)
            {
              temp = this->pclCurrentFrameByScan[scanLine]->points[i - 1];
              Yp << temp.x, temp.y, temp.z;
              temp = this->pclCurrentFrameByScan[scanLine]->points[i];
              Y << temp.x, temp.y, temp.z;
              dY = Y - Yp;
              // if there is a gap in the neihborhood
              // we do not invalidate the rest of neihborhood
              if (dY.norm() > ratioExpectedLength * expectedLength)
              {
                break;
              }
            }
            this->IsPointValid[scanLine][i] = 0;
          }
        }
        // invalid previous part
        else
        {
          for (int i = index - this->NeighborWidth; i <= index; ++i)
          {
            if (i < index)
            {
              temp = this->pclCurrentFrameByScan[scanLine]->points[i + 1];
              Yn << temp.x, temp.y, temp.z;
              temp = this->pclCurrentFrameByScan[scanLine]->points[i];
              Y << temp.x, temp.y, temp.z;
              dY = Yn - Y;
              // if there is a gap in the neihborhood
              // we do not invalidate the rest of neihborhood
              if (dY.norm() > ratioExpectedLength * expectedLength)
              {
                break;
              }
            }
            this->IsPointValid[scanLine][i] = 0;
          }
        }
      }
      // Invalid points which are too close from the sensor
      if (L < this->MinDistanceToSensor)
      {
        this->IsPointValid[scanLine][index] = 0;
      }

      // Invalid points which are on a planar
      // surface nearly parallel to the laser
      // beam direction
      dLp = (X - Xp).norm();
      if ((dLp > 1 / 4.0 * ratioExpectedLength * expectedLength) && (dLn > 1 / 4.0 * ratioExpectedLength * expectedLength))
      {
        this->IsPointValid[scanLine][index] = 0;
      }
    }
  }
}

//-----------------------------------------------------------------------------
void SpinningSensorKeypointExtractor::SetKeyPointsLabels()
{
  this->EdgesIndex.clear(); this->EdgesIndex.resize(0);
  this->PlanarIndex.clear(); this->PlanarIndex.resize(0);
  this->BlobIndex.clear(); this->BlobIndex.resize(0);
  double squaredEdgeDepthGapThreshold = std::pow(this->EdgeDepthGapThreshold, 2);

  // loop over the scan lines
  for (unsigned int scanLine = 0; scanLine < this->NLasers; ++scanLine)
  {
    int Npts = this->pclCurrentFrameByScan[scanLine]->size();
    unsigned int nbrEdgePicked = 0;
    unsigned int nbrPlanarPicked = 0;

    // We split the validity of points between the edges
    // keypoints and planar keypoints. This allows to take
    // some points as planar keypoints even if they are close
    // to an edge keypoint.
    std::vector<double> IsPointValidForPlanar = this->IsPointValid[scanLine];

    // if the line is almost empty, skip it
    if (Npts < 3 * this->NeighborWidth)
    {
      continue;
    }

    // Sort the curvature score in a decreasing order
    std::vector<size_t> sortedDepthGapIdx = sortIdx<double>(this->DepthGap[scanLine]);
    std::vector<size_t> sortedAnglesIdx = sortIdx<double>(this->Angles[scanLine]);
    std::vector<size_t> sortedSaillancyIdx = sortIdx<double>(this->SaillantPoint[scanLine]);
    std::vector<size_t> sortedIntensityGap = sortIdx<double>(this->IntensityGap[scanLine]);

    double depthGap, sinAngle, saillancy, intensity;
    int index = 0;

    // Edges using depth gap
    for (int k = 0; k < Npts; ++k)
    {
      index = sortedDepthGapIdx[k];
      depthGap = this->DepthGap[scanLine][index];

      // thresh
      if (depthGap < squaredEdgeDepthGapThreshold)
      {
        break;
      }

      // if the point is invalid continue
      if (this->IsPointValid[scanLine][index] == 0)
      {
        continue;
      }

      // else indicate that the point is an edge
      this->Label[scanLine][index] = 4;
      this->EdgesIndex.push_back(std::pair<int, int>(scanLine, index));
      nbrEdgePicked++;
      //IsPointValidForPlanar[index] = 0;

      // invalid its neighborhod
      int indexBegin = index - this->NeighborWidth + 1;
      int indexEnd = index + this->NeighborWidth - 1;
      indexBegin = std::max(0, indexBegin);
      indexEnd = std::min(Npts - 1, indexEnd);
      for (int j = indexBegin; j <= indexEnd; ++j)
      {
        this->IsPointValid[scanLine][j] = 0;
      }
    }

    // Edges using angles
    for (int k = 0; k < Npts; ++k)
    {
      index = sortedAnglesIdx[k];
      sinAngle = this->Angles[scanLine][index];

      // thresh
      if (sinAngle < this->EdgeSinAngleThreshold)
      {
        break;
      }

      // if the point is invalid continue
      if (this->IsPointValid[scanLine][index] == 0)
      {
        continue;
      }

      // else indicate that the point is an edge
      this->Label[scanLine][index] = 4;
      this->EdgesIndex.push_back(std::pair<int, int>(scanLine, index));
      nbrEdgePicked++;
      //IsPointValidForPlanar[index] = 0;

      // invalid its neighborhod
      int indexBegin = index - this->NeighborWidth;
      int indexEnd = index + this->NeighborWidth;
      indexBegin = std::max(0, indexBegin);
      indexEnd = std::min(Npts - 1, indexEnd);
      for (int j = indexBegin; j <= indexEnd; ++j)
      {
        this->IsPointValid[scanLine][j] = 0;
      }
    }

    // Edges using saillancy
    for (int k = 0; k < Npts; ++k)
    {
      index = sortedSaillancyIdx[k];
      saillancy = this->SaillantPoint[scanLine][index];

      // thresh
      if (saillancy < this->SaillancyThreshold)
      {
        break;
      }

      // if the point is invalid continue
      if (this->IsPointValid[scanLine][index] == 0)
      {
        continue;
      }

      // else indicate that the point is an edge
      this->Label[scanLine][index] = 4;
      this->EdgesIndex.push_back(std::pair<int, int>(scanLine, index));
      nbrEdgePicked++;
      //IsPointValidForPlanar[index] = 0;

      // invalid its neighborhod
      int indexBegin = index - this->NeighborWidth + 1;
      int indexEnd = index + this->NeighborWidth - 1;
      indexBegin = std::max(0, indexBegin);
      indexEnd = std::min(Npts - 1, indexEnd);
      for (int j = indexBegin; j <= indexEnd; ++j)
      {
        this->IsPointValid[scanLine][j] = 0;
      }
    }

    // Edges using intensity
    for (int k = 0; k < Npts; ++k)
    {
      index = sortedIntensityGap[k];
      intensity = this->IntensityGap[scanLine][index];

      // thresh
      if (intensity < 50.0)
      {
        break;
      }

      // if the point is invalid continue
      if (this->IsPointValid[scanLine][index] == 0)
      {
        continue;
      }

      // else indicate that the point is an edge
      this->Label[scanLine][index] = 4;
      this->EdgesIndex.push_back(std::pair<int, int>(scanLine, index));
      nbrEdgePicked++;
      //IsPointValidForPlanar[index] = 0;

      // invalid its neighborhood
      int indexBegin = index - 1;
      int indexEnd = index + 1;
      indexBegin = std::max(0, indexBegin);
      indexEnd = std::min(Npts - 1, indexEnd);
      for (int j = indexBegin; j <= indexEnd; ++j)
      {
        this->IsPointValid[scanLine][j] = 0;
      }
    }

    // Blobs Points
//    if (!this->FastSlam)
//    {
      for (int k = 0; k < Npts; k = k + 3)
      {
        this->BlobIndex.push_back(std::pair<int, int>(scanLine, k));
      }
//    }

    // Planes
    for (int k = Npts - 1; k >= 0; --k)
    {
      index = sortedAnglesIdx[k];
      sinAngle = this->Angles[scanLine][index];

      // thresh
      if (sinAngle > this->PlaneSinAngleThreshold)
      {
        break;
      }

      // if the point is invalid continue
      if (IsPointValidForPlanar[index] == 0)
      {
        continue;
      }

      // else indicate that the point is a planar one
      if ((this->Label[scanLine][index] != 4) && (this->Label[scanLine][index] != 3))
        this->Label[scanLine][index] = 2;
      this->PlanarIndex.push_back(std::pair<int, int>(scanLine, index));
      IsPointValidForPlanar[index] = 0;
      this->IsPointValid[scanLine][index] = 0;

      // Invalid its neighbor so that we don't have too
      // many planar keypoints in the same region. This is
      // required because of the k-nearest search + plane
      // approximation realized in the odometry part. Indeed,
      // if all the planar points are on the same scan line the
      // problem is degenerated since all the points are distributed
      // on a line.
      int indexBegin = index - 4;
      int indexEnd = index + 4;
      indexBegin = std::max(0, indexBegin);
      indexEnd = std::min(Npts - 1, indexEnd);
      for (int j = indexBegin; j <= indexEnd; ++j)
      {
        IsPointValidForPlanar[j] = 0;
      }
      nbrPlanarPicked++;
    }
  }

  // add keypoints in increasing scan id order
  std::sort(this->EdgesIndex.begin(), this->EdgesIndex.end());
  std::sort(this->PlanarIndex.begin(), this->PlanarIndex.end());
  std::sort(this->BlobIndex.begin(), this->BlobIndex.end());

  // fill the keypoints vectors and compute the max dist keypoints
  this->FarestKeypointDist = 0.0;
  Point p;
  for (unsigned int k = 0; k < this->EdgesIndex.size(); ++k)
  {
    p = this->pclCurrentFrameByScan[this->EdgesIndex[k].first]->points[this->EdgesIndex[k].second];
    this->EdgesPoints->push_back(p);
    this->FarestKeypointDist = std::max(this->FarestKeypointDist, static_cast<double>(std::pow(p.x, 2) + std::pow(p.y, 2) + std::pow(p.z, 2)));
  }
  for (unsigned int k = 0; k < this->PlanarIndex.size(); ++k)
  {
    p = this->pclCurrentFrameByScan[this->PlanarIndex[k].first]->points[this->PlanarIndex[k].second];
    this->PlanarsPoints->push_back(p);
    this->FarestKeypointDist = std::max(this->FarestKeypointDist, static_cast<double>(std::pow(p.x, 2) + std::pow(p.y, 2) + std::pow(p.z, 2)));
  }
  for (unsigned int k = 0; k < this->BlobIndex.size();  ++k)
  {
    p = this->pclCurrentFrameByScan[this->BlobIndex[k].first]->points[this->BlobIndex[k].second];
    this->BlobsPoints->push_back(p);
    this->FarestKeypointDist = std::max(this->FarestKeypointDist, static_cast<double>(std::pow(p.x, 2) + std::pow(p.y, 2) + std::pow(p.z, 2)));
  }
  this->FarestKeypointDist = std::sqrt(this->FarestKeypointDist);

  // keypoints extraction informations
  std::cout << "Extracted Edges: " << this->EdgesPoints->size() << " Planars: "
            << this->PlanarsPoints->size() << " Blobs: "
            << this->BlobsPoints->size() << std::endl;
}

//-----------------------------------------------------------------------------
std::unordered_map<std::string, std::vector<double> >
SpinningSensorKeypointExtractor::GetDebugArray()
{
  auto get1DVector =  [this](std::vector<std::vector<double>> array) {
    std::vector<double> v (this->pclCurrentFrame->size());
    std::vector<int> indexPerByScanLine(this->NLasers, 0);
    for (int i = 0; i < this->pclCurrentFrame->size(); i++)
    {
      double laserId = this->LaserIdMapping[this->pclCurrentFrame->points[i].laserId];
      v[i] = array[laserId][indexPerByScanLine[laserId]];
      indexPerByScanLine[laserId]++;
    }
    return v;
  }; // end of lambda expression

  std::unordered_map<std::string, std::vector<double> > map;
  map["angles_line"]    = get1DVector(this->Angles);
  map["saillant_point"] = get1DVector(this->SaillantPoint);
  map["depth_gap"]      = get1DVector(this->DepthGap);
  map["intensity_gap"]  = get1DVector(this->IntensityGap);
  map["is_point_valid"] = get1DVector(this->IsPointValid);
  map["keypoint_label"] = get1DVector(this->Label);
  return map;
}


//
// author: Ed Valeev (eduard@valeyev.net)
// date  : July 8, 2014
// the use of this software is permitted under the conditions GNU General Public License (GPL) version 2
//

// standard C++ headers
#include <cmath>
#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <vector>
#include <chrono>

// OpenMP
#include <omp.h>

// Eigen matrix algebra library
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

// Libint Gaussian integrals library
#include <libint2.hpp>

typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
        Matrix;  // import dense, dynamically sized Matrix type from Eigen;
                 // this is a matrix with row-major storage (http://en.wikipedia.org/wiki/Row-major_order)
                 // to meet the layout of the integrals returned by the Libint integral library

struct Atom {
    int atomic_number;
    double x, y, z;
};

std::vector<Atom> read_geometry(const std::string& filename);
std::vector<libint2::Shell> make_sto3g_basis(const std::vector<Atom>& atoms);
size_t nbasis(const std::vector<libint2::Shell>& shells);
std::vector<size_t> map_shell_to_basis_function(const std::vector<libint2::Shell>& shells);
Matrix compute_soad(const std::vector<Atom>& atoms);
Matrix compute_1body_ints(const std::vector<libint2::Shell>& shells,
                          libint2::Operator t,
                          const std::vector<Atom>& atoms = std::vector<Atom>());
Matrix compute_2body_fock(const std::vector<libint2::Shell>& shells,
                                 const Matrix& D);

int main(int argc, char *argv[]) {

  using std::cout;
  using std::cerr;
  using std::endl;

  using libint2::Shell;
  using libint2::BasisSet;
  using libint2::Engine;
  using libint2::Operator;

  double fock_time = 0.0;
  double start;

  try {

    /*** =========================== ***/
    /*** initialize molecule         ***/
    /*** =========================== ***/

    // read geometry from a file; by default read from h2o.geom, else take filename (.xyz or .geom) from the command line
    const auto filename = (argc > 1) ? argv[1] : "h2o.geom";
    std::vector<Atom> atoms = read_geometry(filename);

    // count the number of electrons
    auto nelectron = 0;
    for (auto i = 0; i < atoms.size(); ++i)
      nelectron += atoms[i].atomic_number;
    const auto ndocc = nelectron / 2;

    // compute the nuclear repulsion energy
    auto enuc = 0.0;
    for (auto i = 0; i < atoms.size(); i++)
      for (auto j = i + 1; j < atoms.size(); j++) {
        auto xij = atoms[i].x - atoms[j].x;
        auto yij = atoms[i].y - atoms[j].y;
        auto zij = atoms[i].z - atoms[j].z;
        auto r2 = xij*xij + yij*yij + zij*zij;
        auto r = sqrt(r2);
        enuc += atoms[i].atomic_number * atoms[j].atomic_number / r;
      }
    cout << "\tNuclear repulsion energy = " << enuc << endl;

    /*** =========================== ***/
    /*** create basis set            ***/
    /*** =========================== ***/

    auto shells = make_sto3g_basis(atoms);
    size_t nao = 0;
    for (auto s=0; s<shells.size(); ++s)
      nao += shells[s].size();

    /*** =========================== ***/
    /*** compute 1-e integrals       ***/
    /*** =========================== ***/

    // initializes the Libint integrals library ... now ready to compute
    libint2::initialize();

    // compute overlap integrals
    auto S = compute_1body_ints(shells, Operator::overlap);

    // compute kinetic-energy integrals
    auto T = compute_1body_ints(shells, Operator::kinetic);

    // compute nuclear-attraction integrals
    Matrix V = compute_1body_ints(shells, Operator::nuclear, atoms);

    // Core Hamiltonian = T + V
    Matrix H = T + V;

    // T and V no longer needed, free up the memory
    T.resize(0,0);
    V.resize(0,0);

    /*** =========================== ***/
    /*** build initial-guess density ***/
    /*** =========================== ***/

    const auto use_hcore_guess = false;  // use core Hamiltonian eigenstates to guess density?
                                         // set to true to match the result of versions 0, 1, and 2 of the code
                                         // HOWEVER !!! even for medium-size molecules hcore will usually fail !!!
                                         // thus set to false to use Superposition-Of-Atomic-Densities (SOAD) guess
    Matrix D;
    if (use_hcore_guess) { // hcore guess
      // solve H C = e S C
      Eigen::GeneralizedSelfAdjointEigenSolver<Matrix> gen_eig_solver(H, S);
      auto eps = gen_eig_solver.eigenvalues();
      auto C = gen_eig_solver.eigenvectors();

      // compute density, D = C(occ) . C(occ)T
      auto C_occ = C.leftCols(ndocc);
      D = C_occ * C_occ.transpose();
    }
    else {  // SOAD as the guess density, assumes STO-nG basis
      D = compute_soad(atoms);
    }

    /*** =========================== ***/
    /*** main iterative loop         ***/
    /*** =========================== ***/

    const auto maxiter = 100;
    const auto conv = 1e-12;
    auto iter = 0;
    auto rmsd = 0.0;
    auto ediff = 0.0;
    auto ehf = 0.0;
    do {
      const auto tstart = std::chrono::high_resolution_clock::now();
      ++iter;

      // Save a copy of the energy and the density
      auto ehf_last = ehf;
      auto D_last = D;

      // build a new Fock matrix
      auto F = H;
      start = omp_get_wtime();
      F += compute_2body_fock(shells, D);
      fock_time += omp_get_wtime() - start;

      // solve F C = e S C
      Eigen::GeneralizedSelfAdjointEigenSolver<Matrix> gen_eig_solver(F, S);
      auto eps = gen_eig_solver.eigenvalues();
      auto C = gen_eig_solver.eigenvectors();

      // compute density, D = C(occ) . C(occ)T
      auto C_occ = C.leftCols(ndocc);
      D = C_occ * C_occ.transpose();

      // compute HF energy
      ehf = 0.0;
      for (auto i = 0; i < nao; i++)
        for (auto j = 0; j < nao; j++)
          ehf += D(i,j) * (H(i,j) + F(i,j));

      // compute difference with last iteration
      ediff = ehf - ehf_last;
      rmsd = (D - D_last).norm();

      const auto tstop = std::chrono::high_resolution_clock::now();
      const std::chrono::duration<double> time_elapsed = tstop - tstart;

      if (iter == 1)
        std::cout <<
        "\n\n Iter        E(elec)              E(tot)               Delta(E)             RMS(D)         Time(s)\n";
      printf(" %02d %20.12f %20.12f %20.12f %20.12f %10.5lf\n", iter, ehf, ehf + enuc,
             ediff, rmsd, time_elapsed.count());

    } while (((fabs(ediff) > conv) || (fabs(rmsd) > conv)) && (iter < maxiter));

    printf("** Hartree-Fock energy = %20.12f\n", ehf + enuc);

    libint2::finalize(); // done with libint

  } // end of try block; if any exceptions occurred, report them and exit cleanly

  catch (const char* ex) {
    cerr << "caught exception: " << ex << endl;
    return 1;
  }
  catch (std::string& ex) {
    cerr << "caught exception: " << ex << endl;
    return 1;
  }
  catch (std::exception& ex) {
    cerr << ex.what() << endl;
    return 1;
  }
  catch (...) {
    cerr << "caught unknown exception\n";
    return 1;
  }

  printf("\n\nFock build time = %.2f\n", fock_time);

  return 0;
}

// this reads the geometry in the same format used in older versions of the code
std::vector<Atom> read_dotgeom(std::istream& is) {
  size_t natom;
  is >> natom;

  std::vector<Atom> atoms(natom);
  for (auto i = 0; i < natom; i++)
    is >> atoms[i].atomic_number >> atoms[i].x >> atoms[i].y >> atoms[i].z;

  return atoms;
}

// this reads the geometry in the standard xyz format supported by most chemistry software
std::vector<Atom> read_dotxyz(std::istream& is) {
  // line 1 = # of atoms
  size_t natom;
  is >> natom;
  // read off the rest of line 1 and discard
  std::string rest_of_line;
  std::getline(is, rest_of_line);

  // line 2 = comment (possibly empty)
  std::string comment;
  std::getline(is, comment);

  std::vector<Atom> atoms(natom);
  for (auto i = 0; i < natom; i++) {
    std::string element_label;
    double x, y, z;
    is >> element_label >> x >> y >> z;

    // .xyz files report element labels, hence convert to atomic numbers
    int Z;
    if (element_label == "H")
      Z = 1;
    else if (element_label == "C")
      Z = 6;
    else if (element_label == "N")
      Z = 7;
    else if (element_label == "O")
      Z = 8;
    else if (element_label == "F")
      Z = 9;
    else if (element_label == "S")
      Z = 16;
    else if (element_label == "Cl")
      Z = 17;
    else {
      std::cerr << "read_dotxyz: element label \"" << element_label << "\" is not recognized" << std::endl;
      throw "Did not recognize element label in .xyz file";
    }

    atoms[i].atomic_number = Z;

    // .xyz files report Cartesian coordinates in angstroms; convert to bohr
    const auto angstrom_to_bohr = 1 / 0.52917721092; // 2010 CODATA value
    atoms[i].x = x * angstrom_to_bohr;
    atoms[i].y = y * angstrom_to_bohr;
    atoms[i].z = z * angstrom_to_bohr;
  }

  return atoms;
}

std::vector<Atom> read_geometry(const std::string& filename) {

  std::cout << "Will read geometry from " << filename << std::endl;
  std::ifstream is(filename);
  assert(is.good());

  // to prepare for MPI parallelization, we will read the entire file into a string that can be
  // broadcast to everyone, then converted to an std::istringstream object that can be used just like std::ifstream
  std::ostringstream oss;
  oss << is.rdbuf();
  // use ss.str() to get the entire contents of the file as an std::string
  // broadcast
  // then make an std::istringstream in each process
  std::istringstream iss(oss.str());

  // check the extension: if .xyz, assume the standard XYZ format, otherwise use the same format used by hf.v1
  if ( filename.rfind(".xyz") != std::string::npos)
    return read_dotxyz(iss);
  else if ( filename.rfind(".geom") != std::string::npos)
    return read_dotgeom(iss);
  else
    throw std::invalid_argument("unknown filename extension");
}

std::vector<libint2::Shell> make_sto3g_basis(const std::vector<Atom>& atoms) {

  using libint2::Shell;

  std::vector<Shell> shells;

  for(auto a=0; a<atoms.size(); ++a) {

    // STO-3G basis set
    // cite: W. J. Hehre, R. F. Stewart, and J. A. Pople, The Journal of Chemical Physics 51, 2657 (1969)
    //       doi: 10.1063/1.1672392
    // obtained from https://bse.pnl.gov/bse/portal
    switch (atoms[a].atomic_number) {
      case 1: // Z=1: hydrogen
        shells.push_back(
            {
              {3.425250910, 0.623913730, 0.168855400}, // exponents of primitive Gaussians
              {  // contraction 0: s shell (l=0), spherical=false, contraction coefficients
                {0, false, {0.15432897, 0.53532814, 0.44463454}}
              },
              {{atoms[a].x, atoms[a].y, atoms[a].z}}   // origin coordinates
            }
        );
        break;

      case 6: // Z=6: carbon
        shells.push_back(
            {
              {71.616837000, 13.045096000, 3.530512200},
              {
                {0, false, {0.15432897, 0.53532814, 0.44463454}}
              },
              {{atoms[a].x, atoms[a].y, atoms[a].z}}
            }
        );
        shells.push_back(
            {
              {2.941249400, 0.683483100, 0.222289900},
              {
                {0, false, {-0.09996723, 0.39951283, 0.70011547}}
              },
              {{atoms[a].x, atoms[a].y, atoms[a].z}}
            }
        );
        shells.push_back(
            {
              {2.941249400, 0.683483100, 0.222289900},
              { // contraction 0: p shell (l=1), spherical=false
                {1, false, {0.15591627, 0.60768372, 0.39195739}}
              },
              {{atoms[a].x, atoms[a].y, atoms[a].z}}
            }
        );
        break;

      case 7: // Z=7: nitrogen
        shells.push_back(
            {
              {99.106169000, 18.052312000, 4.885660200},
              {
                {0, false, {0.15432897, 0.53532814, 0.44463454}}
              },
              {{atoms[a].x, atoms[a].y, atoms[a].z}}
            }
        );
        shells.push_back(
            {
              {3.780455900, 0.878496600, 0.285714400},
              {
                {0, false, {-0.09996723, 0.39951283, 0.70011547}}
              },
              {{atoms[a].x, atoms[a].y, atoms[a].z}}
            }
        );
        shells.push_back(
            {
          {3.780455900, 0.878496600, 0.285714400},
              { // contraction 0: p shell (l=1), spherical=false
                {1, false, {0.15591627, 0.60768372, 0.39195739}}
              },
              {{atoms[a].x, atoms[a].y, atoms[a].z}}
            }
        );
        break;

      case 8: // Z=8: oxygen
        shells.push_back(
            {
              {130.709320000, 23.808861000, 6.443608300},
              {
                {0, false, {0.15432897, 0.53532814, 0.44463454}}
              },
              {{atoms[a].x, atoms[a].y, atoms[a].z}}
            }
        );
        shells.push_back(
            {
              {5.033151300, 1.169596100, 0.380389000},
              {
                {0, false, {-0.09996723, 0.39951283, 0.70011547}}
              },
              {{atoms[a].x, atoms[a].y, atoms[a].z}}
            }
        );
        shells.push_back(
            {
              {5.033151300, 1.169596100, 0.380389000},
              { // contraction 0: p shell (l=1), spherical=false
                {1, false, {0.15591627, 0.60768372, 0.39195739}}
              },
              {{atoms[a].x, atoms[a].y, atoms[a].z}}
            }
        );
        break;

      default:
        throw "do not know STO-3G basis for this Z";
    }

  }

  return shells;
}

size_t nbasis(const std::vector<libint2::Shell>& shells) {
  size_t n = 0;
  for (const auto& shell: shells)
    n += shell.size();
  return n;
}

size_t max_nprim(const std::vector<libint2::Shell>& shells) {
  size_t n = 0;
  for (auto shell: shells)
    n = std::max(shell.nprim(), n);
  return n;
}

int max_l(const std::vector<libint2::Shell>& shells) {
  int l = 0;
  for (auto shell: shells)
    for (auto c: shell.contr)
      l = std::max(c.l, l);
  return l;
}

std::vector<size_t> map_shell_to_basis_function(const std::vector<libint2::Shell>& shells) {
  std::vector<size_t> result;
  result.reserve(shells.size());

  size_t n = 0;
  for (auto shell: shells) {
    result.push_back(n);
    n += shell.size();
  }

  return result;
}

// computes Superposition-Of-Atomic-Densities guess for the molecular density matrix
// in minimal basis; occupies subshell by smearing electrons evenly over the orbitals
Matrix compute_soad(const std::vector<Atom>& atoms) {

  // compute number of atomic orbitals
  size_t nao = 0;
  for(const auto& atom: atoms) {
    const auto Z = atom.atomic_number;
    if (Z == 1 || Z == 2) // H, He
      nao += 1;
    else if (Z <= 10) // Li - Ne
      nao += 5;
    else
      throw "SOAD with Z > 10 is not yet supported";
  }

  // compute the minimal basis density
  Matrix D = Matrix::Zero(nao, nao);
  size_t ao_offset = 0; // first AO of this atom
  for(const auto& atom: atoms) {
    const auto Z = atom.atomic_number;
    if (Z == 1 || Z == 2) { // H, He
      D(ao_offset, ao_offset) = Z; // all electrons go to the 1s
      ao_offset += 1;
    }
    else if (Z <= 10) {
      D(ao_offset, ao_offset) = 2; // 2 electrons go to the 1s
      D(ao_offset+1, ao_offset+1) = (Z == 3) ? 1 : 2; // Li? only 1 electron in 2s, else 2 electrons
      // smear the remaining electrons in 2p orbitals
      const double num_electrons_per_2p = (Z > 4) ? (double)(Z - 4)/3 : 0;
      for(auto xyz=0; xyz!=3; ++xyz)
        D(ao_offset+2+xyz, ao_offset+2+xyz) = num_electrons_per_2p;
      ao_offset += 5;
    }
  }

  return D * 0.5; // we use densities normalized to # of electrons/2
}

Matrix compute_1body_ints(const std::vector<libint2::Shell>& shells,
                          libint2::Operator obtype,
                          const std::vector<Atom>& atoms)
{
  using libint2::Shell;
  using libint2::Engine;
  using libint2::Operator;

  const auto n = nbasis(shells);
  Matrix result(n,n);

  // construct the overlap integrals engine
  Engine engine(obtype, max_nprim(shells), max_l(shells), 0);
  // nuclear attraction ints engine needs to know where the charges sit ...
  // the nuclei are charges in this case; in QM/MM there will also be classical charges
  if (obtype == Operator::nuclear) {
    std::vector<std::pair<double,std::array<double,3>>> q;
    for(const auto& atom : atoms) {
      q.push_back( {static_cast<double>(atom.atomic_number), {{atom.x, atom.y, atom.z}}} );
    }
    engine.set_params(q);
  }

  auto shell2bf = map_shell_to_basis_function(shells);

  // buf[0] points to the target shell set after every call  to engine.compute()
  const auto& buf = engine.results();

  // loop over unique shell pairs, {s1,s2} such that s1 >= s2
  // this is due to the permutational symmetry of the real integrals over Hermitian operators: (1|2) = (2|1)
  for(auto s1=0; s1!=shells.size(); ++s1) {

    auto bf1 = shell2bf[s1]; // first basis function in this shell
    auto n1 = shells[s1].size();

    for(auto s2=0; s2<=s1; ++s2) {

      auto bf2 = shell2bf[s2];
      auto n2 = shells[s2].size();

      // compute shell pair
      engine.compute(shells[s1], shells[s2]);
      if (buf[0] == nullptr)
        continue;  // if all integrals screened out, skip to next quartet

      // "map" buffer to a const Eigen Matrix, and copy it to the corresponding blocks of the result
      Eigen::Map<const Matrix> buf_mat(buf[0], n1, n2);
      result.block(bf1, bf2, n1, n2) = buf_mat;
      if (s1 != s2) // if s1 >= s2, copy {s1,s2} to the corresponding {s2,s1} block, note the transpose!
      result.block(bf2, bf1, n2, n1) = buf_mat.transpose();

    }
  }

  return result;
}

Matrix compute_2body_fock(const std::vector<libint2::Shell>& shells,
                          const Matrix& D) {
  using libint2::Shell;
  using libint2::Engine;
  using libint2::Operator;

  auto time_elapsed = std::chrono::duration<double>::zero();

  const auto n = nbasis(shells);
  auto shell2bf = map_shell_to_basis_function(shells);
  Matrix Gtotal = Matrix::Zero(n,n);

#pragma omp parallel
// or try this if you do not want all variables shared by default
//#pragma omp parallel default(none) shared(shells,shell2bf,D,Gtotal,time_elapsed)
  {
    const int tid = omp_get_thread_num();
    const int nthread = omp_get_num_threads();
    long count = 0;

    // construct the 2-electron repulsion integrals engine
    Engine engine(Operator::coulomb, max_nprim(shells), max_l(shells), 0);
    Matrix G = Matrix::Zero(n,n);

    // buf[0] points to the target shell set after every call  to engine.compute()
    const auto &buf = engine.results();

    // The problem with the simple Fock builder is that permutational symmetries of the Fock,
    // density, and two-electron integrals are not taken into account to reduce the cost.
    // To make the simple Fock builder efficient we must rearrange our computation.
    // The most expensive step in Fock matrix construction is the evaluation of 2-e integrals;
    // hence we must minimize the number of computed integrals by taking advantage of their permutational
    // symmetry. Due to the multiplicative and Hermitian nature of the Coulomb kernel (and realness
    // of the Gaussians) the permutational symmetry of the 2-e ints is given by the following relations:
    //
    // (12|34) = (21|34) = (12|43) = (21|43) = (34|12) = (43|12) = (34|21) = (43|21)
    //
    // (here we use chemists' notation for the integrals, i.e in (ab|cd) a and b correspond to
    // electron 1, and c and d -- to electron 2).
    //
    // It is easy to verify that the following set of nested loops produces a permutationally-unique
    // set of integrals:
    // foreach a = 0 .. n-1
    //   foreach b = 0 .. a
    //     foreach c = 0 .. a
    //       foreach d = 0 .. (a == c ? b : c)
    //         compute (ab|cd)
    //
    // The only complication is that we must compute integrals over shells. But it's not that complicated ...
    //
    // The real trick is figuring out to which matrix elements of the Fock matrix each permutationally-unique
    // (ab|cd) contributes. STOP READING and try to figure it out yourself. (to check your answer see below)

    // loop over permutatinally-unique set of shells
    for (auto s1 = 0; s1 != shells.size(); ++s1) {

      auto bf1_first = shell2bf[s1]; // first basis function in this shell
      auto n1 = shells[s1].size();   // number of basis functions in this shell

      for (auto s2 = 0; s2 <= s1; ++s2) {

        auto bf2_first = shell2bf[s2];
        auto n2 = shells[s2].size();

        for (auto s3 = 0; s3 <= s1; ++s3) {

          auto bf3_first = shell2bf[s3];
          auto n3 = shells[s3].size();

          const auto s4_max = (s1 == s3) ? s2 : s3;
          for (auto s4 = 0; s4 <= s4_max; ++s4, ++count) {

            if (tid == (count%nthread)) {
              auto bf4_first = shell2bf[s4];
              auto n4 = shells[s4].size();

              // compute the permutational degeneracy (i.e. # of equivalents) of the given shell set
              auto s12_deg = (s1 == s2) ? 1.0 : 2.0;
              auto s34_deg = (s3 == s4) ? 1.0 : 2.0;
              auto s12_34_deg = (s1 == s3) ? (s2 == s4 ? 1.0 : 2.0) : 2.0;
              auto s1234_deg = s12_deg * s34_deg * s12_34_deg;

              const auto tstart = std::chrono::high_resolution_clock::now();

              engine.compute(shells[s1], shells[s2], shells[s3], shells[s4]);
              const auto *buf_1234 = buf[0];
              if (buf_1234 == nullptr)
                continue; // if all integrals screened out, skip to next quartet

              const auto tstop = std::chrono::high_resolution_clock::now();
              time_elapsed += tstop - tstart;

              // ANSWER
              // 1) each shell set of integrals contributes up to 6 shell sets of the Fock matrix:
              //    F(a,b) += (ab|cd) * D(c,d)
              //    F(c,d) += (ab|cd) * D(a,b)
              //    F(b,d) -= 1/4 * (ab|cd) * D(a,c)
              //    F(b,c) -= 1/4 * (ab|cd) * D(a,d)
              //    F(a,c) -= 1/4 * (ab|cd) * D(b,d)
              //    F(a,d) -= 1/4 * (ab|cd) * D(b,c)
              // 2) each permutationally-unique integral (shell set) must be scaled by its degeneracy,
              //    i.e. the number of the integrals/sets equivalent to it
              // 3) the end result must be symmetrized
              for (auto f1 = 0, f1234 = 0; f1 != n1; ++f1) {
                const auto bf1 = f1 + bf1_first;
                for (auto f2 = 0; f2 != n2; ++f2) {
                  const auto bf2 = f2 + bf2_first;
                  for (auto f3 = 0; f3 != n3; ++f3) {
                    const auto bf3 = f3 + bf3_first;
                    for (auto f4 = 0; f4 != n4; ++f4, ++f1234) {
                      const auto bf4 = f4 + bf4_first;

                      const auto value = buf_1234[f1234];

                      const auto value_scal_by_deg = value * s1234_deg;

                      G(bf1, bf2) += D(bf3, bf4) * value_scal_by_deg;
                      G(bf3, bf4) += D(bf1, bf2) * value_scal_by_deg;
                      G(bf1, bf3) -= 0.25 * D(bf2, bf4) * value_scal_by_deg;
                      G(bf2, bf4) -= 0.25 * D(bf1, bf3) * value_scal_by_deg;
                      G(bf1, bf4) -= 0.25 * D(bf2, bf3) * value_scal_by_deg;
                      G(bf2, bf3) -= 0.25 * D(bf1, bf4) * value_scal_by_deg;
                    }
                  }
                }
              }
            }

          }
        }
      }
    }

#pragma omp critical
    { Gtotal += G; }

  } // OMP parallel section

  // symmetrize the result and return
  Matrix Gtotal_t = Gtotal.transpose();
  return 0.5 * (Gtotal + Gtotal_t);
}


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <sophus/se3.hpp>
#include <chrono>

using namespace std;
using namespace cv;

// void find_feature_matches(
//   const Mat &img_1, const Mat &img_2,
//   std::vector<KeyPoint> &keypoints_1,
//   std::vector<KeyPoint> &keypoints_2,
//   std::vector<DMatch> &matches);

Point2d pixel2cam(const Point2d &p, const Mat &K) {
  return Point2d
    (
      (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
      (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
    );
}

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches) {
  Mat descriptors_1, descriptors_2;
  // used in OpenCV3
  Ptr<FeatureDetector> detector = ORB::create();
  Ptr<DescriptorExtractor> descriptor = ORB::create();
  // use this if you are in OpenCV2
  // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
  // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
  detector->detect(img_1, keypoints_1);
  detector->detect(img_2, keypoints_2);

  descriptor->compute(img_1, keypoints_1, descriptors_1);
  descriptor->compute(img_2, keypoints_2, descriptors_2);

  vector<DMatch> match;
  // BFMatcher matcher ( NORM_HAMMING );
  matcher->match(descriptors_1, descriptors_2, match);

  double min_dist = 10000, max_dist = 0;

  for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
    if (dist < min_dist) min_dist = dist;
    if (dist > max_dist) max_dist = dist;
  }

  printf("-- Max dist : %f \n", max_dist);
  printf("-- Min dist : %f \n", min_dist);

  for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 30.0)) {
      matches.push_back(match[i]);
    }
  }
}

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

struct ProjectionError
{
  ProjectionError(const Eigen::Vector2d& measurement, const Eigen::Vector3d& point,
                  const Eigen::Matrix3d& K) : _x(measurement), _X(point), _K(K)
    {}

  template <class T>
  bool operator() (const T* const params, T* residuals) const
  {
    T X[3] = {T(_X.x()), T(_X.y()), T(_X.z())};
    T rot_X[3];
    ceres::UnitQuaternionRotatePoint(params, X, rot_X);
    rot_X[0] += params[4];
    rot_X[1] += params[5];
    rot_X[2] += params[6];
    double fx = _K(0, 0);
    double fy = _K(1, 1);
    double cx = _K(0, 2);
    double cy = _K(1, 2);

    // T fx = T(_K(0, 0));
    // T fy = T(_K(1, 1));
    // T cx = T(_K(0, 2));
    // T cy = T(_K(1, 2));

    residuals[0] = _x[0] - ((rot_X[0] / rot_X[2]) * fx + cx); // parentheses have importance but do not known why
    residuals[1] = _x[1] - ((rot_X[1] / rot_X[2]) * fy + cy);

    // T rot[9];
    // ceres::QuaternionToRotation(params, rot);
    // Eigen::Matrix<T, 3, 3> R;
    // R << rot[0], rot[1], rot[2],
    //      rot[3], rot[4], rot[5],
    //      rot[6], rot[7], rot[8];

    // Eigen::Matrix<T, 3, 1> p(T(_X.x()), T(_X.y()), T(_X.z()));
    // Eigen::Matrix<T, 3, 1> t(params[4], params[5], params[6]);

    // Eigen::Matrix<T, 3, 3> K;
    // K << T(_K(0, 0)),  T(_K(0, 1)),  T(_K(0, 2)),
    //      T(_K(1, 0)),  T(_K(1, 1)),  T(_K(1, 2)),
    //      T(_K(2, 0)),  T(_K(2, 1)),  T(_K(2, 2));
    // Eigen::Matrix<T, 3, 1> p2 = K * (R * p + t);

    // residuals[0] = _x[0] - p2.x()/p2.z();
    // residuals[1] = _x[1] - p2.y()/p2.z();

    return true;
  }

  private:
    Eigen::Vector2d _x;
    Eigen::Vector3d _X;
    Eigen::Matrix3d _K;
};

void pose_refinement_ceres(const VecVector3d& points_3d, const VecVector2d& points_2d, const Eigen::Matrix3d& K, Sophus::SE3d& pose)
{
  Eigen::Vector3d trans = pose.translation();
  Eigen::Quaterniond quat = pose.so3().unit_quaternion();

  double pose_params[7] = {
    quat.w(), quat.x(), quat.y(), quat.z(), trans.x(), trans.y(), trans.z()
  };

  ceres::Problem problem;
  for (int i = 0; i < points_3d.size(); ++i)
  {
    problem.AddResidualBlock(
      new ceres::AutoDiffCostFunction<ProjectionError, 2, 7>(
      // new ceres::NumericDiffCostFunction<ProjectionError, ceres::CENTRAL, 2, 7>(
        new ProjectionError(points_2d[i], points_3d[i], K)
      ),
      nullptr,
      pose_params
    );
  }
  auto *quat_t_parameterization = new ceres::ProductParameterization(new ceres::QuaternionParameterization(),
                                                                     new ceres::IdentityParameterization(3));
  problem.SetParameterization(pose_params, quat_t_parameterization);

  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
  options.minimizer_progress_to_stdout = false;

  ceres::Solver::Summary summary;
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  ceres::Solve(options, &problem, &summary);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();

  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double >>(t2 - t1);
  cout << "optimization with ceres costs time: " << time_used.count() << " seconds." << endl;
  std::cout << summary.BriefReport() << std::endl;

  Eigen::Quaterniond q(pose_params[0], pose_params[1], pose_params[2], pose_params[3]);
  Eigen::Vector3d t(pose_params[4], pose_params[5], pose_params[6]);

  pose = Sophus::SE3d(q.toRotationMatrix(), t);
}

int main(int argc, char **argv) {
  // if (argc != 5) {
  //   cout << "usage: pose_estimation_3d2d img1 img2 depth1 depth2" << endl;
  //   return 1;
  // }
  string f1 = "../1.png"; //argv[1];
  string f2 = "../2.png"; //argv[2];
  string f3 = "../1_depth.png"; //argv[3];
  Mat img_1 = imread(f1, CV_LOAD_IMAGE_COLOR);
  Mat img_2 = imread(f2, CV_LOAD_IMAGE_COLOR);
  assert(img_1.data && img_2.data && "Can not load images!");

  vector<KeyPoint> keypoints_1, keypoints_2;
  vector<DMatch> matches;
  find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
  cout << "一共找到了" << matches.size() << "组匹配点" << endl;

  Mat d1 = imread(f3, CV_LOAD_IMAGE_UNCHANGED);
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
  vector<Point3f> pts_3d;
  vector<Point2f> pts_2d;
  for (DMatch m:matches) {
    ushort d = d1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
    if (d == 0)   // bad depth
      continue;
    float dd = d / 5000.0;
    Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
    pts_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd));
    pts_2d.push_back(keypoints_2[m.trainIdx].pt);
  }

  cout << "3d-2d pairs: " << pts_3d.size() << endl;

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  Mat r, t;
  solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false);
  Mat R;
  cv::Rodrigues(r, R);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve pnp in opencv cost time: " << time_used.count() << " seconds." << endl;

  cout << "R=" << endl << R << endl;
  cout << "t=" << endl << t << endl;

  VecVector3d pts_3d_eigen;
  VecVector2d pts_2d_eigen;
  for (size_t i = 0; i < pts_3d.size(); ++i) {
    pts_3d_eigen.push_back(Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z));
    pts_2d_eigen.push_back(Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y));
  }
  Eigen::Matrix3d K_eigen;
  K_eigen << 520.9, 0, 325.1,
             0, 521.0, 249.7,
             0, 0, 1;

  // Ceres
  cout << "Custom Ceres" << endl;
  Sophus::SE3d pose_ceres;
  pose_refinement_ceres(pts_3d_eigen, pts_2d_eigen, K_eigen, pose_ceres);
  Eigen::Vector3d t_ceres = pose_ceres.translation();
  Eigen::Matrix3d R_ceres = pose_ceres.so3().unit_quaternion().toRotationMatrix();
  cout << "R_ceres = " << endl << R_ceres << endl;
  cout << "t_ceres = " << endl << t_ceres << endl;

  return 0;
}


#include <alglib/ap.h>
#include <alglib/interpolation.h>

#include "spline-1d.hpp"

#define This Spline1d

namespace perceive
{
// ----------------------------------------------------------- inclusive-between

// inclusive_between<int>(5, x, 8);
template<typename T>
inline bool
inclusive_between(const T& low_bound, const T& value, const T& high_bound)
{
   return value <= high_bound && value >= low_bound;
}

// ----------------------------------------------------------------------- Pimpl

class This::Pimpl
{
 public:
   Pimpl()
       : is_init(false)
       , n_control_points(0)
       , approx_length(dNAN)
   {}

   bool is_init;
   uint n_control_points;
   double approx_length;
   alglib::spline1dinterpolant spline_x;
   alglib::spline1dfitreport rep_x;

   double min_x{0.0};
   double max_x{1.0};
   double range_inv{1.0}; // 1.0 / (max_x - min_x)

   void update_range_inv() { range_inv = 1.0 / (max_x - min_x); }
};

// ---------------------------------------------------------------- Construction

This::Spline1d()
    : pimpl(make_unique<Pimpl>())
{}

This::Spline1d(const Spline1d& rhs)
    : Spline1d()
{
   *this = rhs;
}

This::Spline1d(Spline1d&& rhs)
    : pimpl{nullptr}
{
   *this = std::move(rhs);
}

This::Spline1d(const std::vector<double>& ps, double rho, int M_factor)
    : Spline1d()
{
   init(ps, rho, M_factor);
}
This::Spline1d(const std::vector<float>& ps, double rho, int M_factor)
    : Spline1d()
{
   init(ps, rho, M_factor);
}
This::Spline1d(const std::vector<Vector2>& ps, double rho, int M_factor)
    : Spline1d()
{
   init(ps, rho, M_factor);
}
This::Spline1d(const std::deque<double>& ps, double rho, int M_factor)
    : Spline1d()
{
   init(ps, rho, M_factor);
}
This::Spline1d(const std::deque<float>& ps, double rho, int M_factor)
    : Spline1d()
{
   init(ps, rho, M_factor);
}
This::Spline1d(const std::deque<Vector2>& ps, double rho, int M_factor)
    : Spline1d()
{
   init(ps, rho, M_factor);
}

This::Spline1d(const std::vector<array<double, 6>>& coefficients)
    : Spline1d()
{
   init(coefficients);
}

This::~Spline1d() = default;

Spline1d& This::operator=(const Spline1d& rhs)
{
   if(this != &rhs) *pimpl = *rhs.pimpl;
   return *this;
}

Spline1d& This::operator=(Spline1d&& rhs) = default;

// ------------------------------------------------------------------- Accessors

bool This::is_init() const { return pimpl->is_init; }
uint This::n_control_points() const { return pimpl->n_control_points; }
double This::approx_length() const { return pimpl->approx_length; }

double This::rms_error() const { return pimpl->rep_x.rmserror; }
double This::avg_error() const { return pimpl->rep_x.avgerror; }
double This::avg_rel_error() const { return pimpl->rep_x.avgrelerror; }
double This::max_error() const { return pimpl->rep_x.maxerror; }

real This::min_x() const { return pimpl->min_x; }
real This::max_x() const { return pimpl->max_x; }
void This::set_min_x(real x)
{
   pimpl->min_x = x;
   pimpl->update_range_inv();
}
void This::set_max_x(real x)
{
   pimpl->max_x = x;
   pimpl->update_range_inv();
}

// -------------------------------------------------------------------- p-spline

/*************************************************************************
Fitting by penalized cubic spline.

Equidistant grid with M nodes on [min(x,xc),max(x,xc)] is  used  to  build
basis functions. Basis functions are cubic splines with  natural  boundary
conditions. Problem is regularized by  adding non-linearity penalty to the
usual least squares penalty function:

    S(x) = arg min { LS + P }, where
    LS   = SUM { w[i]^2*(y[i] - S(x[i]))^2 } - least squares penalty
    P    = C*10^rho*integral{ S''(x)^2*dx } - non-linearity penalty
    rho  - tunable constant given by user
    C    - automatically determined scale parameter,
           makes penalty invariant with respect to scaling of X, Y, W.

INPUT PARAMETERS:
    X   -   points, array[0..N-1].
    Y   -   function values, array[0..N-1].
    N   -   number of points (optional):
            * N>0
            * if given, only first N elements of X/Y are processed
            * if not given, automatically determined from X/Y sizes
    M   -   number of basis functions ( = number_of_nodes), M>=4.
    Rho -   regularization  constant  passed   by   user.   It   penalizes
            nonlinearity in the regression spline. It  is  logarithmically
            scaled,  i.e.  actual  value  of  regularization  constant  is
            calculated as 10^Rho. It is automatically scaled so that:
            * Rho=2.0 corresponds to moderate amount of nonlinearity
            * generally, it should be somewhere in the [-8.0,+8.0]
            If you do not want to penalize nonlineary,
            pass small Rho. Values as low as -15 should work.

OUTPUT PARAMETERS:
    Info-   same format as in LSFitLinearWC() subroutine.
            * Info>0    task is solved
            * Info<=0   an error occured:
                        -4 means inconvergence of internal SVD or
                           Cholesky decomposition; problem may be
                           too ill-conditioned (very rare)
    S   -   spline interpolant.
    Rep -   Following fields are set:
            * RMSError      rms error on the (X,Y).
            * AvgError      average error on the (X,Y).
            * AvgRelError   average relative error on the non-zero Y
            * MaxError      maximum error
                            NON-WEIGHTED ERRORS ARE CALCULATED

IMPORTANT:
    this subroitine doesn't calculate task's condition number for K<>0.

NOTE 1: additional nodes are added to the spline outside  of  the  fitting
interval to force linearity when x<min(x,xc) or x>max(x,xc).  It  is  done
for consistency - we penalize non-linearity  at [min(x,xc),max(x,xc)],  so
it is natural to force linearity outside of this interval.

NOTE 2: function automatically sorts points,  so  caller may pass unsorted
array.

  -- ALGLIB PROJECT --
     Copyright 18.08.2009 by Bochkanov Sergey
*************************************************************************/

template<typename U>
bool init_pspline(Spline1d& curve,
                  const U& container,
                  double rho,
                  unsigned M_factor)
{
   curve.pimpl->n_control_points = 0;
   curve.pimpl->is_init          = false;
   curve.pimpl->approx_length    = dNAN;

   const uint32_t len = uint32_t(container.size());
   if(len < 5) return false;

   alglib::real_1d_array T;
   alglib::real_1d_array X;
   T.setlength(len);
   X.setlength(len);

   alglib::ae_int_t counter = 0;
   double t                 = 0.0;
   double dt                = 1.0 / double(len - 1);
   for(const auto& p : container) {
      T[counter] = t;
      X[counter] = real(p);
      t += dt;
      counter++;
   }

   alglib::ae_int_t info_x;
   alglib::ae_int_t M = std::max<int>(4, int(M_factor));

   alglib::spline1dfitpenalized(
       T, X, M, rho, info_x, curve.pimpl->spline_x, curve.pimpl->rep_x);

   curve.pimpl->n_control_points = unsigned(M);

   if(info_x > 0) {
      curve.pimpl->is_init       = true;
      curve.pimpl->approx_length = 0.0;
      double dt                  = 0.1 / double(M - 1);
      real p                     = curve.evaluate_t(0.0);
      for(double t = dt; t <= 1.0; t += dt) {
         real q = curve.evaluate_t(t);
         curve.pimpl->approx_length += fabs(p - q);
         p = q;
      }
   }

   return curve.pimpl->is_init;
}

template<typename U>
bool init_pspline_xy(Spline1d& curve,
                     const U& container,
                     double rho,
                     unsigned M_factor)
{
   curve.pimpl->n_control_points = 0;
   curve.pimpl->is_init          = false;
   curve.pimpl->approx_length    = dNAN;

   const unsigned len = unsigned(container.size());
   if(len < 5) return false;

   alglib::real_1d_array X;
   alglib::real_1d_array Y;
   X.setlength(len);
   Y.setlength(len);

   double min_x = std::numeric_limits<double>::max();
   double max_x = std::numeric_limits<double>::lowest();

   uint counter = 0;
   for(const auto& p : container) {
      X[counter] = p.x;
      Y[counter] = p.y;
      if(p.x < min_x) min_x = p.x;
      if(p.x > max_x) max_x = p.x;
      counter++;
   }

   double range_inv = 1.0 / (max_x - min_x);

   // Now scale the 'X' values
   for(unsigned i = 0; i < len; ++i) X[i] = (X[i] - min_x) * range_inv;

   alglib::ae_int_t info_x;
   alglib::ae_int_t M = std::max<int>(4, int(M_factor));

   alglib::spline1dfitpenalized(
       X, Y, M, rho, info_x, curve.pimpl->spline_x, curve.pimpl->rep_x);

   curve.pimpl->n_control_points = unsigned(M);

   if(info_x > 0) {
      curve.pimpl->is_init       = true;
      curve.pimpl->approx_length = 0.0;
      curve.set_min_x(min_x);
      curve.set_max_x(max_x);
   }

   return curve.pimpl->is_init;
}

// ------------------------------------------------------------------------ Init

bool This::init(const std::vector<double>& ps, double rho, int M_factor)
{
   return init_pspline(*this, ps, rho, unsigned(M_factor));
}

bool This::init(const std::deque<double>& ps, double rho, int M_factor)
{
   return init_pspline(*this, ps, rho, unsigned(M_factor));
}

bool This::init(const std::vector<float>& ps, double rho, int M_factor)
{
   return init_pspline(*this, ps, rho, unsigned(M_factor));
}

bool This::init(const std::deque<float>& ps, double rho, int M_factor)
{
   return init_pspline(*this, ps, rho, unsigned(M_factor));
}

bool This::init(const std::vector<Vector2>& ps, double rho, int M_factor)
{
   return init_pspline_xy(*this, ps, rho, unsigned(M_factor));
}

bool This::init(const std::deque<Vector2>& ps, double rho, int M_factor)
{
   return init_pspline_xy(*this, ps, rho, unsigned(M_factor));
}

// -------------------------------------------------------------------- Evaluate

real This::evaluate_t(double t) const
{
   // assert(t >= 0.0 && t <= 1.0);
   return is_init() ? real(alglib::spline1dcalc(pimpl->spline_x, t))
                    : real(NAN);
}

real This::evaluate(double x) const
{
   // assert(min-x >= 0.0 && max-x <= 1.0);
   // cout << format("t = {}", (x - pimpl->min_x) * pimpl->range_inv) << endl;
   return evaluate_t((x - pimpl->min_x) * pimpl->range_inv);
}

// ---------------------------------------------------------------------- Unpack

std::vector<array<double, 6>> This::unpack() const
{
   std::vector<array<double, 6>> out;
   unpack(out);
   return out;
}

void This::unpack(std::vector<array<double, 6>>& out) const
{
   alglib::ae_int_t nx{0};
   alglib::real_2d_array tbl_x;
   spline1dunpack(pimpl->spline_x, nx, tbl_x);

   size_t N = size_t(tbl_x.rows());

   out.resize(N + 1);
   for(size_t ind = 0; ind < N; ++ind) {
      auto& coeff = out[size_t(ind)];
      auto ptr    = &coeff[0];
      for(unsigned i = 0; i < 6; ++i) *ptr++ = tbl_x(alglib::ae_int_t(ind), i);
   }

   std::fill(out[N].begin(), out[N].end(), 0.0);
   out[N][0] = pimpl->min_x;
   out[N][1] = pimpl->max_x;
}

void This::init(const std::vector<array<double, 6>>& coefficients)
{
   using alglib::spline1dinterpolant;
   auto make_interpolant = [&](spline1dinterpolant& interpolant) {
      const unsigned n = unsigned(coefficients.size());
      alglib::real_1d_array x, y, d; // (x, f(x), f'(x))
      x.setlength(n);
      y.setlength(n);
      d.setlength(n);
      unsigned pos = 0; // write position

      auto write = [&](double t,
                       double X0,
                       double X1,
                       double C0,
                       double C1,
                       double C2,
                       double C3) {
         x(pos) = X0 + t * (X1 - X0);
         y(pos) = C0 + C1 * t + C2 * t * t + C3 * t * t * t;
         d(pos) = C1 + 2.0 * C2 * t + 3.0 * C3 * t * t;
         pos++;
      };

      for(unsigned ind = 0; ind < coefficients.size() - 1; ++ind) {
         const auto& CC = coefficients[ind];
         auto X0        = CC[0];
         auto X1        = CC[1];
         auto C0        = CC[2];
         auto C1        = CC[3];
         auto C2        = CC[4];
         auto C3        = CC[5];
         write(0.0, X0, X1, C0, C1, C2, C3);

         // Write that final coefficient
         if(ind == coefficients.size() - 2) write(1.0, X0, X1, C0, C1, C2, C3);
      }

      if(pos != n) { FATAL(format("pos = {} != {} = n", pos, n)); }

      alglib::spline1dbuildhermite(x, y, d, interpolant);
   };

   make_interpolant(pimpl->spline_x);

   pimpl->is_init           = true;
   pimpl->n_control_points  = unsigned(coefficients.size());
   pimpl->rep_x.rmserror    = 0.0;
   pimpl->rep_x.avgerror    = 0.0;
   pimpl->rep_x.maxerror    = 0.0;
   pimpl->rep_x.avgrelerror = 0.0;
   pimpl->min_x             = coefficients.back()[0];
   set_max_x(coefficients.back()[1]);
}

// ----------------------------------------------------------------- Interpolate

void This::interpolate_t(unsigned n,
                         std::vector<real>& out,
                         double min_t,
                         double max_t)
{
   assert(min_t >= 0.0 && min_t <= 1.0);
   assert(max_t >= 0.0 && max_t <= 1.0);
   assert(min_t <= max_t);

   out.reserve(n);

   if(n == 0)
      return;
   else if(n == 1)
      out.push_back(evaluate_t(max_t));
   else {
      double t  = min_t;
      double dt = (max_t - min_t) / double(n - 1);
      for(unsigned i = 0; i < n - 1; ++i) {
         out.push_back(evaluate_t(clamp(t, min_t, max_t)));
         t += dt;
      }
      out.push_back(evaluate_t(max_t));
   }

   assert(out.size() == n);
}

// ---------------------------------------------------------------------- Smooth

bool This::smooth(std::vector<real>& path, double rho, int M_factor)
{
   if(path.size() < 5) return false;
   Spline1d spline;
   if(!spline.init(path, rho, M_factor)) return false;
   std::vector<real> t;
   spline.interpolate_t(unsigned(path.size()), t);
   if(!(t.size() == path.size()))
      FATAL(format("Container size mismatch: expected {}, but got {}",
                   path.size(),
                   t.size()));
   auto src = t.begin();
   auto dst = path.begin();
   while(src != t.end()) *src++ = *dst++;
   return true;
}

} // namespace perceive