/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu> Licensed
 * under the MIT license. See the license file LICENSE.
 */

#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include <string>
#include <pcl/io/ply_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/point_cloud_color_handlers.h>
#include <pcl/common/transforms.h>

#include <cuda_runtime.h>
#include <nvidia/helper_cuda.h>
#include <jsCore/global.hpp>

//#include "rtDDPvMF/rtDDPvMF.hpp"
//#include "rtDDPvMF/realtimeDDPvMF_openni.hpp"
#include "bbTrans/lower_bound_R3.h"
#include "bbTrans/upper_bound_indep_R3.h"
#include "bbTrans/upper_bound_convex_R3.h"
#include "bbTrans/lower_bound_S3.h"
#include "bbTrans/upper_bound_indep_S3.h"
#include "bbTrans/upper_bound_convex_S3.h"
#include "bbTrans/lower_bound_Lin.h"
#include "bbTrans/upper_bound_Lin.h"
#include "bbTrans/node_TpS3.h"
#include "bbTrans/node_AA.h"
#include "bbTrans/branch_and_bound.h"
#include "bbTrans/vmf.h"
#include "bbTrans/vmf_mm.h"
#include "bbTrans/normal.h"
#include "dpOptTrans/dp_vmf_opt_rot.h"
#include "dpOptTrans/pc_helper.h"
#include "dpOptTrans/pcHelpers.h"
#include "dpMMlowVar/ddpmeansCUDA.hpp"
#include "dpMMlowVar/dpmeans.hpp"
#include "jsCore/timer.hpp"

#include <boost/program_options.hpp>
namespace po = boost::program_options;

template <typename M>
using eigen_vector = std::vector<M, Eigen::aligned_allocator<M>>;

//struct CfgRtDDPvMF
//{
//
//  CfgRtDDPvMF() : f_d(540.0), lambda(-1.), beta(1e5), Q(-2.),
//      nSkipFramesSave(30), nFramesSurvive_(0),lambdaDeg_(90.)
//  {};
//  CfgRtDDPvMF(const CfgRtDDPvMF& cfg)
//    : f_d(cfg.f_d), lambda(cfg.lambda), beta(cfg.beta), Q(cfg.Q),
//      nSkipFramesSave(cfg.nSkipFramesSave), nFramesSurvive_(cfg.nFramesSurvive_),
//      lambdaDeg_(cfg.lambdaDeg_)
//  {
//    pathOut = cfg.pathOut;
//  };
//
//  double f_d;
//  double lambda;
//  double beta;
//  double Q;
//
//  int32_t nSkipFramesSave;
//  std::string pathOut;
//
//  int32_t nFramesSurvive_;
//  double lambdaDeg_;
//
//  void lambdaFromDeg(double lambdaDeg)
//  {
//    lambdaDeg_ = lambdaDeg;
//    lambda = cos(lambdaDeg*M_PI/180.0)-1.;
//  };
//  void QfromFrames2Survive(int32_t nFramesSurvive)
//  {
//    nFramesSurvive_ = nFramesSurvive;
//    Q = nFramesSurvive == 0? -2. : lambda/double(nFramesSurvive);
//  };
//};
//
Eigen::Vector3d ComputePcMean(pcl::PointCloud<pcl::PointXYZRGBNormal>&
    pc) {
  // take 3 values (x,y,z of normal) with an offset of 4 values (x,y,z
  // and one float which is undefined) and the step is 12 (4 for xyz, 4
  // for normal xyz and 4 for curvature and rgb).
  auto xyz = pc.getMatrixXfMap(3, 12, 0); // this works for PointXYZRGBNormal
  Eigen::Vector3d mean =  Eigen::Vector3d::Zero();

  for (size_t i=0; i<xyz.cols(); ++i) {
    mean += xyz.col(i).cast<double>();
  }
  return  mean /= xyz.cols();
}

Eigen::Matrix3d ComputePcCov(pcl::PointCloud<pcl::PointXYZRGBNormal>&
    pc) {

  // take 3 values (x,y,z of normal) with an offset of 4 values (x,y,z
  // and one float which is undefined) and the step is 12 (4 for xyz, 4
  // for normal xyz and 4 for curvature and rgb).
  auto xyz = pc.getMatrixXfMap(3, 12, 0); // this works for PointXYZRGBNormal
  Eigen::Vector3d mean = ComputePcMean(pc);
  Eigen::Matrix3d S = Eigen::Matrix3d::Zero();
  for (size_t i=0; i<xyz.cols(); ++i) {
    S += (xyz.col(i).cast<double>()-mean)*(xyz.col(i).cast<double>()-mean).transpose();
  }
  return  S /= (xyz.cols()-1);
}

bool ComputevMFMMfromPC(const pcl::PointCloud<pcl::PointXYZRGBNormal>&
    pc, const CfgRtDDPvMF& cfg, std::vector<vMF<3>>& vmfs) {
  // take 3 values (x,y,z of normal) with an offset of 4 values (x,y,z
  // and one float which is undefined) and the step is 12 (4 for xyz, 4
  // for normal xyz and 4 for curvature and rgb).
  auto n_map = pc.getMatrixXfMap(3, 12, 4); // this works for PointXYZRGBNormal
  boost::shared_ptr<Eigen::MatrixXf> n(new Eigen::MatrixXf(n_map));
//  std::cout << "normals: " << std::endl << n->rows()
//    << "x" << n->cols() << std::endl;
  // Setup the DPvMF clusterer.
  shared_ptr<jsc::ClDataGpuf> cld(new jsc::ClDataGpuf(n,0));
  dplv::DDPMeansCUDA<float,dplv::Spherical<float>> pddpvmf(cld,
      cfg.lambda, cfg.Q, cfg.beta);
  // Run the clustering algorithm.
  pddpvmf.nextTimeStep(n);
  for(uint32_t i=0; i<10; ++i)
  {
//    cout<<"@"<<i<<" :"<<endl;
    pddpvmf.updateLabels();
    pddpvmf.updateCenters();
    if(pddpvmf.convergedCounts(n->cols()/100)) break;
  }
  pddpvmf.getZfromGpu(); // cache z_ back from gpu
  const VectorXu& z = pddpvmf.z();
  // Output results.
  MatrixXf centroids = pddpvmf.centroids();
  VectorXf counts = pddpvmf.counts().cast<float>();
//  std::cout << counts.transpose() << std::endl;
//  std::cout << centroids << std::endl;
  uint32_t K = counts.rows();
//  std::cout << "K: " << K << std::endl;
  // Compute vMF statistics: area-weighted sum over surface normals
  // associated with respective cluster.
  MatrixXd xSum = MatrixXd::Zero(3,K);
  VectorXf ws = Eigen::VectorXf::Zero(K);
  for (uint32_t i=0; i<n->cols(); ++i)
    if(z(i) < K) {
      float w = pc.at(i).curvature; // curvature is used to store the weights.
      xSum.col(z(i)) += n->col(i).cast<double>()*w;
      ws(z(i)) += w;
    }
  // Fractions belonging to each cluster.
  Eigen::VectorXd pis = (ws.array() / ws.sum()).matrix().cast<double>();
  Eigen::VectorXf taus(K);
  for(uint32_t k=0; k<K; ++k)
    if (counts(k) > 5) {
      taus(k) = bb::vMF<3>::MLEstimateTau(xSum.col(k),
          xSum.col(k).cast<double>()/xSum.col(k).norm(), ws(k));
    }

  for(uint32_t k=0; k<K; ++k)
    if (counts(k) > 5) {
      vmfs.push_back(bb::vMF<3>(xSum.col(k).cast<double>()/xSum.col(k).norm(),
            taus(k), pis(k)));
      vmfs.back().GetXSum() = xSum.col(k);
    }
  std::cout << "# vMF components after filter: " << vmfs.size() << std::endl;
  return true;
}

bool ComputeGMMfromPC(pcl::PointCloud<pcl::PointXYZRGBNormal>&
    pc, const CfgRtDDPvMF& cfg, std::vector<Normal<3>>& gmm,
    bool colorLables) {
  // take 3 values (x,y,z of normal) with an offset of 4 values (x,y,z
  // and one float which is undefined) and the step is 12 (4 for xyz, 4
  // for normal xyz and 4 for curvature and rgb).
  auto xyz_map = pc.getMatrixXfMap(3, 12, 0); // this works for PointXYZRGBNormal
  boost::shared_ptr<Eigen::MatrixXf> xyz(new Eigen::MatrixXf(xyz_map));
//  std::cout << "xyz: " << std::endl << xyz->rows()
//    << "x" << xyz->cols() << std::endl;

  // Setup the DPvMF clusterer.
  shared_ptr<jsc::ClDataGpuf> cld(new jsc::ClDataGpuf(xyz,0));
  dplv::DDPMeansCUDA<float,dplv::Euclidean<float> > dpmeans(cld,
      cfg.lambda, cfg.Q, cfg.beta);
  // Run the clustering algorithm.
  dpmeans.nextTimeStep(xyz);
  for(uint32_t i=0; i<10; ++i)
  {
//    cout<<"@"<<i<<" :"<<endl;
    dpmeans.updateLabels();
    dpmeans.updateCenters();
    if(dpmeans.convergedCounts(xyz->cols()/100)) break;
  }
  dpmeans.getZfromGpu(); // cache z_ back from gpu
  const VectorXu& z = dpmeans.z();
  // Output results.
  MatrixXf centroids = dpmeans.centroids();
  VectorXf counts = dpmeans.counts().cast<float>();
//  std::cout << counts.transpose() << std::endl;
//  std::cout << centroids << std::endl;
  uint32_t K = counts.rows();
  std::cout << "lambda=" << cfg.lambda << " K=" << K << std::endl;
  // Compute Gaussian statistics:
  std::vector<MatrixXd> Ss(K,Matrix3d::Zero());
  Eigen::MatrixXd xSum = Eigen::MatrixXd::Zero(3,K);
  Eigen::VectorXf ws = Eigen::VectorXf::Zero(K);
  for (uint32_t i=0; i<xyz->cols(); ++i)
    if(z(i) < K) {
      float w = pc.at(i).curvature; // curvature is used to store the weights.
      ws(z(i)) += w;
      xSum.col(z(i)) += xyz->col(i).cast<double>()*w;
    }
  for(uint32_t k=0; k<K; ++k)
    centroids.col(k) = xSum.col(k).cast<float>()/ws(k);

  for (uint32_t i=0; i<xyz->cols(); ++i)
    if(z(i) < K) {
      float w = pc.at(i).curvature; // curvature is used to store the weights.
      Ss[z(i)] += w*(xyz->col(i) - centroids.col(z(i))).cast<double>()
        * (xyz->col(i) - centroids.col(z(i))).cast<double>().transpose();
    }
  if (colorLables) {
    for (uint32_t i=0; i<xyz->cols(); ++i)
      if(z(i) < K) {
        uint8_t r = floor(float(z(i))/float(K-1)*255.);
        uint8_t g = floor(float(K-1-z(i))/float(K-1)*255.);
        uint8_t b = floor(float(z(i)<=(K-1)/2? z(i)+(K-1)/2 :
              z(i)-(K-1)/2-1)/float(K-1)*255.);
        pc.at(i).rgb = ((int)r) << 16 | ((int)g) << 8 | ((int)b);
      }
  }
  // Fractions belonging to each cluster.
  Eigen::VectorXd pis = (ws.array() / ws.sum()).matrix().cast<double>();

  const double maxEvFactor = 1e-2;
  for(uint32_t k=0; k<K; ++k)
//    if (pis(k) > 0.) {
    if (counts(k) > 100) {
      Matrix3d cov = Ss[k]/float(ws(k));
      Eigen::SelfAdjointEigenSolver<Matrix3d> eig(cov);
      Eigen::Vector3d e = eig.eigenvalues();
      uint32_t iMax = 0;
      double eMax = e.maxCoeff(&iMax);
//      std::cout << cov << std::endl;
      bool regularized = false;
      for (uint32_t i=0; i<3; ++i)
        if (i!=iMax && eMax*maxEvFactor > e(i)) {
          std::cout << "small eigenvalue: " << e(i) << " replaced by "
            << eMax*maxEvFactor << std::endl;
          e(i) = eMax*maxEvFactor;
          regularized = true;
        }
      if (regularized) {
        Eigen::Matrix3d V = eig.eigenvectors();
        cov = V*e.asDiagonal()*V.inverse();
      }
//      std::cout << cov << std::endl;
      gmm.push_back(bb::Normal<3>(centroids.col(k).cast<double>(),
            cov, pis(k)));
//        Ss[k]/float(ws(k)), pis(k)));
    }
  std::cout << "# GMM components after filter: " << gmm.size() << std::endl;
  return true;
}

