//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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
//==============================================================================
#ifndef NT2_ELLIPTIC_FUNCTIONS_GENERIC_ELLIPJ_HPP_INCLUDED
#define NT2_ELLIPTIC_FUNCTIONS_GENERIC_ELLIPJ_HPP_INCLUDED

#include <nt2/elliptic/functions/ellipj.hpp>
#include <boost/fusion/tuple.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/functions/simd/am.hpp>
#include <nt2/include/functions/simd/if_zero_else.hpp>
#include <nt2/include/functions/simd/is_eqz.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/functions/simd/oneminus.hpp>
#include <nt2/include/functions/simd/sincos.hpp>
#include <nt2/include/functions/simd/sqr.hpp>
#include <nt2/include/functions/simd/sqrt.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  (ellipj_, tag::cpu_,
                             (A0)(A1),
                             (generic_<unspecified_<A0> >)
                             (generic_<floating_<A1> >)

                            )
  {
    typedef boost::fusion::tuple<A0,A0,A0>        result_type;

    inline result_type operator()(A0 const& a0,A1 const & a1) const
    {
      typedef typename nt2::meta::scalar_of<A0>::type sA0;
      A0 s, c, d;
      nt2::ellipj(a0, a1, nt2::Eps<sA0>(), s, c, d);
      return result_type(s, c, d);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  (ellipj_, tag::cpu_,
                             (A0)(A1)(A2),
                             (generic_<unspecified_<A0> >)
                             (generic_<floating_<A1> >)
                             (scalar_<floating_<A2> >)

                            )
  {
    typedef boost::fusion::tuple<A0,A0,A0>        result_type;

    inline result_type operator()(A0 const& a0,A1 const & a1,A2 const & a2) const
    {
      A0 s, c, d;
      nt2::ellipj(a0, a1, a2, s, c, d);
      return result_type(s, c, d);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  (  ellipj_, tag::cpu_,
                               (A0)(A1)(A2),
                               (generic_<unspecified_<A0> >)
                               (generic_<floating_<A1> >)
                               (scalar_<floating_<A2> >)
                               (generic_<unspecified_<A0> >)
                            )
  {
    typedef boost::fusion::tuple<A0,A0>        result_type;

    inline result_type operator()(A0 const& a0,A1 const & a1,A2 const & a2,A0 & a3) const
    {
      A0 s, c;
      nt2::ellipj(a0,a1,a2,s,c,a3);
      return result_type(s, c);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  (  ellipj_, tag::cpu_,
                               (A0)(A1)(A2),
                               (generic_<unspecified_<A0> >)
                               (generic_<floating_<A1> >)
                               (unspecified_<A2>)               //passing _()
                               (generic_<unspecified_<A0> >)
                            )
  {
    typedef boost::fusion::tuple<A0,A0>        result_type;

    inline result_type operator()(A0 const& a0,A1 const & a1,A2 const &,A0 & a3) const
    {
      typedef typename nt2::meta::scalar_of<A0>::type sA0;
      A0 s, c;
      nt2::ellipj(a0,a1,Eps<sA0>(),s,c,a3);
      return result_type(s, c);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  (  ellipj_, tag::cpu_,
                               (A0)(A1)(A2),
                               (generic_<unspecified_<A0> >)
                               (generic_<floating_<A1> >)
                               (scalar_<floating_<A2> >)
                               (generic_<unspecified_<A0> >)
                               (generic_<unspecified_<A0> >)
                            )
  {
    typedef A0 result_type;
    inline result_type operator()(A0 const& a0,A1 const & a1,A2 const & a2,A0 & a3,A0 & a4) const
    {
      A0 s;
      nt2::ellipj(a0,a1,a2,s,a3,a4);
      return s;
    }
  };

  BOOST_DISPATCH_IMPLEMENT  (  ellipj_, tag::cpu_,
                               (A0)(A1)(A2),
                               (generic_<unspecified_<A0> >)
                               (generic_<floating_<A1> >)
                               (unspecified_<A2>)         //passing _()
                               (generic_<unspecified_<A0> >)
                               (generic_<unspecified_<A0> >)
                            )
  {
    typedef A0 result_type;
    inline result_type operator()(A0 const& a0,A1 const & a1,A2 const &,A0 & a3,A0 & a4) const
    {
      typedef typename nt2::meta::scalar_of<A0>::type sA0;
      A0 s;
      nt2::ellipj(a0,a1,Eps<sA0>(),s,a3,a4);
      return s;
    }
  };

  BOOST_DISPATCH_IMPLEMENT  (  ellipj_, tag::cpu_,
                               (A0)(A1)(A2),
                               (generic_<floating_<A0> >)
                               (generic_<floating_<A1> >)
                               (scalar_<floating_<A2> >)
                               (generic_<floating_<A0> >)
                               (generic_<floating_<A0> >)
                               (generic_<floating_<A0> >)
                            )
  {
    typedef void result_type;
    inline result_type operator()(A0 const& a0, A1 const & a1, A2 const & a2,
                                  A0 & a3, A0 & a4, A0 & a5) const
    {
      A0 a = am(a0, a1, a2, 'm');
      a3 = nt2::sincos(a, a4);
      a5 = nt2::sqrt(oneminus(if_zero_else(is_eqz(a1), a1*nt2::sqr(a3))));
    }
  };

  BOOST_DISPATCH_IMPLEMENT  (  ellipj_, tag::cpu_,
                               (A0)(A1)(A2),
                               (generic_<floating_<A0> >)
                               (generic_<floating_<A1> >)
                               (unspecified_<A2>)       //passing _()
                               (generic_<floating_<A0> >)
                               (generic_<floating_<A0> >)
                               (generic_<floating_<A0> >)
                            )
  {
    typedef void result_type;
    inline result_type operator()(A0 const& a0, A1 const & a1, A2 const &,
                                  A0 & a3, A0 & a4, A0 & a5) const
    {
      typedef typename nt2::meta::scalar_of<A0>::type sA0;
      A0 a = am(a0, a1, Eps<sA0>(), 'm');
      a3 = nt2::sincos(a, a4);
      a5 = nt2::sqrt(oneminus(if_zero_else(is_eqz(a1), a1*nt2::sqr(a3))));
    }
  };

} }
#endif


#include "icp-ceres.h"

#include <Eigen/Dense>
#include <math.h>
#include <unordered_map>
#include <vector>

//#include "Visualize.h"

#include <ceres/local_parameterization.h>
#include <ceres/autodiff_local_parameterization.h>
#include <ceres/types.h>
#include <ceres/rotation.h>
#include <ceres/ceres.h>

#include <ceres/loss_function.h>

#define useLocalParam

namespace ICP_Ceres {

/*
Ceres Solving FAQ extracted from http://ceres-solver.org/solving_faqs.html:

1. For small (a few hundred parameters) or dense problems use DENSE_QR.

2. For general sparse problems (i.e., the Jacobian matrix has a substantial number of zeros) use SPARSE_NORMAL_CHOLESKY.
This requires that you have SuiteSparse or CXSparse installed.

3. For bundle adjustment problems with up to a hundred or so cameras, use DENSE_SCHUR.

4. For larger bundle adjustment problems with sparse Schur Complement/Reduced camera matrices use SPARSE_SCHUR.
This requires that you build Ceres with support for SuiteSparse, CXSparse or Eigen’s sparse linear algebra libraries.
If you do not have access to these libraries for whatever reason, ITERATIVE_SCHUR with SCHUR_JACOBI is an excellent alternative.

5. For large bundle adjustment problems (a few thousand cameras or more) use the ITERATIVE_SCHUR solver.
There are a number of preconditioner choices here. SCHUR_JACOBI offers an excellent balance of speed and accuracy.
This is also the recommended option if you are solving medium sized problems for which DENSE_SCHUR is too slow but SuiteSparse is not available.
Note: If you are solving small to medium sized problems, consider setting Solver::Options::use_explicit_schur_complement to true, it can result in a substantial performance boost.
If you are not satisfied with SCHUR_JACOBI‘s performance try CLUSTER_JACOBI and CLUSTER_TRIDIAGONAL in that order. They require that you have SuiteSparse installed.
Both of these preconditioners use a clustering algorithm. Use SINGLE_LINKAGE before CANONICAL_VIEWS.
*/
ceres::Solver::Options getOptions(){
    // Set a few options
    ceres::Solver::Options options;
    //options.use_nonmonotonic_steps = true;
    //options.preconditioner_type = ceres::IDENTITY;
    options.linear_solver_type = ceres::DENSE_QR;
    options.max_num_iterations = 50;

//    options.preconditioner_type = ceres::SCHUR_JACOBI;
//    options.linear_solver_type = ceres::DENSE_SCHUR;
//    options.use_explicit_schur_complement=true;
//    options.max_num_iterations = 100;

    cout << "Ceres Solver getOptions()" << endl;
    cout << "Ceres preconditioner type: " << options.preconditioner_type << endl;
    cout << "Ceres linear algebra type: " << options.sparse_linear_algebra_library_type << endl;
    cout << "Ceres linear solver type: " << options.linear_solver_type << endl;

    return options;
}

ceres::Solver::Options getOptionsMedium(){
    // Set a few options
    ceres::Solver::Options options;

    #ifdef _WIN32
        options.sparse_linear_algebra_library_type = ceres::EIGEN_SPARSE;
        options.linear_solver_type = ceres::ITERATIVE_SCHUR;
        options.preconditioner_type = ceres::SCHUR_JACOBI;
    #else
        //options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
        options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
    #endif // _WIN32

    //If you are solving small to medium sized problems, consider setting Solver::Options::use_explicit_schur_complement to true, it can result in a substantial performance boost.
    options.use_explicit_schur_complement=true;
    options.max_num_iterations = 50;

    cout << "Ceres Solver getOptionsMedium()" << endl;
    cout << "Ceres preconditioner type: " << options.preconditioner_type << endl;
    cout << "Ceres linear algebra type: " << options.sparse_linear_algebra_library_type << endl;
    cout << "Ceres linear solver type: " << options.linear_solver_type << endl;

    return options;
}

void solve(ceres::Problem &problem, bool smallProblem=false){
    ceres::Solver::Summary summary;
    ceres::Solve(smallProblem ? getOptions() : getOptionsMedium(), &problem, &summary);
    if(!smallProblem) std::cout << "Final report:\n" << summary.FullReport();
}

void isoToAngleAxis(const Isometry3d& pose, double* cam){
//    Matrix<const double,3,3> rot(pose.linear());
//    cout<<"rotation : "<<pose.linear().data()<<endl;
//    auto begin = pose.linear().data();
    RotationMatrixToAngleAxis(ColumnMajorAdapter4x3(pose.linear().data()), cam);
    Vector3d t(pose.translation());
    cam[3]=t.x();
    cam[4]=t.y();
    cam[5]=t.z();
}

Isometry3d axisAngleToIso(const double* cam){
    Isometry3d poseFinal = Isometry3d::Identity();
    Matrix3d rot;
    ceres::AngleAxisToRotationMatrix(cam,rot.data());
    poseFinal.linear() = rot;
    poseFinal.translation() = Vector3d(cam[3],cam[4],cam[5]);
    return poseFinal;//.cast<float>();
}

Isometry3d eigenQuaternionToIso(const Eigen::Quaterniond& q, const Vector3d& t){
    Isometry3d poseFinal = Isometry3d::Identity();
    poseFinal.linear() = q.toRotationMatrix();
    poseFinal.translation() = t;
    return poseFinal;//.cast<float>();
}

Sophus::SE3d isoToSophus(const Isometry3d& pose){
    return Sophus::SE3d(pose);
}

Isometry3d sophusToIso(Sophus::SE3d soph){
    //    return Isometry3d(soph.matrix());
    Isometry3d poseFinal = Isometry3d::Identity();
    poseFinal.linear() = soph.rotationMatrix();
    poseFinal.translation() = soph.translation();
    return poseFinal;
}

Isometry3d pointToPoint_CeresAngleAxis(vector<Vector3d>&src,vector<Vector3d>&dst){

    double cam[6] = {0,0,0,0,0,0};

    ceres::Problem problem;

    for (int i = 0; i < src.size(); ++i) {
        // first viewpoint : dstcloud, fixed
        // second viewpoint: srcCloud, moves
        ceres::CostFunction* cost_function = ICPCostFunctions::PointToPointError_CeresAngleAxis::Create(dst[i],src[i]);
        problem.AddResidualBlock(cost_function, NULL, cam);
    }

    solve(problem);

    return axisAngleToIso(cam);
}

Isometry3d pointToPoint_EigenQuaternion(vector<Vector3d>&src,vector<Vector3d>&dst){
    Eigen::Quaterniond q = Eigen::Quaterniond::Identity();
    Eigen::Vector3d t(0,0,0);

    ceres::Problem problem;

    for (int i = 0; i < src.size(); ++i) {
        // first viewpoint : dstcloud, fixed
        // second viewpoint: srcCloud, moves
        ceres::CostFunction* cost_function = ICPCostFunctions::PointToPointError_EigenQuaternion::Create(dst[i],src[i]);
        problem.AddResidualBlock(cost_function, NULL, q.coeffs().data(), t.data());
    }

#ifdef useLocalParam
    ceres::LocalParameterization *quaternion_parameterization = new eigen_quaternion::EigenQuaternionParameterization;
    problem.SetParameterization(q.coeffs().data(),quaternion_parameterization);
#endif

    solve(problem);

    return eigenQuaternionToIso(q,t);
}

Isometry3d pointToPlane_CeresAngleAxis(vector<Vector3d> &src,vector<Vector3d> &dst,vector<Vector3d> &nor){

    double cam[6] = {0,0,0,0,0,0};

    ceres::Problem problem;

    for (int i = 0; i < src.size(); ++i) {
        // first viewpoint : dstcloud, fixed
        // second viewpoint: srcCloud, moves
        // nor is normal of dst
        ceres::CostFunction* cost_function = ICPCostFunctions::PointToPlaneError_CeresAngleAxis::Create(dst[i],src[i],nor[i]);
        problem.AddResidualBlock(cost_function, NULL, cam);
    }

    solve(problem);

    return axisAngleToIso(cam);
}

Isometry3d pointToPlane_EigenQuaternion(vector<Vector3d>&src,vector<Vector3d>&dst,vector<Vector3d> &nor){
    Eigen::Quaterniond q = Eigen::Quaterniond::Identity();
    Eigen::Vector3d t(0,0,0);

    ceres::Problem problem;

    for (int i = 0; i < src.size(); ++i) {
        // first viewpoint : dstcloud, fixed
        // second viewpoint: srcCloud, moves
        ceres::CostFunction* cost_function = ICPCostFunctions::PointToPlaneError_EigenQuaternion::Create(dst[i],src[i],nor[i]);
        problem.AddResidualBlock(cost_function, NULL, q.coeffs().data(), t.data());
    }

#ifdef useLocalParam
    ceres::LocalParameterization *quaternion_parameterization = new eigen_quaternion::EigenQuaternionParameterization;
    problem.SetParameterization(q.coeffs().data(),quaternion_parameterization);
#endif

    solve(problem);

    return eigenQuaternionToIso(q,t);
}

void ceresOptimizer(vector< std::shared_ptr<Frame> >& frames, bool pointToPlane, bool robust){

    ceres::Problem problem;

//    double* cameras = new double[frames.size()*7]; //4 quaternion, 3 translation

    vector<Eigen::Quaterniond> qs(frames.size());
    vector<Eigen::Vector3d> ts(frames.size());

    //extract initial camera poses

    for(int i=0; i<frames.size(); i++){
      Isometry3d originalPose = frames[i]->pose;
      Eigen::Quaterniond q;// = Eigen::Map<Eigen::Quaterniond>(cameras+i*7);
      Eigen::Vector3d t;// = Eigen::Map<Eigen::Vector3d>(cameras+i*7+4);

      q=Eigen::Quaterniond(originalPose.linear());
      t=Eigen::Vector3d(originalPose.translation());

      qs[i]=q;
      ts[i]=t;

      if (i==0){
          frames[i]->fixed=true;
      }
    }

    cout<<"ok ceres"<<endl;

//    Visualize::spin(1);

    //add edges
    for(int src_id=0; src_id<frames.size(); src_id++){

        Frame& srcCloud = *frames[src_id];
        if(srcCloud.fixed) continue;

        Eigen::Quaterniond& srcQ = qs[src_id];
        Eigen::Vector3d& srcT = ts[src_id];

        for (int j = 0; j < srcCloud.neighbours.size(); ++j) {

            OutgoingEdge& dstEdge = srcCloud.neighbours[j];
            Frame& dstCloud = *frames.at(dstEdge.neighbourIdx);

            int dst_id=dstEdge.neighbourIdx;

            Eigen::Quaterniond& dstQ = qs[dst_id];
            Eigen::Vector3d& dstT = ts[dst_id]; //dstCloud

            for(auto corr : dstEdge.correspondances){

                // first viewpoint : dstcloud, fixed
                // second viewpoint: srcCloud, moves

                ceres::CostFunction* cost_function;

                if(pointToPlane){
                    cost_function = ICPCostFunctions::PointToPlaneErrorGlobal::Create(dstCloud.pts[corr.second],srcCloud.pts[corr.first],dstCloud.nor[corr.second]);
                }else{
                    cost_function = ICPCostFunctions::PointToPointErrorGlobal::Create(dstCloud.pts[corr.second],srcCloud.pts[corr.first]);
                }

                ceres::LossFunction* loss = NULL;
                if(robust) loss = new ceres::SoftLOneLoss(dstEdge.weight);

//                cout<<dstEdge.weight<<endl;
//                problem.AddResidualBlock(cost_function,  srcQ.coeffs().data(),srcT.data(),dstQ.coeffs().data(), dstT.data());
                problem.AddResidualBlock(cost_function, loss, srcQ.coeffs().data(),srcT.data(),dstQ.coeffs().data(), dstT.data());

//                problem.AddResidualBlock(cost_function, NULL, &cameras[src_id*7],&cameras[src_id*7+4],&cameras[dst_id*7],&cameras[dst_id*7+4]);

            }
        }
    }

#ifdef useLocalParam
    eigen_quaternion::EigenQuaternionParameterization *quaternion_parameterization = new eigen_quaternion::EigenQuaternionParameterization;
#endif

    for (int i = 0; i < frames.size(); ++i) {
        #ifdef useLocalParam
//        problem.SetParameterization(&cameras[i*7],quaternion_parameterization);
        problem.SetParameterization(qs[i].coeffs().data(),quaternion_parameterization);
        #endif

        if(frames[i]->fixed){
            std::cout<<i<<" fixed"<<endl;
//            problem.SetParameterBlockConstant(&cameras[i*7]);
//            problem.SetParameterBlockConstant(&cameras[i*7+4]);
            problem.SetParameterBlockConstant(qs[i].coeffs().data());
            problem.SetParameterBlockConstant(ts[i].data());
        }
    }

    solve(problem);

    //update camera poses
    for (int i = 0; i < frames.size(); ++i) {
//        poseFinal.linear() = Eigen::Map<Eigen::Quaterniond>(cameras+i*7).toRotationMatrix();
//        poseFinal.translation() = Eigen::Map<Eigen::Vector3d>(cameras+i*7+4);
        frames[i]->pose=eigenQuaternionToIso(qs[i],ts[i]);
    }
}

void ceresOptimizer_ceresAngleAxis(vector< std::shared_ptr<Frame> >& frames, bool pointToPlane,bool robust){

    ceres::Problem problem;

    double* cameras = new double[frames.size()*6];

    //extract initial camera poses
    for(int i=0; i<frames.size(); i++){
      isoToAngleAxis(frames[i]->pose,&cameras[i*6]);

//      cout<<"pose "<<i<<endl;
//      cout<<frames[i]->pose.matrix()<<endl;
//      cout<<"pose test "<<i<<endl;
//      cout<<axisAngleToIso(&cameras[i*6]).matrix()<<endl;

//      Visualize::spin();

      if (i==0){
          frames[i]->fixed=true;
      }
    }

    //add edges
    for(int src_id=0; src_id<frames.size(); src_id++){

        Frame& srcCloud = *frames[src_id];
        if(srcCloud.fixed) continue;

        for (int j = 0; j < srcCloud.neighbours.size(); ++j) {

            OutgoingEdge& dstEdge = srcCloud.neighbours[j];
            Frame& dstCloud = *frames.at(dstEdge.neighbourIdx);

            int dst_id=dstEdge.neighbourIdx;

            for(auto corr : dstEdge.correspondances){

                // first viewpoint : dstcloud, fixed
                // second viewpoint: srcCloud, moves

                ceres::CostFunction* cost_function;

                if(pointToPlane){
                    cost_function = ICPCostFunctions::PointToPlaneErrorGlobal_CeresAngleAxis::Create(dstCloud.pts[corr.second],srcCloud.pts[corr.first],dstCloud.nor[corr.second]);
                }else{
                    cost_function = ICPCostFunctions::PointToPointErrorGlobal_CeresAngleAxis::Create(dstCloud.pts[corr.second],srcCloud.pts[corr.first]);
                }

                ceres::LossFunction* loss = NULL;
                if(robust) loss = new ceres::SoftLOneLoss(dstEdge.weight);

                problem.AddResidualBlock(cost_function, loss, &cameras[src_id*6],&cameras[dst_id*6]);

            }
        }
    }

    for (int i = 0; i < frames.size(); ++i) {
        if(frames[i]->fixed){
            std::cout<<i<<" fixed"<<endl;
            problem.SetParameterBlockConstant(&cameras[i*6]);
        }
    }

    solve(problem);

    //update camera poses
    for (int i = 0; i < frames.size(); ++i) {
        frames[i]->pose=axisAngleToIso(&cameras[i*6]);
    }
}

void ceresOptimizer_sophusSE3(vector< std::shared_ptr<Frame> >& frames, bool pointToPlane,bool robust,bool automaticDiff){

    ceres::Problem problem;

    std::vector<Sophus::SE3d> cameras;

    //extract initial camera poses
    for(int i=0; i<frames.size(); i++){
      Sophus::SE3d soph = isoToSophus(frames[i]->pose);

//      cout<<"pose "<<i<<endl;
//      cout<<frames[i]->pose.matrix()<<endl;
//      cout<<"pose test "<<i<<endl;
//      cout<<sophusToIso(soph).matrix()<<endl;

//      Visualize::spin();
      cameras.push_back(soph);

      if (i==0){
          frames[i]->fixed=true;
      }
    }

    //add edges
    for(int src_id=0; src_id<frames.size(); src_id++){

        Frame& srcCloud = *frames[src_id];
        if(srcCloud.fixed) continue;

        for (int j = 0; j < srcCloud.neighbours.size(); ++j) {

            OutgoingEdge& dstEdge = srcCloud.neighbours[j];
            Frame& dstCloud = *frames.at(dstEdge.neighbourIdx);

            int dst_id=dstEdge.neighbourIdx;

            for(auto corr : dstEdge.correspondances){

                // first viewpoint : dstcloud, fixed
                // second viewpoint: srcCloud, moves

                ceres::CostFunction* cost_function;

                if(pointToPlane){
                    cost_function = ICPCostFunctions::PointToPlaneErrorGlobal_SophusSE3::Create(dstCloud.pts[corr.second],srcCloud.pts[corr.first],dstCloud.nor[corr.second]);
                }else{
                    cost_function = ICPCostFunctions::PointToPointErrorGlobal_SophusSE3::Create(dstCloud.pts[corr.second],srcCloud.pts[corr.first]);
                }

                ceres::LossFunction* loss = NULL;
                if(robust) loss = new ceres::SoftLOneLoss(dstEdge.weight);

                problem.AddResidualBlock(cost_function, loss, cameras[src_id].data(),cameras[dst_id].data());

            }
        }
    }
#ifdef useLocalParam
    ceres::LocalParameterization* param = sophus_se3::getParameterization(automaticDiff);
#endif
    for (int i = 0; i < frames.size(); ++i) {
        #ifdef useLocalParam
        problem.SetParameterization(cameras[i].data(),param);
        #endif
        if(frames[i]->fixed){
            std::cout<<i<<" fixed"<<endl;
            problem.SetParameterBlockConstant(cameras[i].data());
        }
    }

    solve(problem);

    //update camera poses
    for (int i = 0; i < frames.size(); ++i) {
        frames[i]->pose=sophusToIso(cameras[i]);
    }
}

//Isometry3d pointToPoint_SophusSE3(vector<Vector3d> &src,vector<Vector3d> &dst){
//    Sophus::SE3d soph = isoToSophus(Isometry3d::Identity());
//    Sophus::SE3d soph2 = isoToSophus(Isometry3d::Identity());

//    ceres::Problem problem;

//    for (int i = 0; i < src.size(); ++i) {
//        // first viewpoint : dstcloud, fixed
//        // second viewpoint: srcCloud, moves
//        ceres::CostFunction* cost_function = ICPCostFunctions::PointToPointErrorGlobal_SophusSE3::Create(dst[i],src[i]);
//        problem.AddResidualBlock(cost_function, NULL,soph.data(),soph2.data());
//    }

//    ceres::LocalParameterization* param = sophus_se3::getParameterization(false);

//    problem.SetParameterization(soph.data(),param);
//    problem.SetParameterization(soph2.data(),param);
//    problem.SetParameterBlockConstant(soph2.data());

//    solve(problem);

//    return sophusToIso(soph);
//}

//Isometry3d pointToPlane_SophusSE3(vector<Vector3d> &src,vector<Vector3d> &dst,vector<Vector3d> &nor){
//    Sophus::SE3d soph = isoToSophus(Isometry3d::Identity());
//    Sophus::SE3d soph2 = isoToSophus(Isometry3d::Identity());

//    ceres::Problem problem;

//    for (int i = 0; i < src.size(); ++i) {
//        // first viewpoint : dstcloud, fixed
//        // second viewpoint: srcCloud, moves
//        ceres::CostFunction* cost_function = ICPCostFunctions::PointToPlaneErrorGlobal_SophusSE3::Create(dst[i],src[i],nor[i]);
//        problem.AddResidualBlock(cost_function, NULL,soph.data(),soph2.data());
//    }

//    ceres::LocalParameterization* param = sophus_se3::getParameterization(false);

//    problem.SetParameterization(soph.data(),param);
//    problem.SetParameterization(soph2.data(),param);
//    problem.SetParameterBlockConstant(soph2.data());

//    solve(problem);

//    return sophusToIso(soph);
//}

Isometry3d pointToPoint_SophusSE3(vector<Vector3d> &src,vector<Vector3d> &dst,bool autodiff){
    Sophus::SE3d soph = isoToSophus(Isometry3d::Identity());

    ceres::Problem problem;

    for (int i = 0; i < src.size(); ++i) {
        // first viewpoint : dstcloud, fixed
        // second viewpoint: srcCloud, moves
        ceres::CostFunction* cost_function = ICPCostFunctions::PointToPointError_SophusSE3::Create(dst[i],src[i]);
        problem.AddResidualBlock(cost_function, NULL,soph.data());
    }
#ifdef useLocalParam
    ceres::LocalParameterization* param = sophus_se3::getParameterization(autodiff);
    problem.SetParameterization(soph.data(),param);
#endif

    solve(problem);

    return sophusToIso(soph);
}

Isometry3d pointToPlane_SophusSE3(vector<Vector3d> &src,vector<Vector3d> &dst,vector<Vector3d> &nor, bool autodiff){
    Sophus::SE3d soph = isoToSophus(Isometry3d::Identity());

    ceres::Problem problem;

    for (int i = 0; i < src.size(); ++i) {
        // first viewpoint : dstcloud, fixed
        // second viewpoint: srcCloud, moves
        ceres::CostFunction* cost_function = ICPCostFunctions::PointToPlaneError_SophusSE3::Create(dst[i],src[i],nor[i]);
        problem.AddResidualBlock(cost_function, NULL,soph.data());
    }
#ifdef useLocalParam
    ceres::LocalParameterization* param = sophus_se3::getParameterization(autodiff);
    problem.SetParameterization(soph.data(),param);
#endif

    solve(problem);

    return sophusToIso(soph);
}

} //end namespace


#include <test/test-models/good/variational/hier_logistic_cp.hpp>
#include <stan/variational/advi.hpp>
#include <stan/callbacks/writer.hpp>
#include <stan/callbacks/stream_logger.hpp>
#include <gtest/gtest.h>
#include <test/unit/util.hpp>
#include <vector>
#include <string>
#include <iostream>
#include <boost/random/additive_combine.hpp>  // L'Ecuyer RNG

typedef boost::ecuyer1988 rng_t;
typedef hier_logistic_cp_model_namespace::hier_logistic_cp_model Model_cp;

TEST(advi_test, hier_logistic_cp_constraint_meanfield) {
  // Create mock data_var_context
  std::fstream data_stream(
      "src/test/test-models/good/variational/hier_logistic.data.R",
      std::fstream::in);
  stan::io::dump data_var_context(data_stream);
  data_stream.close();

  std::stringstream output;
  output.clear();

  // Instantiate model
  Model_cp my_model(data_var_context);

  // RNG
  rng_t base_rng(0);

  // Dummy input
  Eigen::VectorXd cont_params = Eigen::VectorXd::Zero(my_model.num_params_r());

  // ADVI
  stan::variational::advi<Model_cp, stan::variational::normal_meanfield, rng_t>
      test_advi(my_model, cont_params, base_rng, 10, 100, 100, 1);

  stan::callbacks::writer writer;
  stan::callbacks::stream_logger logger(output, output, output, output, output);

  test_advi.run(0.01, false, 50, 1, 2e4, logger, writer, writer);
}


/*
 * Copyright 2018 James Dyer
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 *
 * This is being developed for the TANGO Project: http://tango-project.eu
 */

#include <sstream>
#include <NTL/RR.h>
#include <json/json.h>
#include "HE1NEncrypter.h"
#include "Random.h"

/**
 * Default destructor
 */
HE1NEncrypter::~HE1NEncrypter()
{
	delete kappamod;
}

HE1NEncrypter::HE1NEncrypter(int lambda, int eta, int nu){
	kappa = NTL::RandomPrime_ZZ(nu, 20);
	generateParameters(lambda,eta);
	NTL::ZZ_p::init(modulus);
	NTL::ZZ_p tmp = to_ZZ_p(kappa);
	kappamod = new NTL::ZZ_p(tmp);
}

HE1NEncrypter::HE1NEncrypter(int n, int d, int rho, int rhoprime)
{
	NTL::RR two(2);
	NTL::RR exp1(rho*d);
	NTL::RR nplusone(n+1);
	NTL::RR exp2(d);
	NTL::RR exp3(rho);
	NTL::RR kloBound = pow(two,exp1)*pow(nplusone,exp2);
	NTL::ZZ kappaLowerBound;
	conv(kappaLowerBound,kloBound);
	long nu = NumBits(kappaLowerBound)+1;
	kappa = NTL::RandomPrime_ZZ(nu, 20);
	NTL::RR kappareal = to_RR(kappa);
	NTL::RR ploBound = pow(pow(two,exp3)+pow(kappareal,two),exp2)*pow(nplusone,exp2);
	NTL::ZZ pLowerBound;
	conv(pLowerBound,ploBound);
	long lambda = NumBits(pLowerBound) + 1;
	long eta = ((lambda * lambda / rhoprime) - lambda);
	generateParameters(lambda,eta);
	NTL::ZZ_p::init(modulus);
	NTL::ZZ_p tmp = to_ZZ_p(kappa);
	kappamod = new NTL::ZZ_p(tmp);
};

HE1NEncrypter::HE1NEncrypter(std::string& secrets, std::string& parameters){
	Json::Value rootP;
	Json::Reader reader;
	bool parsingSuccessful = reader.parse(parameters,rootP);
	if (parsingSuccessful){
		modulus = NTL::conv<NTL::ZZ>(rootP["modulus"].asCString());
		NTL::ZZ_p::init(modulus);
		Json::Value rootS;
		bool parsingSuccessful = reader.parse(secrets,rootS);
		if (parsingSuccessful){
			p = NTL::conv<NTL::ZZ>(rootS["p"].asCString());
			NTL::ZZ_p ptmp = to_ZZ_p(p);
			pmod = new NTL::ZZ_p(ptmp);
			q = modulus/p;
			kappa = NTL::conv<NTL::ZZ>(rootS["kappa"].asCString());
			NTL::ZZ_p ktmp = to_ZZ_p(kappa);
			kappamod = new NTL::ZZ_p(ktmp);
		}
	}
}

NTL::vec_ZZ HE1NEncrypter::getKey()
{
	NTL::vec_ZZ ret;
	ret.append(p);
	ret.append(kappa);
    return ret;
};

NTL::ZZ_p HE1NEncrypter::encrypt(NTL::ZZ& plaintext)
{
    NTL::ZZ_p r = to_ZZ_p(rng->nextBigInteger(ONE, q));
    NTL::ZZ_p s = to_ZZ_p(rng->nextBigInteger(kappa));
    NTL::ZZ_p ptext = to_ZZ_p(plaintext);
    return ptext+r*(*pmod)+s*(*kappamod);
};

std::string HE1NEncrypter::writeSecretsToJSON()
{
	Json::Value root;
	std::stringstream pStr, kappaStr;
	pStr << p;
	kappaStr << kappa;
	root["p"] = pStr.str();
	root["kappa"] = kappaStr.str();
	Json::FastWriter writer;
	return writer.write(root);
};


//
// Created by brad on 1/4/16.
//

#include "MFCCs.h"
#include "armadillo"
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
//#include <log4cxx/logger.h>
#include <iostream>
//#include <boost/foreach.hpp>

using namespace std;

//initialize everything to default params
MFCCs::MFCCs(){// :
//  logger(log4cxx::Logger::getLogger("ade.asr.MFCCs")) {
  normVolume = false;
  numDiffs = 2;
  smoothFactor = .25;
}

MFCCs::~MFCCs() {

}

vector<vector<double>> MFCCs::getFeatures(std::vector<double> PCMData) {
  vector<vector<double>> out;

  //TODO: is it worth getting rid of this redundancy?
  //calculating minimum possible size for PCMData
  int fs = 16000;
  int Tw = 25;
  int Ts = 10;

  int numWindows = (int) round((1E-3) * Tw * fs);  //frame duration in samples
  int numShift = (int) round((1E-3) * Ts * fs);

  int minSegmentSize = -numShift + numWindows;

  if ((((int) PCMData.size() - numWindows) / numShift + 1) > 0) {

    //normalize volume
    if (normVolume) PCMData = normalizeVolume(PCMData);

    //caluclate MFCCS
    //will always smooth, can set smooth PCT to 0 for no smoothing
    vector<vector<double>> mfccs = smooth(calculateMFCCs(PCMData));

    if (numDiffs > 0) {

      vector<vector<double>> firstDiffs;
      vector<vector<double>> secondDiffs;

      if (numDiffs >= 1) {
        firstDiffs = smooth(getDiffs(mfccs));
      }
      if (numDiffs >= 2) {
        secondDiffs = smooth(getDiffs(firstDiffs));
      }
      if (numDiffs >= 3) {
        //LOG4CXX_WARN(logger, "we only support 2 diffs, and that's probably all you want");
        cout<< "we only support 2 diffs, and that's probably all you want";
      }

      //combine into single feature vector
      for (int i = 0; i < mfccs.size(); i++) {
        vector<double> combinedFeature;
        combinedFeature.insert(combinedFeature.end(), mfccs[i].begin(), mfccs[i].end());
        if (numDiffs >= 1) {
          combinedFeature.insert(combinedFeature.end(), firstDiffs[i].begin(),
                                 firstDiffs[i].end());
        }
        if (numDiffs >= 2) {
          combinedFeature.insert(combinedFeature.end(), secondDiffs[i].begin(),
                                 secondDiffs[i].end());
        }
        out.push_back(combinedFeature);
      }
    }
    else {
      out = mfccs;
    }
  }
  else {
    //LOG4CXX_WARN(logger,
    //             "No MFCCs extracted because input segment is too small.\n segment size:"<<
    //             PCMData.size()<<" min segment size: "<<minSegmentSize<<
    //             ". Try changing filter params");
  }
  //LOG4CXX_DEBUG(logger, "MFCCs generated: "<<out.size());
 // cout<<"MFCCs generated: "<<out.size();
  return out;
}

vector<double> MFCCs::normalizeVolume(vector<double> raw) {
  vector<double> norm(raw.size(), 0);
  double maxPCM = 0;
  for (double r: raw) {
    if (r > maxPCM)maxPCM = r;
  }
  for (int i = 0; i < raw.size(); i++) {
    norm[i] = raw[i] / maxPCM;
  }
  return norm;
}

vector<vector<double>> MFCCs::smooth(vector<vector<double>> rough) {
  vector<double> padding(rough[0].size(), 0.0);
  vector<vector<double>> smoothed;
  smoothed.push_back(padding);

  //double smoothFactor = .1;

  for (int i = 0; i < rough.size(); i++) {
    vector<double> smoothedPoint(rough[i].size(), 0);
    for (int k = 0; k < smoothedPoint.size(); k++) {
      smoothedPoint[k] +=
        smoothed[i][k] * (1.0 - smoothFactor) + rough[i][k] * smoothFactor;
    }
    smoothed.push_back(smoothedPoint);
  }
  smoothed.erase(smoothed.begin());
  return smoothed;
}

vector<vector<double>> MFCCs::getDiffs(vector<vector<double>> original) {
  vector<vector<double>> diffs(original.size(),
                               vector<double>(original[0].size(), 0));
  //insert copies of first and last time step
  vector<double> first = original[0];
  original.insert(original.begin(), first);
  vector<double> last = original[original.size() - 1];
  original.insert(original.end(), last);

  for (int i = 1; i < original.size() - 1; i++) {
    for (int j = 0; j < original[i].size(); j++) {
      diffs[i - 1][j] = (original[i + 1][j] - original[i - 1][j]) / 2.0;
    }
  }
  return diffs;
}

//cepstral lifter routine
inline vector<double> MFCCs::ceplifter(double n, double l) {
  vector<double> lift;
  for (int i = 0; i < n; i++) {
    lift.push_back(1 + 0.5 * l * sin(M_PI * i / l));
  }
  return lift;
}

inline int MFCCs::NextPowerOf2(int val) {
  val--;
  val = (val>>1) | val;
  val = (val>>2) | val;
  val = (val>>4) | val;
  val = (val>>8) | val;
  val = (val>>16) | val;
  return ++val;
}

//htk pre-emphasis function
vector<double> MFCCs::preEmphasis(double coeff, vector<double> x) {
  vector<double> y(x.size(), 0);
  y[0] = x[0];
  for (int n = 1; n < x.size(); n++) {
    y[n] = x[n] - coeff * x[n - 1];
  }
  return y;
}

//hamming window a la matlab
vector<double> MFCCs::window(int windowSize) {
  vector<double> windowed(windowSize, 0);
  for (int n = 0; n < windowSize; n++) {
    windowed[n] = 0.54 - .46 * cos((2 * M_PI) * ((double) n / windowSize - 1));
  }
  return windowed;
}

//based on mike's adaptation of  Kamil Wojcicki's "HTK MFCC MATLAB CODE"
//http://www.mathworks.com/matlabcentral/fileexchange/32849-htk-mfcc-matlab/content/mfcc/mfcc.m
vector<vector<double>> MFCCs::calculateMFCCs(vector<double> signal) {
  vector<vector<double>> out;
  //ofstream fout;
  //fout.open("mfccstuff.txt");

  //params hard coded for now
  int fs = 16000;
  int Tw = 25;
  int Ts = 10;
  double alpha = .97;
  int numFilterBankChannels = 26;
  int numCC = 13;//12 + 1;// but don't use the first
  int cepstralSineLifter = 22;

  double fMin = 0; // filter coefficients start at this frequency (Hz)
  double fLow = 20; //minFreq;//       % 20 lower cutoff frequency (Hz) for the filterbank
  double fHigh = 4400; //maxFreq;//      % 4400 upper cutoff frequency (Hz) for the filterbank
  double fMax =
    0.5 * fs; //     % filter coefficients end at this frequency (Hz)

  int numWindows = (int) round(
    (1E-3) * Tw * fs);  // % frame duration (in samples)
  int numShift = (int) round((1E-3) * Ts * fs);   //% frame shift (in samples)

  double power = log2(NextPowerOf2(numWindows));
  int nfft = pow(2, power);     // %length of FFT analysis
  int uniqueFFT = (nfft / 2) + 1;    //% length of the unique part of the FFT

  //preemphasize signal (high pass filter)

  //setting up frame buckets for fft
  //int numFrames = int((filteredSpeech.size() - numWindows) / (numShift) + 1);
  //vector<vector<double>> frames;
  //for (int i = 0; i < numFrames; i++) {
  //  vector<double> nextFrame(filteredSpeech.begin() + (i * numShift),
  //                           filteredSpeech.begin() + (i * numShift) + numWindows);
  //  frames.push_back(nextFrame);
  //}
  int numFrames = ((int) (signal.size()) - numWindows) / (numShift) + 1;
  vector<vector<double>> frames;
  for (int i = 0; i < numFrames; i++) {
    vector<double> nextFrame(signal.begin() + (i * numShift),
                             signal.begin() + (i * numShift) + numWindows);
    //frames.push_back(filter(filterVec,1,nextFrame));
    frames.push_back(preEmphasis(alpha, nextFrame));
  }

  //create matrix for application of Hamming Window
  vector<double> windowed = window(numWindows);
  arma::vec aWindowed(windowed);
  arma::mat windowMat = arma::diagmat(aWindowed);

  //convert to armadillo for fft and matrix multiplication
  arma::mat mFrames(frames[0].size(), frames.size());
  for (int i = 0; i < frames[0].size(); i++) {
    for (int j = 0; j < frames.size(); j++) {
      mFrames(i, j) = frames[j][i];
    }
  }

  //Apply hamming window to frame buckets
  arma::mat wFrames = windowMat * mFrames;

  //get energy
  double silenceFloor = 50; //50db defaults from HTK
  double energyScale = .1; //default vlaue from HTK
  vector<double> energy(wFrames.n_cols, 0);

  for (int i = 0; i < wFrames.n_cols; i++) {
    double e = 0;
    for (int j = 0; j < wFrames.n_rows; j++) {
      e += pow(wFrames(j, i), 2);
    }
    energy[i] = log(e);
  }

  //normalize energy
  double maxEnergy = 0;
  for (double e: energy) {
    if (e > maxEnergy)maxEnergy = e;
  }
  double minEnergy = maxEnergy - (silenceFloor * log(10.0)) / 10.0;
  for (int i = 0; i < energy.size(); i++) {
    if (energy[i] < minEnergy) energy[i] = minEnergy;
    energy[i] = 1.0 - (maxEnergy - energy[i]) * energyScale;
  }

  // Magnitude spectrum computation
  // could implement HTK's c++ fft here to get results closer to theirs, not sure if its worth it
  arma::mat MAG = arma::abs(arma::fft(wFrames, nfft));

  //calculate frequency range in Hz, based on sized of unique part fo fft
  vector<double> fRange(uniqueFFT, 0);
  for (int i = 0; i < uniqueFFT; i++) {
    fRange[i] = i * (fMax / (uniqueFFT - 1));
  }

  double hz2melf_low = 2595 * log10(1 + fLow / 700);
  double hz2melf_high = 2595 * log10(1 + fHigh / 700);

  vector<double> melChannels;
  for (int i = 0; i <= numFilterBankChannels + 1; i++) {
    double temp = hz2melf_low + i * ((hz2melf_high - hz2melf_low) /
                                     (numFilterBankChannels + 1));
    double x = 700 * (pow(10, (temp / 2595)) - 1);
    melChannels.push_back(x);
  }

  // create filter bank matrix to put magnitude spectrum into Mel Frequency Bands
  // super contrived, based on doing it in MATLAB but probably doesn't need to be a matrix;
  arma::mat H = arma::zeros(numFilterBankChannels, uniqueFFT);
  for (int m = 0; m < numFilterBankChannels; m++) {
    for (int k = 0; k < uniqueFFT; k++) {
      if (fRange[k] >= melChannels[m] && fRange[k] < melChannels[m + 1]) {
        H(m, k) =
          (fRange[k] - melChannels[m]) / (melChannels[m + 1] - melChannels[m]);
      }
      if (fRange[k] >= melChannels[m + 1] && fRange[k] <= melChannels[m + 2]) {
        H(m, k) = (melChannels[m + 2] - fRange[k]) /
                  (melChannels[m + 2] - melChannels[m + 1]);
      }
    }
  }

  // GET FREQUENCY BAND ENERGY
  // Filterbank application to unique part of the magnitude spectrum
  arma::mat FBE =
    H * MAG(arma::span(0, uniqueFFT - 1), arma::span(0, frames.size() - 1));

  // DCT Matrix Computation
  arma::mat DCT = arma::zeros(numCC, numFilterBankChannels);
  for (int i = 0; i < numCC; i++) {
    for (int j = 0; j < numFilterBankChannels; j++) {
      DCT(i, j) = sqrt(2.0 / numFilterBankChannels) *
                  cos((i) * (M_PI * (j + 1 - .5) / numFilterBankChannels));
    }
  }

  // Conversion of logFBEs to cepstral coefficients through DCT
  arma::mat CC = DCT * arma::log(FBE);

  // Cepstral lifter computation
  arma::vec lifter(ceplifter(numCC, cepstralSineLifter));
  arma::mat lifterMat = arma::diagmat(lifter);

  // Cepstral liftering gives liftered cepstral coefficients
  arma::mat MFCCs = lifterMat * CC;

  //convert back to std::vector
  typedef vector<double> stdvec;
  for (int i = 0; i < MFCCs.n_cols; i++) {
    //vector<double> features(MFCCs.begin_col(i) + 1, MFCCs.end_col(i));
    //features.push_back(energy[i]);

    //this order for compatability with Mike's code
    vector<double> features;
    features.push_back(energy[i]);
    features.insert(features.end(), MFCCs.begin_col(i) + 1, MFCCs.end_col(i));

    out.push_back(features);
  }

  //for (int i = 0; i < out.size(); i++) {
  //  for (double j: out[i]) {
  //    fout << setw(10) << j << " ";
  //  }
  //  fout << endl;
  //}
  //
  //fout.close();
  return out;
}


#include <numpy_eigen/boost_python_headers.hpp>

#include <aslam/backend/BSplineMotionError.hpp>
#include <aslam/splines/BSplinePoseDesignVariable.hpp>
#include <aslam/splines/EuclideanBSplineDesignVariable.hpp>
#include <aslam/splines/BSplineRSPoseDesignVariable.hpp>
#include <aslam/backend/BSplineMotionErrorFactory.hpp>
#include <boost/shared_ptr.hpp>
#include <aslam/splines/BSplineDesignVariable.hpp>

using namespace boost::python;
using namespace aslam::backend;

void exportBSplineMotionError()
{

  def("addMotionErrorTerms", &addMotionErrorTerms<aslam::splines::EuclideanBSplineDesignVariable>, "void addMotionErrorTerms( OptimizationProblemBase, SplineDv, W, errorTermOrder)");
  def("addMotionErrorTerms", &addMotionErrorTerms<aslam::splines::BSplinePoseDesignVariable>, "void addMotionErrorTerms( OptimizationProblemBase, SplineDv, W, errorTermOrder)");
  def("addMotionErrorTerms", &addMotionErrorTerms<aslam::splines::BSplineDesignVariable<1> >, "void addMotionErrorTerms( OptimizationProblemBase, SplineDv, W, errorTermOrder)");
  def("addMotionErrorTerms", &addMotionErrorTerms<aslam::splines::BSplineDesignVariable<2> >, "void addMotionErrorTerms( OptimizationProblemBase, SplineDv, W, errorTermOrder)");
  def("addMotionErrorTerms", &addMotionErrorTerms<aslam::splines::BSplineDesignVariable<3> >, "void addMotionErrorTerms( OptimizationProblemBase, SplineDv, W, errorTermOrder)");

    class_<BSplineMotionError<aslam::splines::EuclideanBSplineDesignVariable>, boost::shared_ptr<BSplineMotionError<aslam::splines::EuclideanBSplineDesignVariable> >, bases<ErrorTerm> >
        ("BSplineEuclideanMotionError", init<aslam::splines::EuclideanBSplineDesignVariable*, Eigen::MatrixXd >("BSplineEuclideanMotionError(EuclideanBSplineDesignVariable, W)"))
         .def(init<aslam::splines::EuclideanBSplineDesignVariable*, Eigen::MatrixXd, unsigned int >("BSplineGenericMotionError(EuclideanBSplineDesignVariable, W, errorTermOrder)"))
         ;
    using namespace aslam::splines;
    class_<BSplineMotionError< BSplineDesignVariable<1> >, boost::shared_ptr<BSplineMotionError< BSplineDesignVariable<1> > >, bases<ErrorTerm> >
        ("BSpline1MotionError", init<BSplineDesignVariable<1> *, Eigen::MatrixXd >("BSpline1MotionError(BSpline1DesignVariable, W)"))
         .def(init<aslam::splines::BSplineDesignVariable<1>*, Eigen::MatrixXd, unsigned int >("BSpline1MotionError(BSpline1DesignVariable, W, errorTermOrder)"))
         ;
    class_<BSplineMotionError< BSplineDesignVariable<2> >, boost::shared_ptr<BSplineMotionError< BSplineDesignVariable<2> > >, bases<ErrorTerm> >
        ("BSpline2MotionError", init<BSplineDesignVariable<2> *, Eigen::MatrixXd >("BSpline2MotionError(BSpline2DesignVariable, W)"))
         .def(init<aslam::splines::BSplineDesignVariable<2>*, Eigen::MatrixXd, unsigned int >("BSpline2MotionError(BSpline2DesignVariable, W, errorTermOrder)"))
         ;

    class_<BSplineMotionError< BSplineDesignVariable<3> >, boost::shared_ptr<BSplineMotionError< BSplineDesignVariable<3> > >, bases<ErrorTerm> >
        ("BSpline3MotionError", init<BSplineDesignVariable<3> *, Eigen::MatrixXd >("BSpline3MotionError(BSpline3DesignVariable, W)"))
         .def(init<aslam::splines::BSplineDesignVariable<3>*, Eigen::MatrixXd, unsigned int >("BSpline3MotionError(BSpline3DesignVariable, W, errorTermOrder)"))
         ;

    class_<BSplineMotionError<aslam::splines::BSplinePoseDesignVariable>, boost::shared_ptr<BSplineMotionError<aslam::splines::BSplinePoseDesignVariable> >, bases<ErrorTerm> >
        ("BSplineMotionError", init<aslam::splines::BSplinePoseDesignVariable*, Eigen::MatrixXd >("BSplineMotionError(BSplinePoseDesignVariable, W)"))
         .def(init<aslam::splines::BSplinePoseDesignVariable*, Eigen::MatrixXd, unsigned int >("BSplineMotionError(BSplinePoseDesignVariable, W, errorTermOrder)"))
         ;

    class_<BSplineMotionError<aslam::splines::BSplineRSPoseDesignVariable>, boost::shared_ptr<BSplineMotionError<aslam::splines::BSplineRSPoseDesignVariable> >, bases<ErrorTerm> >
    ("BSplineRSMotionError", init<aslam::splines::BSplineRSPoseDesignVariable*, Eigen::MatrixXd >("BSplineRSMotionError(BSplinePoseDesignVariable, W)"))
     .def(init<aslam::splines::BSplineRSPoseDesignVariable*, Eigen::MatrixXd, unsigned int >("BSplineRSMotionError(BSplinePoseDesignVariable, W, errorTermOrder)"))
    ;

}


#include <metaSMT/frontend/fmi.hpp>
#include <metaSMT/frontend/QF_BV.hpp>
#include <boost/proto/debug.hpp>
#include <boost/test/unit_test.hpp>
#include <string>

using namespace metaSMT;
using boost::proto::display_expr;

template<typename Context, typename E1, typename E2>
void require_equal_expr(Context & ctx
    , E1 const & e1
    , E2 const & e2
) {
  BOOST_REQUIRE(( boost::proto::matches<E1, fmi::grammar>::value ));
  boost::proto::display_expr(e1);
  transform::fmiToQF_BV toBV;
  metaSMT::assumption(ctx, metaSMT::logic::equal ( toBV(e1), toBV(e2) ));
  BOOST_REQUIRE(solve(ctx));
  metaSMT::assumption(ctx, metaSMT::logic::nequal( toBV(e1), toBV(e2) ));
  BOOST_REQUIRE(!solve(ctx));
}

BOOST_FIXTURE_TEST_SUITE(fmi_t, Solver_Fixture )

BOOST_AUTO_TEST_CASE( syntax )
{
  fmi::Solver solver;
  unsigned lines=0, pos=0;
  fmi::bv control = fmi::new_variable(solver, 3);
  std::vector<fmi::bv> nextGate, currentGate;
  nextGate.push_back( fmi::new_variable(solver, 1) );
  currentGate.push_back( fmi::new_variable(solver, 1) );
  fmi::bv target;
  using fmi::_0;
  using fmi::_1;
  using fmi::_2;
  using fmi::_3;

  //nextGate [ pos ] = fmi::new_variable (solver, lines);
  fmi::bv hit      = fmi::new_variable (solver, 1);

  //boost::proto::display_expr(fmi::generate(_0 == _0));

  using namespace logic::QF_BV;

  fmi::generate (solver
    , _0 %= (_1 & _2 & _3) == _2
    , hit
    , currentGate [ pos ]
    , control // to many arguments
  );

  fmi::generate (solver
      , _0 %= (_1 & _2) == _2
      , hit, currentGate [ pos ] , control );

  fmi::bv zext = fmi::zero_extend (solver, hit, lines-1);

  display_expr( transform::fmiToQF_BV()(
      _0 %= _1 ^ (_2 << _3)
      ));

  fmi::generate (solver,
      _0 %= _1 ^ (_2 & _3),
      nextGate [ pos ], currentGate [ pos  ], zext, target);

  fmi::generate ( solver, hit == ( (currentGate[pos] & control) == control) );
}

BOOST_AUTO_TEST_CASE( simple_sat )
{
  fmi::bv control = fmi::new_variable(ctx, 3);

  fmi::assertion ( ctx, control == control  );

  BOOST_REQUIRE( solve( ctx ) );

  fmi::assertion ( ctx, control != control  );

  BOOST_REQUIRE( !solve( ctx ) );
}

BOOST_AUTO_TEST_CASE( bitwise_expr )
{
  fmi::bv x = fmi::new_variable(ctx, 4);
  fmi::bv y = fmi::new_variable(ctx, 4);

  require_equal_expr(ctx, x & y , bvand(x,y) );
  require_equal_expr(ctx, x | y , bvor(x,y) );
  require_equal_expr(ctx, x ^ y , bvxor(x,y) );
  require_equal_expr(ctx,   ~x  , bvnot(x) );
  require_equal_expr(ctx, x << y, bvshl(x, y) );
  require_equal_expr(ctx, x >> y, bvshr(x, y) );
}

BOOST_AUTO_TEST_CASE( arithmetic_expr )
{
  fmi::bv x = fmi::new_variable(ctx, 4);
  fmi::bv y = fmi::new_variable(ctx, 4);

  require_equal_expr(ctx, x + y , bvadd(x,y) );
  require_equal_expr(ctx, x * y , bvmul(x,y) );
  require_equal_expr(ctx, x - y , bvsub(x,y) );
  require_equal_expr(ctx,   -x  , bvneg(x) );
  require_equal_expr(ctx, x / y , bvudiv(x, y) );
  require_equal_expr(ctx, x % y , bvurem(x, y) );
}

BOOST_AUTO_TEST_SUITE_END() //QF_BV

//  vim: ft=cpp:ts=2:sw=2:expandtab


/*!
 * @file
 * Defines unit tests for `logical or`s.
 *
 *
 * @copyright Louis Dionne 2014
 * Distributed under the Boost Software License, Version 1.0.
 *         (See accompanying file LICENSE.md or copy at
 *             http://www.boost.org/LICENSE_1_0.txt)
 */

//////////////////////////////////////////////////////////////////////////
// GENERATED HEADER: DO NOT EDIT.
//////////////////////////////////////////////////////////////////////////

#ifndef BOOST_MPL11_DETAIL_TEST_LOGICAL_OR_HPP
#define BOOST_MPL11_DETAIL_TEST_LOGICAL_OR_HPP

#include <boost/mpl11/bool.hpp>
#include <boost/mpl11/core.hpp>
#include <boost/mpl11/detail/test/expect.hpp>

namespace boost { namespace mpl11 { namespace detail {

    template <template <typename ...> class or_>
    struct test_logical_or_strict
        : instantiate<
            typename expect<or_<>>::template to_eq<false_>

                    ,typename expect<
                        or_<true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_>
                    >::template to_eq<
                        bool_<false>
                    >

                    ,typename expect<
                        or_<true_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_>
                    >::template to_eq<
                        bool_<false>
                    >

                    ,typename expect<
                        or_<true_,true_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,true_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,false_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,false_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,true_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,true_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_>
                    >::template to_eq<
                        bool_<false>
                    >

                    ,typename expect<
                        or_<true_,true_,true_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,true_,true_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,true_,false_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,true_,false_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,false_,true_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,false_,true_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,false_,false_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,false_,false_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,true_,true_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,true_,true_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,true_,false_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,true_,false_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,true_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,true_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,false_>
                    >::template to_eq<
                        bool_<false>
                    >

                    ,typename expect<
                        or_<true_,true_,true_,true_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,true_,true_,true_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,true_,true_,false_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,true_,true_,false_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,true_,false_,true_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,true_,false_,true_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,true_,false_,false_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,true_,false_,false_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,false_,true_,true_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,false_,true_,true_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,false_,true_,false_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,false_,true_,false_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,false_,false_,true_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,false_,false_,true_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,false_,false_,false_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,false_,false_,false_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,true_,true_,true_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,true_,true_,true_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,true_,true_,false_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,true_,true_,false_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,true_,false_,true_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,true_,false_,true_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,true_,false_,false_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,true_,false_,false_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,true_,true_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,true_,true_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,true_,false_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,true_,false_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,true_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,true_,false_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,false_,true_>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,false_,false_>
                    >::template to_eq<
                        bool_<false>
                    >

        >
    { };

    template <template <typename ...> class or_>
    struct test_logical_or_lazy
        : instantiate<
            test_logical_or_strict<or_>

                    ,typename expect<
                        or_<true_,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,true_,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,true_,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,true_,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,undefined,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,true_,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,true_,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,true_,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,undefined,undefined,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,true_,undefined,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,true_,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,true_,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,false_,true_,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,undefined,undefined,undefined,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,true_,undefined,undefined,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,true_,undefined,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,true_,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,false_,true_,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,false_,false_,true_,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,undefined,undefined,undefined,undefined,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,true_,undefined,undefined,undefined,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,true_,undefined,undefined,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,true_,undefined,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,false_,true_,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,false_,false_,true_,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,false_,false_,false_,true_,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,undefined,undefined,undefined,undefined,undefined,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,true_,undefined,undefined,undefined,undefined,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,true_,undefined,undefined,undefined,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,true_,undefined,undefined,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,false_,true_,undefined,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,false_,false_,true_,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,false_,false_,false_,true_,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,false_,false_,false_,false_,true_,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<true_,undefined,undefined,undefined,undefined,undefined,undefined,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,true_,undefined,undefined,undefined,undefined,undefined,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,true_,undefined,undefined,undefined,undefined,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,true_,undefined,undefined,undefined,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,false_,true_,undefined,undefined,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,false_,false_,true_,undefined,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,false_,false_,false_,true_,undefined,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,false_,false_,false_,false_,true_,undefined,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

                    ,typename expect<
                        or_<false_,false_,false_,false_,false_,false_,false_,false_,true_,undefined>
                    >::template to_eq<
                        bool_<true>
                    >

        >
    { };
}}} // end namespace boost::mpl11::detail

#endif // !BOOST_MPL11_DETAIL_TEST_LOGICAL_OR_HPP


//
//  Copyright (c) 2002-2003
//  Toon Knapen, Kresimir Fresl, Joerg Walter
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_NUMERIC_BINDINGS_TRAITS_MATRIX_RAW_HPP
#define BOOST_NUMERIC_BINDINGS_TRAITS_MATRIX_RAW_HPP

#include <cstddef>
#include <boost/numeric/ublas/config.hpp>
#ifndef BOOST_UBLAS_HAVE_BINDINGS
#  include <boost/numeric/ublas/matrix.hpp>
#endif

namespace boost { namespace numeric { namespace bindings { namespace traits {

  namespace ublas = boost::numeric::ublas;

  // size:

  template <typename M>
  BOOST_UBLAS_INLINE
  int matrix_size1 (const M &m) {
    return (int) m.size1();
  }
  template <typename M>
  BOOST_UBLAS_INLINE
  int matrix_size2 (const M &m) {
    return (int) m.size2();
  }

#if 0
  // MSVC seems to dislike overloads if there is 'generic' template
  template <typename M>
  BOOST_UBLAS_INLINE
  int matrix_size1 (const ublas::matrix_reference<M> &m) {
    return matrix_size1 (m.expression());
  }
  template <typename M>
  BOOST_UBLAS_INLINE
  int matrix_size2 (const ublas::matrix_reference<M> &m) {
    return matrix_size2 (m.expression());
  }
#endif // 0

  template <typename T, typename F, typename A>
  BOOST_UBLAS_INLINE
  int matrix_storage_size (const ublas::matrix<T,F,A>& m) {
    return (int) (m.size1() * m.size2());
  }
  template <typename T, std::size_t M, std::size_t N>
  BOOST_UBLAS_INLINE
  int matrix_storage_size (const ublas::c_matrix<T, M, N> &m) {
    return (int) (M * N);
  }

  template <typename T, typename F, typename A>
  BOOST_UBLAS_INLINE
  int leading_dimension (const ublas::matrix<T,F,A> &m, ublas::row_major_tag) {
    return (int) m.size2();
  }
  template <typename T, typename F, typename A>
  BOOST_UBLAS_INLINE
  int leading_dimension (const ublas::matrix<T,F,A> &m, ublas::column_major_tag) {
    return (int) m.size1();
  }
  template <typename T, typename F, typename A>
  BOOST_UBLAS_INLINE
  int leading_dimension (const ublas::matrix<T,F,A> &m) {
    typedef ublas::matrix<T,F,A> matrix_t;
    return bindings::traits::leading_dimension
      (m, BOOST_UBLAS_TYPENAME matrix_t::orientation_category());
  }
  template <typename M>
  BOOST_UBLAS_INLINE
  int leading_dimension (const ublas::matrix_reference<M> &m) {
    return bindings::traits::leading_dimension (m.expression());
  }
  template <typename M>
  BOOST_UBLAS_INLINE
  int leading_dimension (const ublas::matrix_range<M> &m) {
    return bindings::traits::leading_dimension (m.data());
  }
  template <typename T, std::size_t M, std::size_t N>
  BOOST_UBLAS_INLINE
  int leading_dimension (const ublas::c_matrix<T, M, N> &m) {
    return (int) N;
  }

  // stride:

#if 0
  // MSVC seems to dislike overloads if there is 'generic' template
  template <typename M>
  BOOST_UBLAS_INLINE
  int matrix_stride1 (const M &m) {
    typedef typename M::functor_type functor_type;
    return (int) functor_type::one1 (m.size1(), m.size2());
  }
  template <typename M>
  BOOST_UBLAS_INLINE
  int matrix_stride2 (const M &m) {
    typedef typename M::functor_type functor_type;
    return (int) functor_type::one2 (m.size1(), m.size2());
  }
#endif // 0

  template <typename M, typename F, typename A>
  BOOST_UBLAS_INLINE
  int matrix_stride1 (const ublas::matrix<M,F,A> &m) {
    return (int) F::one1 (m.size1(), m.size2());
  }
  template <typename M, typename F, typename A>
  BOOST_UBLAS_INLINE
  int matrix_stride2 (const ublas::matrix<M,F,A> &m) {
    return (int) F::one2 (m.size1(), m.size2());
  }
  template <typename M>
  BOOST_UBLAS_INLINE
  int matrix_stride1 (const ublas::matrix_reference<M> &m) {
    return matrix_stride1 (m.expression());
  }
  template <typename M>
  BOOST_UBLAS_INLINE
  int matrix_stride2 (const ublas::matrix_reference<M> &m) {
    return matrix_stride2 (m.expression());
  }
  template <typename T, std::size_t M, std::size_t N>
  BOOST_UBLAS_INLINE
  int matrix_stride1 (const ublas::c_matrix<T, M, N> &m) { return (int) N; }
  template <typename T, std::size_t M, std::size_t N>
  BOOST_UBLAS_INLINE
  int matrix_stride2 (const ublas::c_matrix<T, M, N> &m) { return 1; }
  template <typename M>
  BOOST_UBLAS_INLINE
  int matrix_stride1 (const ublas::matrix_range<M> &m) {
    return matrix_stride1 (m.data());
  }
  template <typename M>
  BOOST_UBLAS_INLINE
  int matrix_stride2 (const ublas::matrix_range<M> &m) {
    return matrix_stride2 (m.data());
  }
  template <typename M>
  BOOST_UBLAS_INLINE
  int matrix_stride1 (const ublas::matrix_slice<M> &m) {
    return (int) (m.stride1() * matrix_stride1 (m.data()));
  }
  template <typename M>
  BOOST_UBLAS_INLINE
  int matrix_stride2 (const ublas::matrix_slice<M> &m) {
    return (int) (m.stride2() * matrix_stride2 (m.data()));
  }
  template <typename M>
  BOOST_UBLAS_INLINE
  int vector_stride (const ublas::matrix_row<M> &v) {
    return matrix_stride2 (v.data());
  }
  template <typename M>
  BOOST_UBLAS_INLINE
  int vector_stride (const ublas::matrix_column<M> &v) {
    return matrix_stride1 (v.data());
  }

  // storage:

#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
  template <typename T, typename F, typename A>
  BOOST_UBLAS_INLINE
  typename ublas::matrix<T,F,A>::const_pointer
  matrix_storage (const ublas::matrix<T,F,A> &m) {
    return &m.data().begin()[0];
  }
#endif
  // We need storage_const() mostly due to MSVC 6.0.
  // But how shall we write portable code otherwise?
  template <typename T, typename F, typename A>
  BOOST_UBLAS_INLINE
  typename ublas::matrix<T,F,A>::const_pointer
  matrix_storage_const (const ublas::matrix<T,F,A> &m) {
    return &m.data().begin()[0];
  }
  template <typename T, typename F, typename A>
  BOOST_UBLAS_INLINE
  typename ublas::matrix<T,F,A>::pointer
  matrix_storage (ublas::matrix<T,F,A> &m) {
    return &m.data().begin()[0];
  }

#if 0
  // MSVC seems to dislike overloads if there is 'generic' template
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
  template <typename M>
  BOOST_UBLAS_INLINE
  typename M::const_pointer matrix_storage (const M &m) {
    return &m.data().begin()[0];
  }
#endif
  // We need storage_const() mostly due to MSVC 6.0.
  // But how shall we write portable code otherwise?
  template <typename M>
  BOOST_UBLAS_INLINE
  typename M::const_pointer matrix_storage_const (const M &m) {
    return &m.data().begin()[0];
  }
  template <typename M>
  BOOST_UBLAS_INLINE
  typename M::pointer matrix_storage (M &m) {
    return &m.data().begin()[0];
  }
#endif // 0

#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
  template <typename M>
  BOOST_UBLAS_INLINE
  typename M::const_pointer
  matrix_storage (const ublas::matrix_reference<M> &m) {
    return matrix_storage (m.expression ());
  }
#endif
  // We need storage_const() mostly due to MSVC 6.0.
  // But how shall we write portable code otherwise?
  template <typename M>
  BOOST_UBLAS_INLINE
  typename M::const_pointer
  matrix_storage_const (const ublas::matrix_reference<M> &m) {
    return matrix_storage_const (m.expression ());
  }
  template <typename M>
  BOOST_UBLAS_INLINE
  typename M::pointer matrix_storage (ublas::matrix_reference<M> &m) {
    return matrix_storage (m.expression ());
  }

#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
  template <typename T, std::size_t M, std::size_t N>
  BOOST_UBLAS_INLINE
  typename ublas::c_matrix<T, M, N>::const_pointer
  matrix_storage (const ublas::c_matrix<T, M, N> &m) {
    return m.data();
  }
#endif
#ifndef BOOST_MSVC
  // We need storage_const() mostly due to MSVC 6.0.
  // But how shall we write portable code otherwise?
  template <typename T, std::size_t M, std::size_t N>
  BOOST_UBLAS_INLINE
  typename ublas::c_matrix<T, M, N>::const_pointer
  matrix_storage_const (const ublas::c_matrix<T, M, N> &m) {
    return m.data();
  }
  template <typename T, std::size_t M, std::size_t N>
  BOOST_UBLAS_INLINE
  typename ublas::c_matrix<T, M, N>::pointer
  matrix_storage (ublas::c_matrix<T, M, N> &m) {
    return m.data();
  }
#endif

#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
  template <typename M>
  BOOST_UBLAS_INLINE
  typename M::const_pointer
  matrix_storage (const ublas::matrix_range<M> &m) {
    return matrix_storage (m.data())
      + m.start1() * matrix_stride1 (m.data())
      + m.start2() * matrix_stride2 (m.data());
  }
  template <typename M>
  BOOST_UBLAS_INLINE
  typename M::const_pointer
  matrix_storage (const ublas::matrix_slice<M> &m) {
    return matrix_storage (m.data())
      + m.start1() * matrix_stride1 (m.data ())
      + m.start2() * matrix_stride2 (m.data ());
  }
#endif
  // We need storage_const() mostly due to MSVC 6.0.
  // But how shall we write portable code otherwise?
  template <typename M>
  BOOST_UBLAS_INLINE
  typename M::const_pointer
  matrix_storage_const (const ublas::matrix_range<M> &m) {
    return matrix_storage_const (m.data())
      + m.start1() * matrix_stride1 (m.data ())
      + m.start2() * matrix_stride2 (m.data ());
  }
  template <typename M>
  BOOST_UBLAS_INLINE
  typename M::const_pointer
  matrix_storage_const (const ublas::matrix_slice<M> &m) {
    return matrix_storage_const (m.data())
      + m.start1() * matrix_stride1 (m.data ())
      + m.start2() * matrix_stride2 (m.data ());
  }
  template <typename M>
  BOOST_UBLAS_INLINE
  typename M::pointer matrix_storage (ublas::matrix_range<M> &m) {
    return matrix_storage (m.data())
      + m.start1() * matrix_stride1 (m.data ())
      + m.start2() * matrix_stride2 (m.data ());
  }
  template <typename M>
  BOOST_UBLAS_INLINE
  typename M::pointer matrix_storage (ublas::matrix_slice<M> &m) {
    return matrix_storage (m.data())
      + m.start1() * matrix_stride1 (m.data ())
      + m.start2() * matrix_stride2 (m.data ());
  }

#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
  template <typename M>
  BOOST_UBLAS_INLINE
  typename M::const_pointer
  vector_storage (const ublas::matrix_row<M> &v) {
    return matrix_storage (v.data())
      + v.index() * matrix_stride1 (v.data());
  }
  template <typename M>
  BOOST_UBLAS_INLINE
  typename M::const_pointer
  vector_storage (const ublas::matrix_column<M> &v) {
    return matrix_storage (v.data())
      + v.index() * matrix_stride2 (v.data());
  }
#endif
  // We need storage_const() mostly due to MSVC 6.0.
  // But how shall we write portable code otherwise?
  template <typename M>
  BOOST_UBLAS_INLINE
  typename M::const_pointer
  vector_storage_const (const ublas::matrix_row<M> &v) {
    return matrix_storage_const (v.data())
      + v.index() * matrix_stride1 (v.data());
  }
  template <typename M>
  BOOST_UBLAS_INLINE
  typename M::const_pointer
  vector_storage_const (const ublas::matrix_column<M> &v) {
    return matrix_storage_const (v.data())
      + v.index() * matrix_stride2 (v.data());
  }
  template <typename M>
  BOOST_UBLAS_INLINE
  typename M::pointer vector_storage (ublas::matrix_row<M> &v) {
    return matrix_storage (v.data())
      + v.index() * matrix_stride1 (v.data());
  }
  template <typename M>
  BOOST_UBLAS_INLINE
  typename M::pointer vector_storage (ublas::matrix_column<M> &v) {
    return matrix_storage (v.data())
      + v.index() * matrix_stride2 (v.data());
  }

}}}}

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_NO_SYMMETRIC_TRAITS

#include <boost/numeric/bindings/traits/symm_herm_raw.hpp>

#endif // BOOST_NUMERIC_BINDINGS_POOR_MANS_NO_SYMMETRIC_TRAITS

#endif // BOOST_NUMERIC_BINDINGS_TRAITS_MATRIX_RAW_HPP


// #include <pcl/point_types.h>
// #include <pcl/visualization/pcl_visualizer.h>

#include <string>
#include <tuple>
#include <Eigen/Core>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>

auto read_point_cloud(const std::string& filename, const std::string& filename2){
    const pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>);
    const int retval = pcl::io::loadPLYFile(filename, *pointcloud);
    if (retval == -1 || pointcloud->size() <= 0)
    {
        PCL_ERROR("File load error.");
        exit(-1);
	}
	else {
		printf("load point cloud\n");
	}

	const pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
	const int r = pcl::io::loadPLYFile(filename2, *mesh); // http://docs.pointclouds.org/trunk/group__io.html#ga0cfc645cc531647728e16088b6342204
	if (r == -1 || mesh->polygons.size() <= 0) {
		PCL_ERROR("File load error mesh.");
		exit(-1);
	}
	else {
		printf("load mesh\nfaces: %d, vertecies: %d\n", mesh->polygons.size(), mesh->cloud.data.size());

	}

	// メッシュ生成 http://pointclouds.org/documentation/tutorials/greedy_projection.php
	// Load input file into a PointCloud<T> with an appropriate type
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	//sensor_msgs::PointCloud2 cloud_blob;
	//pcl::io::loadPCDFile("bun0.pcd", cloud_blob);
	//pcl::fromROSMsg(cloud_blob, *cloud);
	//* the data should be available in cloud
	pcl::io::loadPCDFile("bun0.pcd", *cloud);

	// Normal estimation*
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
	tree->setInputCloud(cloud);
	n.setInputCloud(cloud);
	n.setSearchMethod(tree);
	n.setKSearch(20);
	n.compute(*normals);
	//* normals should not contain the point normals + surface curvatures

	// Concatenate the XYZ and normal fields*
	pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
	//pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);
	pcl::concatenateFields<pcl::PointXYZ, pcl::Normal, pcl::PointNormal>(*cloud, *normals, *cloud_with_normals);
	//* cloud_with_normals = cloud + normals

	// Create search tree*
	pcl::search::KdTree<pcl::PointNormal>::Ptr tree2(new pcl::search::KdTree<pcl::PointNormal>);
	tree2->setInputCloud(cloud_with_normals);

	// Initialize objects
	pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
	pcl::PolygonMesh triangles;

	// Set the maximum distance between connected points (maximum edge length)
	gp3.setSearchRadius(0.025);

	// Set typical values for the parameters
	gp3.setMu(2.5);
	gp3.setMaximumNearestNeighbors(100);
	gp3.setMaximumSurfaceAngle(M_PI / 4); // 45 degrees
	gp3.setMinimumAngle(M_PI / 18); // 10 degrees
	gp3.setMaximumAngle(2 * M_PI / 3); // 120 degrees
	gp3.setNormalConsistency(false);

	// Get result
	gp3.setInputCloud(cloud_with_normals);
	gp3.setSearchMethod(tree2);
	gp3.reconstruct(triangles);

	// Additional vertex information
	std::vector<int> parts = gp3.getPartIDs();
	std::vector<int> states = gp3.getPointStates();

	for (auto v : parts) {
		printf("parts: %d\n", v);
	}
	pcl::io::savePLYFile("output_0.ply", triangles);
	printf("done\n");

    return pointcloud;
}

int main (int argc, char *argv[]){
    const auto scene1 = read_point_cloud("usagi.ply", "usagi_m.ply");
    // メッシュデータがあるとエラー。
    // meshlabのデフォルトではメッシュが無くてもproperty list uchar int vertex_indicesの項目が出るが、これも削除しないと以下のエラー出る
    // [pcl::PLYReader] usagi.ply:9: property 'list uint8 int32 vertex_indices' of element 'face' is not handled
    // ファイルパスが間違っていても以下のエラーが出るので注意 https://github.com/PointCloudLibrary/pcl/issues/2763
    // [pcl::PLYReader] usagi.ply:1: parse error: couldn't read the magic string

    return 1;
}