void DisplayPcs(const pcl::PointCloud<pcl::PointXYZRGBNormal>& pcA,
  const pcl::PointCloud<pcl::PointXYZRGBNormal>& pcB,
  const std::vector<bb::Normal<3>>& gmmA,
  const std::vector<bb::Normal<3>>& gmmB,
  const std::vector<bb::vMF<3>>& vmfsA,
  const std::vector<bb::vMF<3>>& vmfsB,
  const Eigen::Quaterniond& q_star, const Eigen::Vector3d& t_star,
  float scale) {

    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcA_ptr = pcA.makeShared();
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcB_ptr = pcB.makeShared();
    // Construct transformed point cloud.
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcB_T_ptr(new
        pcl::PointCloud<pcl::PointXYZRGBNormal>(pcB));
    for (uint32_t i=0; i<pcB_T_ptr->size(); ++i) {
      Eigen::Map<Eigen::Vector3f> p(&(pcB_T_ptr->at(i).x));
      p = q_star.cast<float>().inverse()._transformVector( p - t_star.cast<float>());
      Eigen::Map<Eigen::Vector3f> n(pcB_T_ptr->at(i).normal);
      n = q_star.cast<float>().inverse()._transformVector(n);
      pcB_T_ptr->at(i).rgb = ((int)128) << 16 | ((int)255) << 8 | ((int)128);
    }
    for (uint32_t i=0; i<pcA_ptr->size(); ++i) {
      pcA_ptr->at(i).rgb = ((int)255) << 16 | ((int)128) << 8 | ((int)128);
    }
    // Construct surface normal point clouds.
    pcl::PointCloud<pcl::PointXYZ>::Ptr nA_ptr =
      pcl::PointCloud<pcl::PointXYZ>::Ptr(new
          pcl::PointCloud<pcl::PointXYZ>(pcA.size(),1));
    nA_ptr->getMatrixXfMap(3,4,0) = pcA.getMatrixXfMap(3,12,4);
    pcl::PointCloud<pcl::PointXYZ>::Ptr nB_ptr =
      pcl::PointCloud<pcl::PointXYZ>::Ptr(new
          pcl::PointCloud<pcl::PointXYZ>(pcB.size(),1));
    nB_ptr->getMatrixXfMap(3,4,0) = pcB.getMatrixXfMap(3,12,4);
    pcl::PointCloud<pcl::PointXYZ>::Ptr nB_T_ptr =
      pcl::PointCloud<pcl::PointXYZ>::Ptr(new
          pcl::PointCloud<pcl::PointXYZ>(*nB_ptr));
    for (uint32_t i=0; i<nA_ptr->size(); ++i) {
      nA_ptr->at(i).z -=1.1;
    }
    for (uint32_t i=0; i<nB_T_ptr->size(); ++i) {
      Eigen::Map<Eigen::Vector3f> n(&(nB_T_ptr->at(i).x));
      n = q_star.cast<float>().inverse()._transformVector(n);
      n(2) += 1.1;
    }

    pcA_ptr->sensor_orientation_.setIdentity();
    pcB_ptr->sensor_orientation_.setIdentity();
    pcB_T_ptr->sensor_orientation_.setIdentity();
    nA_ptr->sensor_orientation_.setIdentity();
    nB_ptr->sensor_orientation_.setIdentity();
    nB_T_ptr->sensor_orientation_.setIdentity();

    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewerPc (new
        pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewerPc->initCameraParameters ();
    viewerPc->setBackgroundColor (1., 1., 1.);
//    viewerPc->addCoordinateSystem (scale);

    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>
      rgbA(pcA_ptr);
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>
      rgbB(pcB_ptr);
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>
      rgbB_T(pcB_T_ptr);
    viewerPc->addPointCloud<pcl::PointXYZRGBNormal> (pcA_ptr, rgbA, "cloudA");
//    viewerPc->addPointCloud<pcl::PointXYZRGBNormal> (pcB_ptr, rgbB, "cloudB",v1);
    viewerPc->addPointCloud<pcl::PointXYZRGBNormal> (pcB_T_ptr, rgbB_T,
        "cloudB transformed");
    char label[10];
    for(uint32_t k=0; k<gmmA.size(); ++k) {
      pcl::PointXYZ p;
      p.x = gmmA[k].GetMu()(0); p.y = gmmA[k].GetMu()(1); p.z =
        gmmA[k].GetMu()(2);
      sprintf(label,"SA%d",k);
      viewerPc->addSphere(p, scale, 1,0,0, label);
    }
    for(uint32_t k=0; k<gmmB.size(); ++k) {
      Eigen::Vector3d mu =
        q_star.inverse()._transformVector(gmmB[k].GetMu() -t_star);
      pcl::PointXYZ p;
      p.x = mu(0); p.y = mu(1); p.z = mu(2);
      sprintf(label,"SB%d",k);
      viewerPc->addSphere(p, scale,0,1,0, label);
    }

    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewerNc (new
        pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewerNc->initCameraParameters ();
    viewerNc->setBackgroundColor (0., 0., 0.);
    viewerNc->addCoordinateSystem (1.0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
      nA_color(nA_ptr, 255, 128, 128);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
      nB_color(nB_ptr, 128, 255, 128);
    viewerNc->addPointCloud<pcl::PointXYZ> (nA_ptr, nA_color,
        "normalsA");
//    viewerNc->addPointCloud<pcl::PointXYZRGBNormal> (pcB_ptr, rgbB, "cloudB",v1);
    viewerNc->addPointCloud<pcl::PointXYZ> (nB_T_ptr, nB_color,
        "normals B transformed");

    for(uint32_t k=0; k<vmfsA.size(); ++k) {
      pcl::PointXYZ p;
      p.x = vmfsA[k].GetMu()(0); p.y = vmfsA[k].GetMu()(1); p.z =
        vmfsA[k].GetMu()(2) -1.1;
      sprintf(label,"SvA%d",k);
      viewerNc->addSphere(p, 0.05, 1,0,0, label);
    }
    for(uint32_t k=0; k<vmfsB.size(); ++k) {
      Eigen::Vector3d mu =
        q_star.inverse()._transformVector(vmfsB[k].GetMu());
      pcl::PointXYZ p;
      p.x = mu(0); p.y = mu(1); p.z = mu(2)+1.1;
      sprintf(label,"SvB%d",k);
      viewerNc->addSphere(p, 0.05,0,1,0, label);
    }

    while (!viewerPc->wasStopped () && !viewerNc->wasStopped()) {
      viewerPc->spinOnce (50);
      viewerNc->spinOnce (50);
      boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }
}
//
//bool ComputevMFMMfromPC(const pcl::PointCloud<pcl::PointXYZRGBNormal>&
//    pc, const CfgRtDDPvMF& cfg, std::vector<bb::vMF<3>>& vmfs) {
//  // take 3 values (x,y,z of normal) with an offset of 4 values (x,y,z
//  // and one float which is undefined) and the step is 12 (4 for xyz, 4
//  // for normal xyz and 4 for curvature and rgb).
//  auto n_map = pc.getMatrixXfMap(3, 12, 4); // this works for PointXYZRGBNormal
//  boost::shared_ptr<Eigen::MatrixXf> n(new Eigen::MatrixXf(n_map));
////  std::cout << "normals: " << std::endl << n->rows()
////    << "x" << n->cols() << std::endl;
//  // Setup the DPvMF clusterer.
//  shared_ptr<jsc::ClDataGpuf> cld(new jsc::ClDataGpuf(n,0));
//  dplv::DDPMeansCUDA<float,dplv::Spherical<float> > pddpvmf(cld,
//      cfg.lambda, cfg.Q, cfg.beta);
//  // Run the clustering algorithm.
//  pddpvmf.nextTimeStep(n);
//  for(uint32_t i=0; i<10; ++i)
//  {
////    cout<<"@"<<i<<" :"<<endl;
//    pddpvmf.updateLabels();
//    pddpvmf.updateCenters();
//    if(pddpvmf.convergedCounts(n->cols()/100)) break;
//  }
//  pddpvmf.getZfromGpu(); // cache z_ back from gpu
//  const VectorXu& z = pddpvmf.z();
//  // Output results.
//  MatrixXf centroids = pddpvmf.centroids();
//  VectorXf counts = pddpvmf.counts().cast<float>();
////  std::cout << counts.transpose() << std::endl;
////  std::cout << centroids << std::endl;
//  uint32_t K = counts.rows();
////  std::cout << "K: " << K << std::endl;
//  // Compute vMF statistics: area-weighted sum over surface normals
//  // associated with respective cluster.
//  MatrixXd xSum = MatrixXd::Zero(3,K);
//  VectorXf ws = Eigen::VectorXf::Zero(K);
//  for (uint32_t i=0; i<n->cols(); ++i)
//    if(z(i) < K) {
//      float w = pc.at(i).curvature; // curvature is used to store the weights.
//      xSum.col(z(i)) += n->col(i).cast<double>()*w;
//      ws(z(i)) += w;
//    }
//  // Fractions belonging to each cluster.
//  Eigen::VectorXd pis = (ws.array() / ws.sum()).matrix().cast<double>();
//  Eigen::VectorXf taus(K);
//  for(uint32_t k=0; k<K; ++k)
//    if (counts(k) > 100) {
//      taus(k) = bb::vMF<3>::MLEstimateTau(xSum.col(k),
//          xSum.col(k).cast<double>()/xSum.col(k).norm(), ws(k));
//    }
//
//  for(uint32_t k=0; k<K; ++k)
//    if (counts(k) > 100) {
//      vmfs.push_back(bb::vMF<3>(xSum.col(k).cast<double>()/xSum.col(k).norm(),
//            taus(k), pis(k)));
//    }
//  return true;
//}
//
//bool ComputeGMMfromPC(pcl::PointCloud<pcl::PointXYZRGBNormal>&
//    pc, const CfgRtDDPvMF& cfg, std::vector<bb::Normal<3>>& gmm,
//    bool colorLables) {
//  // take 3 values (x,y,z of normal) with an offset of 4 values (x,y,z
//  // and one float which is undefined) and the step is 12 (4 for xyz, 4
//  // for normal xyz and 4 for curvature and rgb).
//  auto xyz_map = pc.getMatrixXfMap(3, 12, 0); // this works for PointXYZRGBNormal
//  boost::shared_ptr<Eigen::MatrixXf> xyz(new Eigen::MatrixXf(xyz_map));
////  std::cout << "xyz: " << std::endl << xyz->rows()
////    << "x" << xyz->cols() << std::endl;
//
//  // Setup the DPvMF clusterer.
//  shared_ptr<jsc::ClDataGpuf> cld(new jsc::ClDataGpuf(xyz,0));
//  dplv::DDPMeansCUDA<float,dplv::Euclidean<float> > dpmeans(cld,
//      cfg.lambda, cfg.Q, cfg.beta);
//  // Run the clustering algorithm.
//  dpmeans.nextTimeStep(xyz);
//  for(uint32_t i=0; i<10; ++i)
//  {
////    cout<<"@"<<i<<" :"<<endl;
//    dpmeans.updateLabels();
//    dpmeans.updateCenters();
//    if(dpmeans.convergedCounts(xyz->cols()/100)) break;
//  }
//  dpmeans.getZfromGpu(); // cache z_ back from gpu
//  const VectorXu& z = dpmeans.z();
//  // Output results.
//  MatrixXf centroids = dpmeans.centroids();
//  VectorXf counts = dpmeans.counts().cast<float>();
////  std::cout << counts.transpose() << std::endl;
////  std::cout << centroids << std::endl;
//  uint32_t K = counts.rows();
////  std::cout << "K: " << K << std::endl;
//  // Compute Gaussian statistics:
//  std::vector<MatrixXd> Ss(K,Matrix3d::Zero());
//  Eigen::MatrixXd xSum = Eigen::MatrixXd::Zero(3,K);
//  Eigen::VectorXf ws = Eigen::VectorXf::Zero(K);
//  for (uint32_t i=0; i<xyz->cols(); ++i)
//    if(z(i) < K) {
//      float w = pc.at(i).curvature; // curvature is used to store the weights.
//      ws(z(i)) += w;
//      xSum.col(z(i)) += xyz->col(i).cast<double>()*w;
//    }
//  for(uint32_t k=0; k<K; ++k)
//    centroids.col(k) = xSum.col(k).cast<float>()/ws(k);
//
//  for (uint32_t i=0; i<xyz->cols(); ++i)
//    if(z(i) < K) {
//      float w = pc.at(i).curvature; // curvature is used to store the weights.
//      Ss[z(i)] += w*(xyz->col(i) - centroids.col(z(i))).cast<double>()
//        * (xyz->col(i) - centroids.col(z(i))).cast<double>().transpose();
//    }
//  if (colorLables) {
//    for (uint32_t i=0; i<xyz->cols(); ++i)
//      if(z(i) < K) {
//        uint8_t r = floor(float(z(i))/float(K-1)*255.);
//        uint8_t g = floor(float(K-1-z(i))/float(K-1)*255.);
//        uint8_t b = floor(float(z(i)<=(K-1)/2? z(i)+(K-1)/2 :
//              z(i)-(K-1)/2-1)/float(K-1)*255.);
//        pc.at(i).rgb = ((int)r) << 16 | ((int)g) << 8 | ((int)b);
//      }
//  }
//  // Fractions belonging to each cluster.
//  Eigen::VectorXd pis = (ws.array() / ws.sum()).matrix().cast<double>();
//
//  for(uint32_t k=0; k<K; ++k)
////    if (pis(k) > 0.) {
//    if (counts(k) > 100) {
//      gmm.push_back(bb::Normal<3>(centroids.col(k).cast<double>(),
//            Ss[k]/float(ws(k))+0.001*Eigen::Matrix3d::Identity(), pis(k)));
////        Ss[k]/float(ws(k)), pis(k)));
//    }
//  return true;
//}

bool ComputePcBoundaries(const pcl::PointCloud<pcl::PointXYZRGBNormal>&
    pc, Eigen::Vector3d& min, Eigen::Vector3d& max) {
  auto x = pc.getMatrixXfMap(1, 12, 0); // this works for PointXYZRGBNormal
  auto y = pc.getMatrixXfMap(1, 12, 1); // this works for PointXYZRGBNormal
  auto z = pc.getMatrixXfMap(1, 12, 2); // this works for PointXYZRGBNormal
  min << x.minCoeff(), y.minCoeff(), z.minCoeff();
  max << x.maxCoeff(), y.maxCoeff(), z.maxCoeff();
  return true;
}

int main(int argc, char** argv) {
  // Declare the supported options.
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help,h", "produce help message")
    ("lambdaDeg,l", po::value<double>(), "lambda in degree for DPvMF-means")
    ("lambdaT,t", po::value<double>(), "lambda in meters for DPMeans")
    ("in_a,a", po::value<string>(), "path to first input file")
    ("in_b,b", po::value<string>(), "path to second input file")
    ("out,o", po::value<string>(), "path to output file")
    ("scale,s", po::value<float>(),"scale for point-cloud")
    ("maxLvlR3", po::value<int>(),"max BB lvl for R3")
    ("maxLvlS3", po::value<int>(),"max BB lvl for S3")
    ("egi,e", "make the vMF MM pis uniform - like a EGI")
    ("simpleTrans", "use means of PCs to compute translation")
    ("simpleRot", "use 2-vMF-mixture to compute rotation")
    ("tryMfAmbig", "also try all mf rotation ambiguities")
    ("oB0", "output bounds at level 0")
    ("TpS", "use TpS-based tessellation instead of 600-cell based (not recommended)")
    ("AA", "use axis-angle-based tessellation instead of 600-cell based (not recommended)")
    ("display,d", "display results")
    ("verbose,v", "be verbose")
    ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << desc << "\n";
    return 1;
  }

  CfgRtDDPvMF cfg;
  cfg.f_d = 540.;
  cfg.beta = 1e5;
  cfg.nFramesSurvive_ = 1; // DPvMFMM
  cfg.pathOut = std::string("../results/");
  double lambdaDeg = 93.;
  if(vm.count("lambdaDeg")) lambdaDeg = vm["lambdaDeg"].as<double>();
  double lambdaT = 1.0;
  if(vm.count("lambdaT")) lambdaT = vm["lambdaT"].as<double>();

  cfg.lambdaFromDeg(lambdaDeg);
  cfg.QfromFrames2Survive(cfg.nFramesSurvive_);

  bool output_init_bounds = false;
  bool egi_mode = false;
  bool TpS_mode = false;
  bool AA_mode = false;
  bool simpleTrans = false;
  bool simpleRot = false;
  bool tryMfAmbig= false;
  uint32_t maxLvlS3 = 13;
  uint32_t maxLvlR3 = 13;
  string pathA = "";
  string pathB = "";
  std::string pathOut = "";
  //  string mode = "";
  //  if(vm.count("mode")) mode = vm["mode"].as<string>();
  if(vm.count("out")) pathOut = vm["out"].as<std::string>();
  if(vm.count("in_a")) pathA = vm["in_a"].as<string>();
  if(vm.count("in_b")) pathB = vm["in_b"].as<string>();
  float scale = 0.01;
  if(vm.count("scale")) scale = vm["scale"].as<float>();
  if(vm.count("maxLvlR3")) maxLvlR3 = vm["maxLvlR3"].as<int>();
  if(vm.count("maxLvlS3")) maxLvlS3 = vm["maxLvlS3"].as<int>();
  if(vm.count("egi")) egi_mode = true;
  if(vm.count("TpS")) TpS_mode = true;
  if(vm.count("AA")) AA_mode = true;
  if(vm.count("oB0")) output_init_bounds = true;
  if(vm.count("simpleTrans")) simpleTrans = true;
  if(vm.count("simpleRot")) simpleRot = true;
  if(vm.count("tryMfAmbig")) tryMfAmbig= true;
  if (egi_mode)
    std::cout << "Using EGI mode - making pis of vMF MM uniform." << std::endl;

  // Load point clouds.
  pcl::PointCloud<pcl::PointXYZRGBNormal> pcAload, pcBload;
  pcl::PLYReader reader;
  std::cout << reader.read(pathA, pcAload) << std::endl;
  std::cout << "loading pc from " << pathA << std::endl;
  if (reader.read(pathA, pcAload))
    std::cout << "error reading " << pathA << std::endl;
  else
    std::cout << "loaded pc from " << pathA << ": " << pcAload.width << "x"
      << pcAload.height << std::endl;
  std::cout << "loading pc from " << pathB << std::endl;
  if (reader.read(pathB, pcBload))
    std::cout << "error reading " << pathB << std::endl;
  else
    std::cout << "loaded pc from " << pathB << ": " << pcBload.width << "x"
      << pcBload.height << std::endl;

  pcl::PointCloud<pcl::PointXYZRGBNormal> pcA, pcB;

  for (size_t i=0; i<pcAload.size(); ++i) {
    Eigen::Map<Vector3f> n(&(pcAload.at(i).normal_x));
    if (n.norm() > 0.001) {
      n /= n.norm();
      pcA.push_back(pcAload.at(i));
//      pcA.push_back(pcAload.at(i));
//      pcA.back().normal_x *= -1;
//      pcA.back().normal_y *= -1;
//      pcA.back().normal_z *= -1;
//    } else {
//      std::cout << "normal not normalizeable " << n.norm() << std::endl;
    }
  }
  for (size_t i=0; i<pcBload.size(); ++i) {
    Eigen::Map<Vector3f> n(&(pcBload.at(i).normal_x));
    if (n.norm() > 0.001) {
      n /= n.norm();
      pcB.push_back(pcBload.at(i));
//      pcB.push_back(pcBload.at(i));
//      pcB.back().normal_x *= -1;
//      pcB.back().normal_y *= -1;
//      pcB.back().normal_z *= -1;
//    } else {
//      std::cout << "normal not normalizeable " << n.norm() << std::endl;
    }
  }
  findCudaDevice(argc,(const char**)argv);

  jsc::Timer t0;
  ShufflePc(pcA);
  ShufflePc(pcB);

  ComputeAreaWeightsPcRadius(pcA,scale);
  ComputeAreaWeightsPcRadius(pcB,scale);

  // Obtain the range for the translation.
  // TODO: this could be made more tight by getting min and max for the
  // rotated point-clouds.
  Eigen::Vector3d minA0, minB0, maxA0, maxB0, min, max;
  ComputePcBoundaries(pcA, minA0, maxA0);
  ComputePcBoundaries(pcB, minB0, maxB0);

  std::cout<<"rtDDPvMFmeans lambdaDeg="<<cfg.lambdaDeg_<<" beta="<<cfg.beta
    <<"nFramesSurvive="<<cfg.nFramesSurvive_<<std::endl;
  std::cout<<"output path: "<<cfg.pathOut<<std::endl;

  std::vector<bb::vMF<3>> vmfsA;
  ComputevMFMMfromPC(pcA, cfg, vmfsA);
  if(vm.count("verbose")) for(uint32_t k=0; k<vmfsA.size(); ++k) vmfsA[k].Print();

  std::vector<bb::vMF<3>> vmfsB;
  ComputevMFMMfromPC(pcB, cfg, vmfsB);
  if(vm.count("verbose")) for(uint32_t k=0; k<vmfsB.size(); ++k) vmfsB[k].Print();

  if (egi_mode) {
    for(uint32_t k=0; k<vmfsA.size(); ++k)
      vmfsA[k].SetPi(1./float(vmfsA.size()));
    for(uint32_t k=0; k<vmfsB.size(); ++k)
      vmfsB[k].SetPi(1./float(vmfsB.size()));
  }

  cfg.lambda = lambdaT;
  std::vector<bb::Normal<3>> gmmA;
  if (!simpleTrans) {
    ComputeGMMfromPC(pcA, cfg, gmmA, false);
    if(vm.count("verbose")) for(uint32_t k=0; k<gmmA.size(); ++k) gmmA[k].Print();
  }

  std::vector<bb::Normal<3>> gmmB;
  if (!simpleTrans) {
    ComputeGMMfromPC(pcB, cfg, gmmB, false);
    if(vm.count("verbose")) for(uint32_t k=0; k<gmmB.size(); ++k) gmmB[k].Print();
  }

  float tCluster = t0.toctic("timing cluster");

  bb::vMFMM<3> vmfmmA(vmfsA);
  bb::vMFMM<3> vmfmmB(vmfsB);
  bb::LowerBoundS3 lower_bound_S3(vmfmmA, vmfmmB);
  bb::UpperBoundIndepS3 upper_bound_S3(vmfmmA, vmfmmB);
  bb::UpperBoundConvexS3 upper_bound_convex_S3(vmfmmA, vmfmmB);

  std::list<bb::NodeS3> nodesS3;
  Eigen::Quaterniond q_star;
  double lb_star = 1e99;
  double eps = 1e-10;
  //  double eps = 8e-7;
  uint32_t max_it = 100000;
  if (TpS_mode)  {

    std::cout << " Tessellate TpS3" << std::endl;
    std::list<bb::NodeTpS3> nodes = bb::TessellateTpS3();
    std::cout << "# initial nodes: " << nodes.size() << std::endl;

    bb::LowerBoundTpS3 lower_bound_TpS3(lower_bound_S3);
    bb::UpperBoundIndepTpS3 upper_bound_TpS3(upper_bound_S3);
    bb::UpperBoundConvexTpS3 upper_bound_convex_TpS3(upper_bound_convex_S3);
    if (output_init_bounds) {
      WriteBounds<bb::NodeTpS3>(lower_bound_TpS3, upper_bound_TpS3,
          upper_bound_convex_TpS3, nodes);
    }
    std::cout << " BB on S3 eps=" << eps << " max_it=" << max_it << std::endl;
    bb::BranchAndBound<bb::NodeTpS3> bb(lower_bound_TpS3, upper_bound_convex_TpS3);
    bb::NodeTpS3 node_star = bb.Compute(nodes, eps, maxLvlS3, max_it);
//  bb::CountBranchesInTree<bb::NodeS3>(nodes);
    q_star = node_star.GetLbArgument();
    lb_star = node_star.GetLB();
    // output a list of NodeS3s for further processing; use the
    // interior Node for this.
    for (const auto& node : nodes)
      nodesS3.push_back(node.GetNodeS3());
  } else if (AA_mode)  {
    std::cout << " Tessellate axis angle space" << std::endl;
    std::list<bb::NodeAA> nodes = bb::TessellateAA();
    std::cout << "# initial nodes: " << nodes.size() << std::endl;

    bb::LowerBoundAA lower_bound_AA(lower_bound_S3);
    bb::UpperBoundIndepAA upper_bound_AA(upper_bound_S3);
    bb::UpperBoundConvexAA upper_bound_convex_AA(upper_bound_convex_S3);
    if (output_init_bounds) {
      WriteBounds<bb::NodeAA>(lower_bound_AA, upper_bound_AA,
          upper_bound_convex_AA, nodes);
    }
    std::cout << " BB on S3 eps=" << eps << " max_it=" << max_it << std::endl;
    bb::BranchAndBound<bb::NodeAA> bb(lower_bound_AA, upper_bound_convex_AA);
    bb::NodeAA node_star = bb.Compute(nodes, eps, maxLvlS3, max_it);
//  bb::CountBranchesInTree<bb::NodeS3>(nodes);
    q_star = node_star.GetLbArgument();
    lb_star = node_star.GetLB();
    // output a list of NodeS3s for further processing; use the
    // interior Node for this.
    for (const auto& node : nodes)
      nodesS3.push_back(node.GetNodeS3());
  } else if (simpleRot) {
    std::cout << "simple rotation computation" << std::endl;
    if (vmfmmA.GetK() > 1 && vmfmmB.GetK() > 1) {
      // q is q_ba
      bb::Combinations combA(vmfmmA.GetK(), 2);
      bb::Combinations combB(vmfmmB.GetK(), 2);
      std::vector<std::vector<uint32_t>> perm;
      perm.push_back({0,1});
      perm.push_back({1,0});
//      perm.push_back({0,1,2});
//      perm.push_back({0,2,1});
//      perm.push_back({1,0,2});
//      perm.push_back({1,2,0});
//      perm.push_back({2,1,0});
//      perm.push_back({2,0,1});
      lb_star = std::numeric_limits<double>::lowest();
      for (const auto& idsA : combA.Get())
        for (const auto& idsB : combB.Get()) {
          for (const auto& p : perm) {
            Eigen::Matrix3d N = Eigen::Matrix3d::Zero();
            for (size_t i=0; i<p.size(); ++i) {
//              N += vmfmmB.Get(idsB[p[i]]).GetTau()*vmfmmA.Get(idsA[i]).GetTau()*
//                vmfmmB.Get(idsB[p[i]]).GetMu()*vmfmmA.Get(idsA[i]).GetMu().transpose();
              N += vmfmmA.Get(idsA[i]).GetTau()*
                vmfmmB.Get(idsB[p[i]]).GetXSum()*vmfmmA.Get(idsA[i]).GetMu().transpose();
            }
            Eigen::JacobiSVD<Eigen::Matrix3d> svd(N, Eigen::ComputeFullU|
                Eigen::ComputeFullV);
            Eigen::Vector3d diag(1,1,
                (svd.matrixU()*svd.matrixV().transpose()).determinant());
            Eigen::Matrix3d R_ba = svd.matrixU()*diag.asDiagonal()*svd.matrixV().transpose();
            if (R_ba.determinant() < 0) {
              std::cout << " negative determinant for R " << R_ba.determinant() << std::endl;
              return -2;
            }
            Eigen::Quaterniond q(R_ba);
            double lb;
            lower_bound_S3.EvaluateRotation(q,lb);
            if (lb > lb_star) {
              lb_star = lb;
              q_star = q;
              std::cout << "updating lb: " << lb_star << std::endl;
            }
          }
        }
    } else {
      std::cout << "not enough vmf clusters for simple rotation computation"
        << std::endl;
    }
  } else {
    std::cout << " Tessellate S3" << std::endl;
    nodesS3 = bb::GenerateNotesThatTessellateS3();
    std::cout << "# initial nodes: " << nodesS3.size() << std::endl;
    if (output_init_bounds) {
      WriteBounds<bb::NodeS3>(lower_bound_S3, upper_bound_S3,
          upper_bound_convex_S3, nodesS3);
    }
    std::cout << " BB on S3 eps=" << eps << " max_it=" << max_it << std::endl;
    bb::BranchAndBound<bb::NodeS3> bb(lower_bound_S3, upper_bound_convex_S3);
    bb::NodeS3 node_star = bb.Compute(nodesS3, eps, maxLvlS3, max_it);
//  bb::CountBranchesInTree<bb::NodeS3>(nodesS3);
    q_star = node_star.GetLbArgument();
    lb_star = node_star.GetLB();
  }
  std::cout << "optimum BB quaternion: "  << q_star.coeffs().transpose()
      << " angle: " << 2.*acos(q_star.w()) * 180. / M_PI << std::endl
      << q_star.toRotationMatrix()
      << std::endl;
  Eigen::Vector3d t_star;

  Eigen::VectorXd lbsS3;
  eigen_vector<Eigen::Quaterniond> qs;
  eigen_vector<Eigen::Quaterniond> qsPrelim;
  if (simpleRot) {
    qs.push_back(q_star.inverse());
    qs[0].normalize();
    lbsS3 = Eigen::VectorXd(1);
    lbsS3(0) = lb_star;
  } else {

    shared_ptr<Eigen::MatrixXd> qAll(new MatrixXd(4, nodesS3.size()));
    auto it=nodesS3.begin();
    for (uint32_t i=0; i < nodesS3.size(); ++i, it++) {
      qAll->col(i) = it->GetTetrahedron().GetCenter();
    }
    double lambda_q = cos(2.*10.*M_PI/180.) - 1.; // TODO
    dplv::DPMeans<double, dplv::Spherical<double>> dpvMF(qAll, 0, lambda_q);
    for (uint32_t t=0; t<20; ++t) {
      dpvMF.updateLabels();
      dpvMF.updateCenters();
    }
    std::cout << "K " << dpvMF.getK() << std::endl;
//    std::cout << dpvMF.centroids() << std::endl;

    std::vector<double> lbsS3prelim;
    if (dpvMF.K() == 1) {
      qsPrelim.push_back(q_star);
      lbsS3prelim.push_back(lb_star);
    } else {
      std::cout << "======== K > 1: " << dpvMF.K() << std::endl;
      for (uint32_t k=0; k<dpvMF.K(); ++k) {
        auto it = nodesS3.begin();
        auto z = dpvMF.z();
        // Find the node whith maximum LB in each cluster.
        double lb_max = -1.e20;
        Eigen::Quaterniond q_k;
        for (uint32_t i=0; i<nodesS3.size(); ++i, it++)
          if (z(i) == k && lb_max < it->GetLB()) {
            lb_max = it->GetLB();
            q_k = it->GetTetrahedron().GetCenterQuaternion();
          }
        qsPrelim.push_back(q_k);
        lbsS3prelim.push_back(lb_max);
        //  Eigen::Quaterniond q = node_star.GetTetrahedron().GetCenterQuaternion();
        std::cout << "in cluster " << k << ": center = "
          << dpvMF.centroids().col(k).transpose() << std::endl;
        std::cout << " max LB quaternion: "  << q_k.coeffs().transpose()
          << " angle: " << 2.*acos(q_k.w()) * 180. / M_PI << std::endl;
      }
    }

    // TODO could be foldet into above.
    std::vector<bool> toDelete(dpvMF.K(), false);
    uint32_t n_delete = 0;
    for (uint32_t k=0; k<dpvMF.K(); ++k)
      for (uint32_t j=k+1; j<dpvMF.K(); ++j) {
        double dAng = qsPrelim[k].angularDistance(qsPrelim[j]);
        std::cout << " dang " << k << ";" << j << ": " << dAng*180./M_PI << std::endl;
        toDelete[k] = dAng < 0.1/180.*M_PI;
        if (toDelete[k]) ++n_delete;
      }
    lbsS3 = Eigen::VectorXd(qsPrelim.size()-n_delete);
    uint32_t j=0;
    for (uint32_t k=0; k<dpvMF.K(); ++k)
      if (!toDelete[k]) {
        qs.push_back(qsPrelim[k]);
        lbsS3(j++) = lbsS3prelim[k];
      }

    std::cout << "===filtered==== K: " << qs.size() << std::endl;
    for (uint32_t k=0; k<qs.size(); ++k) {
      // This q is the inverse of the rotation that brings B to A.
      std::cout << k << std::endl;
      qs[k] = Eigen::Quaterniond(qs[k].inverse()); // A little ugly but this is because of the way we setup the problem...
    }
  }

  if (tryMfAmbig) {
    std::cout << "trying MW ambiguities" << std::endl;
    size_t Nq = qs.size();
    Eigen::VectorXd lbsS3ambig = Eigen::VectorXd(Nq*24);
    lbsS3ambig.topRows(Nq) = lbsS3;
    for (size_t i=0; i<Nq; ++i) {
      Eigen::Matrix3d R0 = qs[i].toRotationMatrix();
      Eigen::Matrix<double,3,6> M;
      M << R0, -R0;
      bb::Combinations comb(6, 3);
      std::vector<std::vector<uint32_t>> perm;
      perm.push_back({0,1,2});
      perm.push_back({0,2,1});
      perm.push_back({1,0,2});
      perm.push_back({1,2,0});
      perm.push_back({2,1,0});
      perm.push_back({2,0,1});
      size_t j=Nq;
      for (const auto& c : comb.Get()) {
        for (const auto& p : perm) {
          Eigen::Matrix3d R;
          R << M.col(c[p[0]]), M.col(c[p[1]]), M.col(c[p[2]]);
          if (R.determinant() > 0) {
            qs.emplace_back(R);
            std::cout << c[p[0]] << " " << c[p[1]] << " " << c[p[2]] << " "
              << R.determinant() << std::endl;
            lbsS3ambig(j++) = lbsS3(i);
          }
        }
      }
    }
    lbsS3 = lbsS3ambig;
    std::cout << "added "
      << qs.size()-Nq << " permutations of rotation matrices to try" << std::endl;
  }

  eigen_vector<Eigen::Vector3d> ts(qs.size(), Eigen::Vector3d::Zero());
  Eigen::VectorXd lbsR3 = -1.*Eigen::VectorXd::Ones(qs.size());
  Eigen::VectorXd ubsR3 = -1.*Eigen::VectorXd::Ones(qs.size());
  Eigen::VectorXd lbs(qs.size());
  // dont nest parfors (BB is already internally parallelized
  std::cout << "searching for translations in "
    << qs.size() << " rotations" << std::endl;
  for (uint32_t k=0; k<qs.size(); ++k) {
    Eigen::Quaterniond q = qs[k];
    if (simpleTrans) {
      std::cout << "Simple transformation " << std::endl;
      Eigen::Vector3d meanA = ComputePcMean(pcA);
      Eigen::Vector3d meanB = ComputePcMean(pcB);
      std::cout << meanA.transpose() << std::endl << meanB.transpose() << std::endl;
      Eigen::Vector3d t = meanB - q._transformVector(meanA);
      std::cout << q.coeffs().transpose() << std::endl;
      std::cout << t.transpose() << std::endl;
//      std::cout << q._transformVector(meanB) - meanA << std::endl;
//      std::cout << q._transformVector(meanA) - meanB << std::endl;

      ts[k] = t;
      lbsR3(k) = 0.;
      lbs(k) = lbsS3(k);
    } else {
      // To get all corners of the bounding box.
      bb::Box box(minA0, maxA0);
      Eigen::Vector3d minA = minA0;
      Eigen::Vector3d maxA = maxA0;
      // Update the boundaries of the the rotated point cloud A to get
      // the full translation space later on.
      for (uint32_t i=0; i<8; ++i) {
        Eigen::Vector3d c;
        box.GetCorner(i,c);
        c = q._transformVector(c);
        for (uint32_t d=0; d<3; ++d) {
          minA(d) = std::min(minA(d), c(d));
          maxA(d) = std::max(maxA(d), c(d));
        }
      }
      for (uint32_t d=0; d<3; ++d) {
        Eigen::Matrix<double,4,1> dt;
        dt(0) = -minA(d) + minB0(d);
        dt(1) = -minA(d) + maxB0(d);
        dt(2) = -maxA(d) + minB0(d);
        dt(3) = -maxA(d) + maxB0(d);
        min(d) = dt.minCoeff();
        max(d) = dt.maxCoeff();
      }
      std::cout << "min t: " << min.transpose()
        << " max t: " << max.transpose() << std::endl;

//      NodeR3 node0(Box(min, max), 0);
//      std::list<bb::NodeR3> nodesR3(1,node0);
      std::list<bb::NodeR3> nodesR3 =
        bb::GenerateNotesThatTessellateR3(min, max, (max-min).norm());
      bb::LowerBoundR3 lower_bound_R3(gmmA, gmmB, q);
      bb::UpperBoundIndepR3 upper_bound_R3(gmmA, gmmB, q);
      bb::UpperBoundConvexR3 upper_bound_convex_R3(gmmA, gmmB, q);

      if (output_init_bounds) {
        WriteBounds<bb::NodeR3>(lower_bound_R3, upper_bound_R3,
            upper_bound_convex_R3, nodesR3);
      }

      std::cout << "# initial nodes: " << nodesR3.size() << std::endl;
      double eps = 1e-10;
      uint32_t max_it = 100000;
      double lb = 0.;
      double ub = 0.;
#pragma omp critical
      {
        lb = lbsR3.maxCoeff();
        ub = ubsR3.maxCoeff();
      }
      bb::BranchAndBound<bb::NodeR3> bbR3(lower_bound_R3, upper_bound_convex_R3);
      std::cout << " BB on R3 eps=" << eps << " max_it=" << max_it << std::endl;
      bb::NodeR3 nodeR3_star = bbR3.Compute(nodesR3, eps, maxLvlR3, max_it,
          lb);
      Eigen::Vector3d t =  nodeR3_star.GetLbArgument();
      //    bb::CountBranchesInTree<bb::NodeR3>(nodesR3);
      std::cout << "with LB " << nodeR3_star.GetLB() << " optimum translation: "
        << t.transpose() << std::endl;
      ts[k] = t;
      lbsR3(k) = nodeR3_star.GetLB();
      ubsR3(k) = nodeR3_star.GetUB();
//      lbs(k) = lbsS3(k) * lbsR3(k);
      lbs(k) = lbsR3(k);
    }

//    for (auto& node: nodesR3) {
//      std::cout << node.GetLbArgument().transpose() << std::endl;
//    }

    // Display the loaded point clouds.
    if (vm.count("display") && vm.count("verbose")) {
      DisplayPcs(pcA, pcB, gmmA, gmmB, vmfsA, vmfsB, qs[k], ts[k], cfg.lambda/10.);
    }
  }
  float tBB = t0.toctic("timing BB");

  std::cout << "Have LbS3: " << lbsS3.transpose() << std::endl;
  std::cout << "Have LbR3: " << lbsR3.transpose() << std::endl;
  uint32_t id_max = 0;
  double lb_max = lbs.maxCoeff(&id_max);
  double lbS3 = lbsS3(id_max);
  double lbR3 = lbsR3(id_max);
  t_star = ts[id_max];
  q_star = qs[id_max].normalized();
  std::cout << "Updating overall optimum transformation to: " << std::endl;
  std::cout << "q: " << q_star.coeffs().transpose()
    << " t: " << t_star.transpose() <<  std::endl;

  std::cout << "time cluster " << tCluster << " "
    << 100.*tCluster/(tCluster+tBB) << " %" << std::endl;
  std::cout << "time BB " << tBB << " "
    << 100.*tBB/(tCluster+tBB) << " %" << std::endl;

  if(pathOut.size() > 1) {
    std::ofstream out(pathOut + std::string(".csv"));
    out << "q_w q_x q_y q_z t_x t_y t_z lb_S3 lb_R3 KvmfA KvmfB KgmmA KgmmB K" << std::endl;
    out << q_star.w() << " " << q_star.x() << " "
      << q_star.y() << " " << q_star.z() << " " << t_star(0)
      << " " << t_star(1) << " " << t_star(2)
      << " " << lbS3 << " " << lbR3
//      << " " << node_star.GetLB() << " " << nodeR3_star.GetLB()
      << " " << vmfsA.size() << " " << vmfsB.size()
      << " " << gmmA.size() << " " << gmmB.size()
      << " " << qs.size()
      << std::endl;
    out.close();
  }

  // Display the loaded point clouds.
  if (vm.count("display")) {
    DisplayPcs(pcA, pcB, gmmA, gmmB, vmfsA, vmfsB, q_star, t_star, cfg.lambda/10.);
  }

  std::cout<<cudaDeviceReset()<<std::endl;
  return (0);
}


//  Copyright John Maddock 2012.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Basic sanity check that header <boost/math/special_functions/bessel.hpp>
// #includes all the files that it needs to.
//
#include <boost/math/special_functions/hankel.hpp>
//
// Note this header includes no other headers, this is
// important if this test is to be meaningful:
//
#include "test_compile_result.hpp"

inline void check_result_imp(std::complex<float>, std::complex<float>){}
inline void check_result_imp(std::complex<double>, std::complex<double>){}
inline void check_result_imp(std::complex<long double>, std::complex<long double>){}

void compile_and_link_test()
{
   check_result<std::complex<float> >(boost::math::cyl_hankel_1<float>(f, f));
   check_result<std::complex<double> >(boost::math::cyl_hankel_1<double>(d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<std::complex<long double> >(boost::math::cyl_hankel_1<long double>(l, l));
#endif

   check_result<std::complex<float> >(boost::math::cyl_hankel_2<float>(f, f));
   check_result<std::complex<double> >(boost::math::cyl_hankel_2<double>(d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<std::complex<long double> >(boost::math::cyl_hankel_2<long double>(l, l));
#endif

   check_result<std::complex<float> >(boost::math::sph_hankel_1<float>(f, f));
   check_result<std::complex<double> >(boost::math::sph_hankel_1<double>(d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<std::complex<long double> >(boost::math::sph_hankel_1<long double>(l, l));
#endif

   check_result<std::complex<float> >(boost::math::sph_hankel_2<float>(f, f));
   check_result<std::complex<double> >(boost::math::sph_hankel_2<double>(d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<std::complex<long double> >(boost::math::sph_hankel_2<long double>(l, l));
#endif

}


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE permutation

#define timer timer_class
#include <boost/test/unit_test.hpp>
#undef timer

#include <core/utils/bdd_utils.hpp>
#include <core/utils/range_utils.hpp>

#include <reversible/circuit.hpp>
#include <reversible/rcbdd.hpp>
#include <reversible/io/print_circuit.hpp>
#include <reversible/synthesis/exact_synthesis.cpp>
#include <reversible/utils/foreach_vshape.hpp>
#include <reversible/utils/permutation.hpp>

using namespace cirkit;

BOOST_AUTO_TEST_CASE(simple)
{
  /* number of lines */
  auto n = 3u;

  /* for RCBDDs */
  rcbdd cf;
  cf.initialize_manager();
  cf.create_variables( n );

  /* statistics */
  auto total    = 0u;
  auto selfinv  = 0u;
  auto selfdual = 0u;
  auto monotone = 0u;
  auto unate    = 0u;
  auto horn     = 0u;

  std::vector<int> ps; /* for unateness test */

  foreach_vshape( n, [&]( const circuit& circ ) {
      permutation_t perm = circuit_to_permutation( circ );
      BDD func = cf.create_from_circuit( circ );

      ++total;
      if ( cf.is_self_inverse( func ) )         { ++selfinv;  }
      if ( is_selfdual( cf.manager(), func ) )  { ++selfdual; }
      if ( is_monotone( cf.manager(), func ) )  { ++monotone; }
      if ( is_unate( cf.manager(), func, ps ) ) { ++unate;    }
      if ( is_horn( cf.manager(), func ) )      { ++horn;     }
    });

  std::cout << "[i] circuits investigated: " << total << std::endl
            << "[i] - self inverse:        " << selfinv << std::endl
            << "[i] - self dual:           " << selfdual << std::endl
            << "[i] - monotone:            " << monotone << std::endl
            << "[i] - unate:               " << unate << std::endl
            << "[i] - horn:                " << horn << std::endl;

  permutation_t perm = {0u, 1u, 2u, 3u, 4u, 5u, 6u, 7u};

  std::vector<unsigned> sizes( 9u, 0u );

  do
  {
    if ( is_simple( perm ) )
    {
        std::cout << "[i] simple: " << permutation_to_string( perm ) << std::endl;

        binary_truth_table spec;

        for ( auto i = 0u; i < 8u; ++i )
        {
          spec.add_entry( number_to_truth_table_cube( i, 3u ), number_to_truth_table_cube( perm.at( i ), 3u ) );
        }

        circuit circ;
        auto es_settings = std::make_shared<properties>();
        es_settings->set( "negative", false );
        exact_synthesis( circ, spec, es_settings );

        std::cout << "[i] " << circ.num_gates() << std::endl;

        sizes[circ.num_gates()]++;
    }
  } while ( std::next_permutation( perm.begin(), perm.end() ) );

  std::cout << "[i] sizes: " << any_join( sizes, " " ) << std::endl;
}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


#include <tuple>
#include <Eigen/Core>
#include <iostream>

#include <npe.h>

npe_function(sparse_matrix_add)
npe_arg(a, sparse_double, sparse_float)
npe_arg(b, npe_matches(a))
npe_begin_code()

npe_Matrix_a ret1 = a + b;

return npe::move(ret1);
npe_end_code()


#pragma once

/* Liveness analysis */

#include <crab/common/stats.hpp>
#include <crab/common/debug.hpp>
#include <crab/common/types.hpp>
#include <crab/domains/killgen_domain.hpp>
#include <crab/iterators/killgen_fixpoint_iterator.hpp>

#include <unordered_map>
#include <boost/range/iterator_range.hpp>

namespace crab {

  namespace analyzer {

   template<typename V>
   using varset_domain = crab::domains::flat_killgen_domain<V>;

    /**
     * Define the main operations for the liveness variable analysis:
     * compute for each basic block the set of live variables, i.e.,
     * variables that might be used in the future
     **/
   template<class CFG>
   class liveness_analysis_operations:
      public crab::iterators::
      killgen_operations_api<CFG, varset_domain<typename CFG::variable_t>> {

   public:

     typedef varset_domain<typename CFG::variable_t> varset_domain_t;
     typedef typename CFG::basic_block_label_t basic_block_label_t;

   private:

     typedef crab::iterators::killgen_operations_api<CFG, varset_domain_t> parent_type;
     typedef std::pair<varset_domain_t, varset_domain_t> binding_t;
     typedef std::unordered_map<basic_block_label_t, binding_t> liveness_map_t;

     liveness_map_t m_liveness_map;

    public:

     liveness_analysis_operations(CFG cfg): parent_type(cfg) { }

     virtual bool is_forward() { return false; }

     virtual varset_domain_t entry() {
       varset_domain_t res = varset_domain_t::bottom();
       if (this->m_cfg.has_func_decl()) {
	 auto fdecl = this->m_cfg.get_func_decl();
	 for(unsigned i=0, e= fdecl.get_num_outputs(); i<e; ++i) {
	   res += fdecl.get_output_name(i);
	 }
       }
      return res;
     }

     virtual varset_domain_t merge(varset_domain_t d1,  varset_domain_t d2) {
       return d1 | d2;
     }

     virtual void init_fixpoint() {
       for (auto &b: boost::make_iterator_range(this->m_cfg.begin(),this->m_cfg.end())){
	 varset_domain_t kill, gen;
	 for (auto &s: boost::make_iterator_range(b.rbegin(),b.rend())) {
	   auto live = s.get_live();
	   for (auto d: boost::make_iterator_range(live.defs_begin(),
						   live.defs_end())) {
	     kill += d;
	     gen -= d;
	   }
	   for (auto u: boost::make_iterator_range(live.uses_begin(),
						   live.uses_end())) {
	     gen  += u;
	   }
	 }
	 m_liveness_map.insert(std::make_pair(b.label(), binding_t(kill, gen)));
       }
     }

      virtual varset_domain_t analyze(basic_block_label_t bb_id,
				      varset_domain_t in) {
        auto it = m_liveness_map.find(bb_id);
        assert(it != m_liveness_map.end());
        in -= it->second.first;
        in += it->second.second;
        return in;
      }

      virtual std::string name() { return "liveness";}
   };

    /** Live variable analysis **/
    template<typename CFG>
    class liveness_analysis:
      public crab::iterators::
      killgen_fixpoint_iterator<CFG,liveness_analysis_operations<CFG>>{

      typedef liveness_analysis_operations<CFG> liveness_analysis_operations_t;
      typedef crab::iterators::
      killgen_fixpoint_iterator<CFG,liveness_analysis_operations_t> killgen_fixpoint_iterator_t;

      liveness_analysis(const liveness_analysis<CFG>& other) = delete;
      liveness_analysis<CFG>& operator=(const liveness_analysis<CFG>& other) = delete;

    public:

      typedef typename CFG::basic_block_label_t basic_block_label_t;
      typedef typename CFG::statement_t statement_t;
      typedef typename CFG::varname_t varname_t;
      typedef typename liveness_analysis_operations_t::varset_domain_t varset_domain_t;

    private:

      // output of the analysis: map basic blocks to set of live
      // variables at the end of the blocks
      std::unordered_map<basic_block_label_t, varset_domain_t> _live_map;

    public:

      liveness_analysis(CFG cfg)
	: killgen_fixpoint_iterator_t(cfg)
      { }

      void exec() {
	this->run();
	for (auto p : boost::make_iterator_range(this->out_begin(), this->out_end())) {
	  _live_map.insert({p.first, p.second});
	  CRAB_LOG("liveness-live",
		   crab::outs() << cfg_impl::get_label_str(p.first)
		                << " live variables=" << p.second <<"\n";);

	}
	this->release_memory();
      }

      varset_domain_t get(basic_block_label_t bb) const {
	auto it = _live_map.find(bb);
	if (it != _live_map.end()) {
	  return it->second;
	} else {
	  return varset_domain_t::bottom();
	}
      }

      void write (crab_os &o) const {
	o << "TODO: print liveness analysis results\n";
      }

    };

    template<typename CFG>
    inline crab_os& operator<<(crab_os& o, const liveness_analysis<CFG> &l) {
      l.write (o);
      return o;
    }

    /**
     * Dead variable analysis.
     *
     * FIXME: the name "liveness" is not that great so we should
     *        change it at some point. For that we need to change crab
     *        clients.
     **/
    template<typename CFG>
    class liveness {
    public:

      typedef typename CFG::basic_block_label_t basic_block_label_t;
      typedef typename CFG::basic_block_t basic_block_t;
      typedef typename CFG::statement_t statement_t;
      typedef typename CFG::varname_t varname_t;
      typedef typename CFG::variable_t variable_t;
      typedef varset_domain<variable_t> varset_domain_t;

    private:

      typedef liveness_analysis<CFG> liveness_analysis_t;

      // the cfg
      CFG m_cfg;

      // output of the analysis: map basic blocks to set of dead
      // variables at the end of the blocks
      std::unordered_map<basic_block_label_t, varset_domain_t> _dead_map;

      // statistics
      unsigned m_max_live;
      unsigned m_total_live;
      unsigned m_total_blocks;

    public:

      //for backward compatibility
      //XXX: maybe unused already
      typedef varset_domain_t set_t;

      liveness(CFG cfg)
	: m_cfg(cfg)
	, m_max_live(0)
	, m_total_live(0)
	, m_total_blocks (0) { }

      liveness(const liveness<CFG>& other) = delete;

      liveness<CFG>& operator=(const liveness<CFG>& other) = delete;

      void exec() {
	/** Remove dead variables locally **/

	liveness_analysis_t live(m_cfg);
	live.exec();
	for (auto &bb: boost::make_iterator_range(m_cfg.begin(), m_cfg.end())) {
	  varset_domain_t live_set = live.get(bb.label());
	  if (live_set.is_bottom()) continue;

	  varset_domain_t dead_set = m_cfg.get_node(bb.label()).live();
	  // dead variables = (USE(bb) U DEF(bb)) \ live_out(bb)
	  dead_set -= live_set;
	  CRAB_LOG("liveness",
		   crab::outs() << cfg_impl::get_label_str(bb.label())
		                << " dead variables=" << dead_set <<"\n";);
	  _dead_map.insert (std::make_pair(bb.label(), dead_set));
	  // update statistics
	  m_total_live += live_set.size ();
	  m_max_live = std::max (m_max_live, live_set.size ());
	  m_total_blocks ++;
	}
      }

      // Return the set of dead variables at the exit of block bb
      varset_domain_t dead_exit (basic_block_label_t bb) const {
	auto it = _dead_map.find(bb);
	if (it == _dead_map.end()) {
	  return varset_domain_t();
	} else {
	  return it->second;
	}
      }

      void get_stats(unsigned& total_live,
		     unsigned& max_live_per_blk,
		     unsigned& avg_live_per_blk)  const {
	total_live = m_total_live;
	max_live_per_blk = m_max_live;
	avg_live_per_blk = (m_total_blocks == 0 ? 0 : (int) m_total_live / m_total_blocks);
      }

      void write (crab_os &o) const {
	o << "TODO: printing dead variable analysis results\n";
      }
    };

   template<typename CFG>
   inline crab_os& operator<<(crab_os& o, const liveness<CFG> &l) {
     l.write(o);
     return o;
   }

  } // end namespace analyzer
} // end namespace crab


// Boost.Geometry

// Copyright (c) 2016 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#include "test_overlaps.hpp"

#include <algorithms/overlay/overlay_cases.hpp>
#include <algorithms/overlay/multi_overlay_cases.hpp>

#include <boost/geometry/geometries/geometries.hpp>

template <typename P>
void test_polygon_polygon()
{
    typedef bg::model::polygon<P> poly;
    typedef bg::model::ring<P> ring;

    test_geometry<ring, ring>(case_1[0], case_1[1],
                              true);
    test_geometry<ring, poly>(case_1[0], case_1[1],
                              true);

    test_geometry<poly, poly>(case_1[0], case_1[1],
                              true);
    test_geometry<poly, poly>(case_2[0], case_2[1],
                              true);
    test_geometry<poly, poly>(case_3_sph[0], case_3_sph[1],
                              false);
    test_geometry<poly, poly>(case_3_2_sph[0], case_3_2_sph[1],
                              false);
    test_geometry<poly, poly>(case_4[0], case_4[1],
                              true);
    test_geometry<poly, poly>(case_5[0], case_5[1],
                              true);
    test_geometry<poly, poly>(case_6_sph[0], case_6_sph[1],
                              false);

    test_geometry<poly, poly>(case_7[0], case_7[1],
                              false);
    test_geometry<poly, poly>(case_8_sph[0], case_8_sph[1],
                              false);
    test_geometry<poly, poly>(case_9_sph[0], case_9_sph[1],
                              false);
    test_geometry<poly, poly>(case_10_sph[0], case_10_sph[1],
                              false);
    test_geometry<poly, poly>(case_11_sph[0], case_11_sph[1],
                              false);
    test_geometry<poly, poly>(case_12[0], case_12[1],
                              true);

    test_geometry<poly, poly>(case_13_sph[0], case_13_sph[1],
                              false);
    test_geometry<poly, poly>(case_14_sph[0], case_14_sph[1],
                              false);
    test_geometry<poly, poly>(case_15_sph[0], case_15_sph[1],
                              false);
    test_geometry<poly, poly>(case_16_sph[0], case_16_sph[1],
                              false);
    test_geometry<poly, poly>(case_17_sph[0], case_17_sph[1],
                              false);
    test_geometry<poly, poly>(case_18_sph[0], case_18_sph[1],
                              false);
}

template <typename P>
void test_polygon_multi_polygon()
{
    typedef bg::model::polygon<P> poly;
    typedef bg::model::ring<P> ring;
    typedef bg::model::multi_polygon<poly> mpoly;

    test_geometry<ring, mpoly>(case_1[0], case_multi_2[0],
                               true);
    test_geometry<poly, mpoly>(case_2[0], case_multi_2[0],
                               true);
}

template <typename P>
void test_multi_polygon_multi_polygon()
{
    typedef bg::model::polygon<P> poly;
    typedef bg::model::multi_polygon<poly> mpoly;

    test_geometry<mpoly, mpoly>(case_multi_2[0], case_multi_2[1],
                                true);
}

template <typename P>
void test_linestring_linestring()
{
    typedef bg::model::linestring<P> ls;

    test_geometry<ls, ls>("LINESTRING(0 0, 2 2, 3 2)", "LINESTRING(0 0, 2 2, 3 2)", false);

    test_geometry<ls, ls>("LINESTRING(1 0,2 2,2 3)", "LINESTRING(0 0, 2 2, 3 2)", false);
}

template <typename P>
void test_linestring_multi_linestring()
{
    typedef bg::model::linestring<P> ls;
    typedef bg::model::multi_linestring<ls> mls;

    test_geometry<ls, mls>("LINESTRING(0 0,10 0)",
                           "MULTILINESTRING((1 0,2 0),(1 1,2 1))",
                           true);

    test_geometry<ls, mls>("LINESTRING(0 0,5 0,5 5,0 5,0 0)",
                           "MULTILINESTRING((5 5,0 5,0 0),(0 0,5 0,5 5))",
                           false);
}

template <typename P>
void test_multi_linestring_multi_linestring()
{
    typedef bg::model::linestring<P> ls;
    typedef bg::model::multi_linestring<ls> mls;

    test_geometry<mls, mls>("MULTILINESTRING((0 0,0 0,18 0,18 0,19 0,19 0,19 0,30 0,30 0))",
                            "MULTILINESTRING((0 10,5 0,20 0,20 0,30 0))",
                            true);
}

template <typename P>
void test_all()
{
    test_polygon_polygon<P>();
    test_polygon_multi_polygon<P>();
    test_multi_polygon_multi_polygon<P>();

    test_linestring_linestring<P>();
    test_linestring_multi_linestring<P>();
    test_multi_linestring_multi_linestring<P>();
}

int test_main( int , char* [] )
{
    test_all<bg::model::point<double, 2, bg::cs::spherical_equatorial<bg::degree> > >();

    return 0;
}


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/asin.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <boost/math/complex/asin.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/four.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/pio_2.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/i.hpp>

NT2_TEST_CASE_TPL ( asin_real,  NT2_REAL_TYPES)
{
  using nt2::asin;
  using nt2::tag::asin_;
  typedef typename std::complex<T> cT;
  typedef typename nt2::meta::call<asin_(cT)>::type r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, cT);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::Nan  <T>(), nt2::Zero<T>())), cT(nt2::Nan <T>(), nt2::Nan<T>()), 1);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::Nan  <T>(), nt2::One<T>())),  cT(nt2::Nan <T>(), nt2::Nan<T>()), 1);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::Nan  <T>(), nt2::Inf<T>())),  cT(nt2::Nan <T>(), nt2::Inf<T>()), 1);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::Nan  <T>(), nt2::Nan<T>())),  cT(nt2::Nan <T>(), nt2::Nan<T>()), 1);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::Zero <T>(), nt2::Nan <T>())), cT(nt2::Zero<T>(), nt2::Nan<T>()), 1);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::One  <T>(), nt2::Nan <T>())), cT(nt2::Nan<T>(),  nt2::Nan<T>()), 1);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::Inf  <T>(), nt2::Nan<T>())),  cT(nt2::Nan <T>(), nt2::Inf<T>()), 1);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::One  <T>(), nt2::Inf <T>())), cT(nt2::Zero<T>(), nt2::Inf<T>()), 1);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::Inf  <T>(), nt2::One <T>())), cT(nt2::Pio_2<T>(), nt2::Inf <T>()), 1);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::Inf  <T>(), nt2::Inf<T>())),  cT(nt2::Pi<T>()/nt2::Four<T>(), nt2::Inf <T>()), 1);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::Nan  <T>(), -nt2::One<T>())),  cT(nt2::Nan <T>(), -nt2::Nan<T>()), 1);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::Nan  <T>(), -nt2::Inf<T>())),  cT(nt2::Nan <T>(), -nt2::Inf<T>()), 1);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::Nan  <T>(), -nt2::Nan<T>())),  cT(nt2::Nan <T>(), -nt2::Nan<T>()), 1);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::Zero <T>(), -nt2::Nan <T>())), cT(nt2::Zero<T>(), -nt2::Nan<T>()), 1);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::One  <T>(), -nt2::Nan <T>())), cT(nt2::Nan<T>(),  -nt2::Nan<T>()), 1);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::Inf  <T>(), -nt2::Nan<T>())),  cT(nt2::Nan <T>(),  nt2::Inf<T>()), 1);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::One  <T>(), -nt2::Inf <T>())), cT(nt2::Zero<T>(), -nt2::Inf<T>()), 1);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::Inf  <T>(), -nt2::One <T>())), cT(nt2::Pio_2<T>(), -nt2::Inf <T>()), 1);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::Inf  <T>(), -nt2::Inf<T>())),  cT(nt2::Pi<T>()/nt2::Four<T>(), -nt2::Inf <T>()), 1);
#endif
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::Zero<T>(),  nt2::Zero<T>())), cT(nt2::Zero<T>(), nt2::Zero<T>()), 1);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::One  <T>(), nt2::One <T>())),  cT(0.666239432492515, 1.061275061905036), 2);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::One  <T>(), nt2::Mone <T>())),  cT(0.666239432492515, -1.061275061905036), 2);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::Mone  <T>(), nt2::One <T>())),  cT(-0.666239432492515, 1.061275061905036), 2);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(nt2::Mone  <T>(), nt2::Mone <T>())), cT(-0.666239432492515, -1.061275061905036), 2);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(1, 1)),  boost::math::asin(cT(1, 1)), 1);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(1, 10)),  boost::math::asin(cT(1, 10)), 1);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(10, 10)),  boost::math::asin(cT(10, 10)), 1.5);
  NT2_TEST_ULP_EQUAL(nt2::asin(cT(10, 1)),  boost::math::asin(cT(10, 1)), 1);
}


/**
 * @file bidirectional_dijkstra.hpp
 * @author Leonardo Arcari (leonardo1.arcari@gmail.com)
 * @version 1.0.0
 * @date 2018-10-28
 *
 * @copyright Copyright (c) 2018 Leonardo Arcari
 *
 * MIT License
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */

#ifndef BOOST_BIDIRECTIONAL_DIJKSTRA_HPP
#define BOOST_BIDIRECTIONAL_DIJKSTRA_HPP

#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/reverse_graph.hpp>

#include <arlib/details/arlib_utils.hpp>
#include <arlib/routing_kernels/details/bidirectional_dijkstra_impl.hpp>
#include <arlib/routing_kernels/visitor.hpp>
#include <arlib/type_traits.hpp>

#include <deque>
#include <limits>
#include <vector>

/**
 * An Alternative-Routing library for Boost.Graph
 */
namespace arlib {
//===----------------------------------------------------------------------===//
//                   Bidirectional Dijkstra algorithm
//===----------------------------------------------------------------------===//
/**
 * Implementation of Bidirectional Dijkstra method for Boost::Graph to
 *        compute the shortest path between two vertices.
 *
 * This implementation follows the one from nx.graph library:
 * https://networkx.github.io/documentation/networkx-1.10/_modules/networkx/algorithms/shortest_paths/weighted.html#bidirectional_dijkstra
 *
 * You can find the original paper by Nicholson (1996)
 * https://academic.oup.com/comjnl/article/9/3/275/406281
 *
 * @tparam Graph A Boost::VertexAndEdgeListGraph
 * @tparam PredecessorMap The predecessor map records the edges in the shortest
 *         path tree, the tree computed by the traversal of the graph. Upon
 *         completion of the algorithm, the edges (p[u],u) for all u in V are in
 *         the tree. The shortest path from vertex s to each vertex v in the
 *         graph consists of the vertices v, p[v], p[p[v]], and so on until s is
 *         reached, in reverse order. The tree is not guaranteed to be a minimum
 *         spanning tree. If p[u] = u then u is either the source vertex or a
 *         vertex that is not reachable from the source. The PredecessorMap type
 *         must be a Read/Write Property Map whose key and value types are the
 *         same as the vertex descriptor type of the graph.
 * @tparam DistanceMap The shortest path weight from the source vertex s to each
 *         vertex in the graph g is recorded in this property map. The shortest
 *         path weight is the sum of the edge weights along the shortest path.
 *         The type DistanceMap must be a model of Read/Write Property Map. The
 *         vertex descriptor type of the graph needs to be usable as the key
 *         type of the distance map.
 * @tparam WeightMap The weight or ``length'' of each edge in the graph. The
 *         weights must all be non-negative, and the algorithm will throw a
 *         negative_edge exception is one of the edges is negative. The type
 *         WeightMap must be a model of Readable Property Map. The edge
 *         descriptor type of the graph needs to be usable as the key type for
 *         the weight map. The value type for this map must be the same as the
 *         value type of the distance map.
 * @tparam BackGraph A boost::reverse_graph<Graph>
 * @tparam BackPredecessorMap A PredecessorMap of a boost::reverse_graph<Graph>
 * @tparam BackDistanceMap A DistanceMap of a boost::reverse_graph<Graph>
 * @tparam BackWeightMap A WeightMap of a boost::reverse_graph<Graph>
 * @tparam BiDijkstraVisitorImpl An implementation of a BiDijkstraVisitor.
 * @tparam Vertex a vertex_descriptor.
 * @param G The graph.
 * @param s The source vertex.
 * @param t The target vertex.
 * @param predecessor The PredecessorMap for the forward step.
 * @param distance The DistanceMap for the forward step.
 * @param weight The WeightMap for the forward step.
 * @param G_b A boost::reverse_graph<Graph> of @p G.
 * @param predecessor_b The PredecessorMap for the backward step.
 * @param distance_b The DistanceMap for the backward step.
 * @param weight_b The WeightMap for the backward step.
 * @param visitor An implementation of a BiDijkstraVisitor.
 */
template <typename Graph, typename PredecessorMap, typename DistanceMap,
          typename WeightMap, typename BackGraph, typename BackPredecessorMap,
          typename BackDistanceMap, typename BackWeightMap,
          typename BiDijkstraVisitorImpl, typename Vertex = vertex_of_t<Graph>>
void bidirectional_dijkstra(const Graph &G, Vertex s, Vertex t,
                            PredecessorMap predecessor, DistanceMap distance,
                            WeightMap weight, const BackGraph &G_b,
                            BackPredecessorMap predecessor_b,
                            BackDistanceMap distance_b, BackWeightMap weight_b,
                            BiDijkstraVisitor<BiDijkstraVisitorImpl> &visitor) {
  using namespace boost;
  using Length = typename property_traits<DistanceMap>::value_type;
  using Edge = edge_of_t<Graph>;
  using RevEdge = edge_of_t<boost::reverse_graph<Graph>>;

  BOOST_CONCEPT_ASSERT((VertexAndEdgeListGraphConcept<Graph>));
  BOOST_CONCEPT_ASSERT((VertexAndEdgeListGraphConcept<BackGraph>));
  BOOST_CONCEPT_ASSERT((ReadablePropertyMapConcept<WeightMap, Edge>));
  BOOST_CONCEPT_ASSERT((ReadablePropertyMapConcept<BackWeightMap, RevEdge>));

  constexpr Length inf = std::numeric_limits<Length>::max();

  // Initialize distance structures
  details::init_distance_vector(G, distance);
  details::init_distance_vector(G_b, distance_b);

  // Initialize shortest path trees (forward and backward)
  predecessor[s] = s;
  predecessor_b[t] = t;

  // Init fringe structures
  auto fringe = details::Fringe<Vertex, Length>{};
  auto fringe_b = details::Fringe<Vertex, Length>{};
  fringe.push(std::make_pair(s, 0));
  fringe_b.push(std::make_pair(t, 0));

  // Init distances to nodes seen
  auto seen = details::Seen<Vertex, Length>{};
  auto seen_b = details::Seen<Vertex, Length>{};
  seen.insert({s, 0});
  seen_b.insert({t, 0});

  using details::BiDijkStepRes;
  using details::Direction;

  Length final_distance = inf;
  auto final_path = std::deque<Vertex>{};
  auto direction = Direction::backward;
  while (!fringe.empty() && !fringe_b.empty()) {
    direction = details::switch_direction(direction);
    auto result = BiDijkStepRes::next;

    // Run a step
    if (direction == Direction::forward) {
      result = details::bi_dijkstra_step(
          G, s, t, predecessor, distance, weight, fringe, seen, predecessor_b,
          distance_b, fringe_b, seen_b, direction, final_distance, final_path,
          visitor);
    } else {
      result = details::bi_dijkstra_step(
          G_b, s, t, predecessor_b, distance_b, weight_b, fringe_b, seen_b,
          predecessor, distance, fringe, seen, direction, final_distance,
          final_path, visitor);
    }

    // Handle the outcome of this step
    switch (result) {
    case BiDijkStepRes::next:
      // Go next
      break;
    case BiDijkStepRes::end:
      // Fill predecessor map
      details::fill_predecessor(predecessor, final_path);
      return;
    case BiDijkStepRes::negative_weights:
      throw std::domain_error{"Contradictory paths found: negative weights?"};
    default:
      // Unexpected result
      throw std::invalid_argument{"Unexpected result. Exiting..."};
    }
  }

  if (fringe.empty() || fringe_b.empty()) {
    if (final_path.empty()) {
      throw details::target_not_found{"No path found!"};
    } else {
      // Clean exit
      details::fill_predecessor(predecessor, final_path);
      return;
    }
  } else {
    // Otherwise
    throw details::target_not_found{"No path found!"};
  }
}

/**
 * Implementation of Bidirectional Dijkstra method for Boost::Graph to
 *        compute the shortest path between two vertices.
 *
 * This overload does not need a PredecessorMap nor a DistanceMap for the
 * backward step.
 *
 * @see bidirectional_dijkstra(const Graph &G, Vertex s, Vertex t,
 *                             PredecessorMap predecessor, DistanceMap distance,
 *                             WeightMap weight, const BackGraph &G_b,
 *                             BackPredecessorMap predecessor_b,
 *                             BackDistanceMap distance_b,
 *                             BackWeightMap weight_b,
 *                             BiDijkstraVisitor<BiDijkstraVisitorImpl>
 *                                 &visitor)
 *
 * @tparam BackIndexMap This maps each vertex to an integer in the range [0,
 *         num_vertices(g)). This is necessary for efficient updates of the heap
 *         data structure [61] when an edge is relaxed. The type VertexIndexMap
 *         must be a model of Readable Property Map. The value type of the map
 *         must be an integer type. The vertex descriptor type of the graph
 *         needs to be usable as the key type of the map.
 */
template <typename Graph, typename PredecessorMap, typename DistanceMap,
          typename WeightMap, typename BackGraph, typename BackWeightMap,
          typename BackIndexMap, typename BiDijkstraVisitorImpl,
          typename Vertex = vertex_of_t<Graph>>
void bidirectional_dijkstra(const Graph &G, Vertex s, Vertex t,
                            PredecessorMap predecessor, DistanceMap distance,
                            WeightMap weight, const BackGraph &G_b,
                            BackWeightMap weight_b, BackIndexMap index_map_b,
                            BiDijkstraVisitor<BiDijkstraVisitorImpl> &visitor) {
  using namespace boost;
  using Edge = typename graph_traits<Graph>::edge_descriptor;
  using RevEdge =
      typename graph_traits<boost::reverse_graph<Graph>>::edge_descriptor;
  using Length = typename property_traits<DistanceMap>::value_type;

  BOOST_CONCEPT_ASSERT((VertexAndEdgeListGraphConcept<Graph>));
  BOOST_CONCEPT_ASSERT((VertexAndEdgeListGraphConcept<BackGraph>));
  BOOST_CONCEPT_ASSERT((ReadablePropertyMapConcept<WeightMap, Edge>));
  BOOST_CONCEPT_ASSERT((ReadablePropertyMapConcept<BackWeightMap, RevEdge>));

  auto predecessor_b_vec =
      std::vector<Vertex>(vertices(G_b).first, vertices(G_b).second);
  auto predecessor_b =
      make_iterator_property_map(std::begin(predecessor_b_vec), index_map_b);

  auto distance_b_vec = std::vector<Length>(num_vertices(G_b));
  auto distance_b =
      make_iterator_property_map(std::begin(distance_b_vec), index_map_b);

  bidirectional_dijkstra(G, s, t, predecessor, distance, weight, G_b,
                         predecessor_b, distance_b, weight_b, visitor);
}

/**
 * Implementation of Bidirectional Dijkstra method for Boost::Graph to
 * compute the shortest path between two vertices.
 *
 * This overload does not need a PredecessorMap nor a DistanceMap for the
 * backward step nor a BiDijkstraVisitor. This simply runs Bidirectional
 * Dijkstra.
 *
 * @see bidirectional_dijkstra(const Graph &G, Vertex s, Vertex t,
 *                             PredecessorMap predecessor, DistanceMap distance,
 *                             WeightMap weight, const BackGraph &G_b,
 *                             BackPredecessorMap predecessor_b,
 *                             BackDistanceMap distance_b,
 *                             BackWeightMap weight_b,
 *                             BiDijkstraVisitor<BiDijkstraVisitorImpl>
 *                                 &visitor)
 *
 * @tparam BackIndexMap This maps each vertex to an integer in the range [0,
 *         num_vertices(g)). This is necessary for efficient updates of the heap
 *         data structure [61] when an edge is relaxed. The type VertexIndexMap
 *         must be a model of Readable Property Map. The value type of the map
 *         must be an integer type. The vertex descriptor type of the graph
 *         needs to be usable as the key type of the map.
 */
template <typename Graph, typename PredecessorMap, typename DistanceMap,
          typename WeightMap, typename BackGraph, typename BackWeightMap,
          typename BackIndexMap, typename Vertex = vertex_of_t<Graph>>
void bidirectional_dijkstra(const Graph &G, Vertex s, Vertex t,
                            PredecessorMap predecessor, DistanceMap distance,
                            WeightMap weight, const BackGraph &G_b,
                            BackWeightMap weight_b, BackIndexMap index_map_b) {
  using namespace boost;
  using Edge = typename graph_traits<Graph>::edge_descriptor;
  using RevEdge =
      typename graph_traits<boost::reverse_graph<Graph>>::edge_descriptor;

  BOOST_CONCEPT_ASSERT((VertexAndEdgeListGraphConcept<Graph>));
  BOOST_CONCEPT_ASSERT((VertexAndEdgeListGraphConcept<BackGraph>));
  BOOST_CONCEPT_ASSERT((ReadablePropertyMapConcept<WeightMap, Edge>));
  BOOST_CONCEPT_ASSERT((ReadablePropertyMapConcept<BackWeightMap, RevEdge>));

  auto visitor = IdentityBiDijkstraVisitor{};
  bidirectional_dijkstra(G, s, t, predecessor, distance, weight, G_b, weight_b,
                         index_map_b, visitor);
}
} // namespace arlib

#endif


#ifndef SME_HPP
#define SME_HPP

#include <cstddef>
#include <type_traits>
#include <boost/proto/proto.hpp>

#include "detail/evaluate.hpp"
#include "detail/domain.hpp"
#include "detail/terminal.hpp"
#include "detail/varstore.hpp"

namespace sme {

    namespace {
        sme::detail::domain::expr<
            boost::proto::terminal<
                sme::detail::terminal::scoped<>
            >::type
        > const scoped = {};
    }

    template<typename Expr>
    auto evaluate(Expr const& expr) {
        return detail::eval_result::get_value(detail::evaluate(expr, detail::varstore::empty()));
    }

} // namespace sme

// each var must have unique id
#define SME_DEFINE_VAR(NAME, ID)                                                                                \
    sme::detail::domain::expr<                                                                                \
        boost::proto::terminal<                                                                                \
            sme::detail::terminal::var<                                                                                \
                std::integral_constant<std::size_t, ID>                                                                     \
            >                                                                                \
        >::type                                                                                \
    > const NAME = {};

#define SME_DEFINE_FUNCTION(NAME, FUNCTION)                                                                                \
    struct NAME {                                                                                \
        template<typename... Args>                                                                                \
        decltype(auto) operator()(Args&&... args) const {                                                                   \
            return FUNCTION(std::forward<Args>(args)...);                                                                   \
        }                                                                                \
    };                                                                                \
                                                                                \
    sme::detail::domain::expr<                                                                                \
        typename boost::proto::terminal<                                                                                \
            sme::detail::terminal::original_function<NAME>                                                                  \
        >::type                                                                                \
    > const NAME = {};

#endif // SME_HPP


/*
    This file is part of WARG's computer-vision

    Copyright (c) 2015, Waterloo Aerial Robotics Group (WARG)
    All rights reserved.

    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions are met:
    1. Redistributions of source code must retain the above copyright
       notice, this list of conditions and the following disclaimer.
    2. Redistributions in binary form must reproduce the above copyright
       notice, this list of conditions and the following disclaimer in the
       documentation and/or other materials provided with the distribution.
    3. Usage of this code MUST be explicitly referenced to WARG and this code
       cannot be used in any competition against WARG.
    4. Neither the name of the WARG nor the names of its contributors may be used
       to endorse or promote products derived from this software without specific
       prior written permission.

    THIS SOFTWARE IS PROVIDED BY WARG ''AS IS'' AND ANY
    EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
    WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
    DISCLAIMED. IN NO EVENT SHALL WARG BE LIABLE FOR ANY
    DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
    ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
    (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <opencv2/imgproc.hpp>
#include <boost/log/trivial.hpp>
#include "object_detector.h"
#include "pixel_object.h"
#include <vector>
#include "frame.h"

using namespace std;
using namespace cv;

ObjectDetector::ObjectDetector(Filter * filter, ContourCreator * contourCreator){
    this->filter = filter;
    this->ccreator = contourCreator;
}

void ObjectDetector::process_frame(Frame * f){
    Mat & src = f->get_img();
    Mat * filtered = filter->filter(src);
    vector<vector<Point> > contours = *(ccreator->get_contours(*filtered));
    BOOST_LOG_TRIVIAL(info) << "Found " << contours.size() << " Contours in frame " << f->get_id();
    delete filtered;
    for(vector<Point> contour : contours){
        string type;
        Point2d centroid;
        double area;
        double perimeter;
        Scalar colour;
        Point2d error;
        double errorAngle;
        Mat crop;

        // get info from contours/image
        Moments mu = moments(contour, false);
        centroid = Point2d( mu.m10/mu.m00 , mu.m01/mu.m00 );
        perimeter = arcLength(contour, true);
        area = contourArea(contour);
        // TODO: Calculate location error
        Mat mask = Mat::zeros(f->get_img().size(), CV_8UC1);
        drawContours(mask, vector<vector<Point> >({contour}), 0, Scalar(255), CV_FILLED);
        colour = mean(f->get_img(), mask);

        PixelObject * p = new PixelObject(crop, contour, centroid, area, perimeter, colour, error, errorAngle);
        f->add_object(p);
    }
}

ObjectDetector::~ObjectDetector(){

}


#pragma once

#include "object.hpp"
#include <metaSMT/frontend/Logic.hpp>
#include <metaSMT/tags/Cardinality.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/contains.hpp>

namespace metaSMT {
  namespace cardinality {
    namespace bdd {
      namespace cardtags = metaSMT::logic::cardinality::tag;

      /**
       * Generalized cardinality constraint based on a construction
       * using Binary Decision Diagrams (BDD) by Eén and
       * Sörensson [1] and Bailleux et al. [2].
       *
       * [1] N. Eén and N. Sörensson. Translating
       * pseudo-boolean constraints into SAT.  In Journal on
       * Satisfiability, Boolean Modeling and Computation, volume 2,
       * pages 1-26, 2006.
       *
       * [2] O. Bailleux, Y. Boufkhad, and O. Roussel, A Translation of
       * Pseudo-Boolean Constraints to SAT, Journal on Satisfiability,
       * Boolean Modeling and Computation, volume 2, pages 191-200, 2006.
       *
       * The tableau algorithm keeps only two rows of the tableau in
       * memory which we call ``rails''.
       *
       * The construction applies to all operator types (<, <=, =, >=, >).
       * An operator is selected by assigning values to the arguments lt,
       * eq, gt, e.g., the less equal operator corresponds to lt = True,
       * eq = True, gt = False.
       *
       * For instance, the constraint
       *
       *    ps[0] + ps[1] + ps[2] + ps[3] <=> 3 with <=> in {<, >, =, <=, >=}
       *
       * corresponds to the following tableau
       *       RAILS  |   0    1     2     3
       *              |          lt    lt    lt
       *     ---------|------------------------
       *        0     |    eq ps[0] ps[1] ps[2]
       *        1  gt | ps[0] ps[1] ps[2] ps[3]
       *
       * From the bottom right node of the tableau a logic formula is
       * constructed using the ITE-operator where the left neighbor node
       * and the top neighbor node serve as the then and else part,
       * respectively.
       *
       * Moreover, notice that cells with entries lt and gt are not
       * explicitly saved in rails but are directly encoded when the
       * formula is created.
       *
       * Precondition: ps.size > 0 && cardinality > 0 && ps.size() > cardinality
       */
      template < typename Context >
      typename Context::result_type formula(Context &ctx, unsigned n, typename Context::result_type r) {
        using namespace metaSMT::logic;
        unsigned const magic_number = 0;
        if ( n > magic_number ) {
          predicate p = new_variable();
          metaSMT::assertion( ctx, logic::equal(p, r) );
          return evaluate(ctx, p);
        }
        else {
          return r;
        }
      }

      template <typename Context, typename Boolean, typename LT_Expr, typename EQ_Expr, typename GT_Expr>
      typename Context::result_type
      cardinality_any(Context &ctx, std::vector<Boolean> const &ps, unsigned cardinality,
                      LT_Expr lt, EQ_Expr eq, GT_Expr gt) {
        using namespace metaSMT::logic;
        assert(ps.size() > 0 && "Cardinality constraint requires at least one input variable");
        assert(cardinality > 0 && "Unsatisfied precondition for tableau construction");

        assert( ps.size() > cardinality );

        unsigned const rail_size = cardinality+1;
        std::vector<typename Context::result_type> rails[2];
        rails[0].resize(rail_size);
        rails[1].resize(rail_size);

        for (unsigned v = 0; v < ps.size() - cardinality + 1; ++v) {
          for (unsigned u = 0; u < cardinality + 1; ++u) {
            if (u == 0 && v == 0) {
              rails[0][0] = formula(ctx, u+v, evaluate(ctx, eq));
            } else if (u == 0) {
              rails[v%2][0] = formula(ctx, u+v, evaluate(ctx, Ite(evaluate(ctx, ps[v-1]), gt, rails[(v-1)%2][0])));
            } else if (v == 0) {
              rails[0][u] = formula(ctx, u+v, evaluate(ctx, Ite(evaluate(ctx, ps[u-1]), rails[0][u-1], lt)));
            } else {
              rails[v%2][u] = formula(ctx, u+v, evaluate(ctx, Ite(ps[u+v-1], rails[v%2][u-1], rails[(v-1)%2][u])));
            }
          }
        }
        return rails[(ps.size() - cardinality)%2][cardinality];
      }

      template < typename Context, typename Tag, typename Boolean >
      typename Context::result_type
      cardinality(Context &, cardinality::Cardinality< Tag, Boolean > const &) {
        /** error: unknown tag **/
        BOOST_MPL_ASSERT_NOT( (boost::mpl::contains<cardtags::Cardinality_Tags, Tag>) );
      }

      template < typename Context, typename Boolean >
      typename Context::result_type
      cardinality(Context &ctx, cardinality::Cardinality<cardtags::eq_tag, Boolean> const &c) {
        std::vector<Boolean> const &ps = c.ps;
        unsigned const cardinality = c.cardinality;

        assert(ps.size() > 0 && "Equality cardinality constraint requires at least one input variable");

        return cardinality_any(ctx, ps, cardinality, logic::False, logic::True, logic::False);
      }

      template < typename Context, typename Boolean >
      typename Context::result_type
      cardinality(Context &ctx, cardinality::Cardinality<cardtags::lt_tag, Boolean> const &c) {
        std::vector<Boolean> const &ps = c.ps;
        unsigned const cardinality = c.cardinality;

        assert(ps.size() > 0 && "Lower than cardinality constraint requires at least one input variable");
        return evaluate(ctx, logic::Not(cardinality_geq(ctx, ps, cardinality)));
      }

      template < typename Context, typename Boolean >
      typename Context::result_type
      cardinality(Context &ctx, cardinality::Cardinality<cardtags::le_tag, Boolean> const &c) {
        std::vector<Boolean> const &ps = c.ps;
        unsigned const cardinality = c.cardinality;

        assert(ps.size() > 0 && "Lower equal cardinality constraint requires at least one input variable");

        return cardinality_any(ctx, ps, cardinality, logic::True, logic::True, logic::False);
      }

      template < typename Context, typename Boolean >
      typename Context::result_type
      cardinality(Context &ctx, cardinality::Cardinality<cardtags::ge_tag, Boolean> const &c) {
        std::vector<Boolean> const &ps = c.ps;
        unsigned const cardinality = c.cardinality;
        assert(cardinality != 0);
        assert(ps.size() > 0 && "Greater equal cardinality constraint requires at least one input variable");

        return cardinality_any(ctx, ps, cardinality, logic::False, logic::True, logic::True);
      }

      template < typename Context, typename Boolean >
      typename Context::result_type
      cardinality(Context &ctx, cardinality::Cardinality<cardtags::gt_tag, Boolean> const &c) {
        std::vector<Boolean> const &ps = c.ps;
        unsigned const cardinality = c.cardinality;

        assert(ps.size() > 0 && "Greater than cardinality constraint requires at least one input variable");
        return evaluate(ctx, logic::Not(cardinality_leq(ctx, ps, cardinality)));
      }
    } // bdd
  } // cardinality
} // metaSMT