#include "outer_hull.h"
#include "order_facets_around_edges.h"
#include "../outer_facet.h"
#include "../sortrows.h"
#include "../facet_components.h"
#include "../winding_number.h"
#include "../triangle_triangle_adjacency.h"
#include "../unique_edge_map.h"
#include "../barycenter.h"
#include "../per_face_normals.h"
#include "../writePLY.h"
#include "../sort_angles.h"

#include <Eigen/Geometry>
#include <vector>
#include <map>
#include <queue>
#include <iostream>
#include <CGAL/number_utils.h>
//#define IGL_OUTER_HULL_DEBUG

template <
  typename DerivedV,
  typename DerivedF,
  typename DerivedN,
  typename DerivedG,
  typename DerivedJ,
  typename Derivedflip>
IGL_INLINE void igl::outer_hull(
  const Eigen::PlainObjectBase<DerivedV> & V,
  const Eigen::PlainObjectBase<DerivedF> & F,
  const Eigen::PlainObjectBase<DerivedN> & N,
  Eigen::PlainObjectBase<DerivedG> & G,
  Eigen::PlainObjectBase<DerivedJ> & J,
  Eigen::PlainObjectBase<Derivedflip> & flip)
{
#ifdef IGL_OUTER_HULL_DEBUG
  std::cerr << "Extracting outer hull" << std::endl;
#endif
  using namespace Eigen;
  using namespace std;
  typedef typename DerivedF::Index Index;
  Matrix<Index,DerivedF::RowsAtCompileTime,1> C;
  typedef Matrix<typename DerivedV::Scalar,Dynamic,DerivedV::ColsAtCompileTime> MatrixXV;
  typedef Matrix<typename DerivedF::Scalar,Dynamic,DerivedF::ColsAtCompileTime> MatrixXF;
  typedef Matrix<typename DerivedG::Scalar,Dynamic,DerivedG::ColsAtCompileTime> MatrixXG;
  typedef Matrix<typename DerivedJ::Scalar,Dynamic,DerivedJ::ColsAtCompileTime> MatrixXJ;
  typedef Matrix<typename DerivedN::Scalar,1,3> RowVector3N;
  const Index m = F.rows();

  // UNUSED:
  //const auto & duplicate_simplex = [&F](const int f, const int g)->bool
  //{
  //  return
  //    (F(f,0) == F(g,0) && F(f,1) == F(g,1) && F(f,2) == F(g,2)) ||
  //    (F(f,1) == F(g,0) && F(f,2) == F(g,1) && F(f,0) == F(g,2)) ||
  //    (F(f,2) == F(g,0) && F(f,0) == F(g,1) && F(f,1) == F(g,2)) ||
  //    (F(f,0) == F(g,2) && F(f,1) == F(g,1) && F(f,2) == F(g,0)) ||
  //    (F(f,1) == F(g,2) && F(f,2) == F(g,1) && F(f,0) == F(g,0)) ||
  //    (F(f,2) == F(g,2) && F(f,0) == F(g,1) && F(f,1) == F(g,0));
  //};

#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"outer hull..."<<endl;
#endif

#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"edge map..."<<endl;
#endif
  typedef Matrix<typename DerivedF::Scalar,Dynamic,2> MatrixX2I;
  typedef Matrix<typename DerivedF::Index,Dynamic,1> VectorXI;
  typedef Matrix<typename DerivedV::Scalar, 3, 1> Vector3F;
  MatrixX2I E,uE;
  VectorXI EMAP;
  vector<vector<typename DerivedF::Index> > uE2E;
  unique_edge_map(F,E,uE,EMAP,uE2E);
#ifdef IGL_OUTER_HULL_DEBUG
  for (size_t ui=0; ui<uE.rows(); ui++) {
      std::cout << ui << ": " << uE2E[ui].size() << " -- (";
      for (size_t i=0; i<uE2E[ui].size(); i++) {
          std::cout << uE2E[ui][i] << ", ";
      }
      std::cout << ")" << std::endl;
  }
#endif

  std::vector<std::vector<typename DerivedF::Index> > uE2oE;
  std::vector<std::vector<bool> > uE2C;
  order_facets_around_edges(V, F, N, E, uE, EMAP, uE2E, uE2oE, uE2C);
  uE2E = uE2oE;
  VectorXI diIM(3*m);
  for (auto ue : uE2E) {
      for (size_t i=0; i<ue.size(); i++) {
          auto fe = ue[i];
          diIM[fe] = i;
      }
  }

  vector<vector<vector<Index > > > TT,_1;
  triangle_triangle_adjacency(E,EMAP,uE2E,false,TT,_1);
  VectorXI counts;
#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"facet components..."<<endl;
#endif
  facet_components(TT,C,counts);
  assert(C.maxCoeff()+1 == counts.rows());
  const size_t ncc = counts.rows();
  G.resize(0,F.cols());
  J.resize(0,1);
  flip.setConstant(m,1,false);

#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"reindex..."<<endl;
#endif
  // H contains list of faces on outer hull;
  vector<bool> FH(m,false);
  vector<bool> EH(3*m,false);
  vector<MatrixXG> vG(ncc);
  vector<MatrixXJ> vJ(ncc);
  vector<MatrixXJ> vIM(ncc);
  //size_t face_count = 0;
  for(size_t id = 0;id<ncc;id++)
  {
    vIM[id].resize(counts[id],1);
  }
  // current index into each IM
  vector<size_t> g(ncc,0);
  // place order of each face in its respective component
  for(Index f = 0;f<m;f++)
  {
    vIM[C(f)](g[C(f)]++) = f;
  }

#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"barycenters..."<<endl;
#endif
  // assumes that "resolve" has handled any coplanar cases correctly and nearly
  // coplanar cases can be sorted based on barycenter.
  MatrixXV BC;
  barycenter(V,F,BC);

#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"loop over CCs (="<<ncc<<")..."<<endl;
#endif
  for(Index id = 0;id<(Index)ncc;id++)
  {
    auto & IM = vIM[id];
    // starting face that's guaranteed to be on the outer hull and in this
    // component
    int f;
    bool f_flip;
#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"outer facet..."<<endl;
#endif
    outer_facet(V,F,N,IM,f,f_flip);
#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"outer facet: "<<f<<endl;
  cout << V.row(F(f, 0)) << std::endl;
  cout << V.row(F(f, 1)) << std::endl;
  cout << V.row(F(f, 2)) << std::endl;
#endif
    int FHcount = 1;
    FH[f] = true;
    // Q contains list of face edges to continue traversing upong
    queue<int> Q;
    Q.push(f+0*m);
    Q.push(f+1*m);
    Q.push(f+2*m);
    flip(f) = f_flip;
    //std::cout << "face " << face_count++ << ": " << f << std::endl;
    //std::cout << "f " << F.row(f).array()+1 << std::endl;
    //cout<<"flip("<<f<<") = "<<(flip(f)?"true":"false")<<endl;
#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"BFS..."<<endl;
#endif
    while(!Q.empty())
    {
      // face-edge
      const int e = Q.front();
      Q.pop();
      // face
      const int f = e%m;
      // corner
      const int c = e/m;
#ifdef IGL_OUTER_HULL_DEBUG
      std::cout << "edge: " << e << ", ue: " << EMAP(e) << std::endl;
      std::cout << "face: " << f << std::endl;
      std::cout << "corner: " << c << std::endl;
      std::cout << "consistent: " << uE2C[EMAP(e)][diIM[e]] << std::endl;
#endif
      // Should never see edge again...
      if(EH[e] == true)
      {
        continue;
      }
      EH[e] = true;
      // source of edge according to f
      const int fs = flip(f)?F(f,(c+2)%3):F(f,(c+1)%3);
      // destination of edge according to f
      const int fd = flip(f)?F(f,(c+1)%3):F(f,(c+2)%3);
      // edge valence
      const size_t val = uE2E[EMAP(e)].size();
#ifdef IGL_OUTER_HULL_DEBUG
      std::cout << "vd: " << V.row(fd) << std::endl;
      std::cout << "vs: " << V.row(fs) << std::endl;
      std::cout << "edge: " << V.row(fd) - V.row(fs) << std::endl;
      for (size_t i=0; i<val; i++) {
          if (i == diIM(e)) {
              std::cout << "* ";
          } else {
              std::cout << "  ";
          }
          std::cout << i << ": "
              << " (e: " << uE2E[EMAP(e)][i] << ", f: "
              << uE2E[EMAP(e)][i] % m * (uE2C[EMAP(e)][i] ? 1:-1) << ")" << std::endl;
      }
#endif
      //// find overlapping face-edges
      //const auto & neighbors = uE2E[EMAP(e)];
      //// normal after possible flipping
      //const auto & fN = (flip(f)?-1.:1.)*N.row(f);
      //// Edge vector according to f's (flipped) orientation.
      ////const auto & eV = (V.row(fd)-V.row(fs)).normalized();

//#warning "EXPERIMENTAL, DO NOT USE"
      //// THIS IS WRONG! The first face is---after sorting---no longer the face
      //// used for orienting the sort.
      //const auto ui = EMAP(e);
      //const auto fe0 = uE2E[ui][0];
      //const auto es = F(fe0%m,((fe0/m)+1)%3);

      // is edge consistent with edge of face used for sorting
      const int e_cons = (uE2C[EMAP(e)][diIM(e)] ? 1: -1);
      int nfei = -1;
      // Loop once around trying to find suitable next face
      for(size_t step = 1; step<val+2;step++)
      {
        const int nfei_new = (diIM(e) + 2*val + e_cons*step*(flip(f)?-1:1))%val;
        const int nf = uE2E[EMAP(e)][nfei_new] % m;
        // Don't consider faces with identical dihedral angles
        //if ((di[EMAP(e)][diIM(e)].array() != di[EMAP(e)][nfei_new].array()).any())
        //if((di[EMAP(e)][diIM(e)] != di[EMAP(e)][nfei_new]))
//#warning "THIS IS HACK, FIX ME"
//        if( abs(di[EMAP(e)][diIM(e)] - di[EMAP(e)][nfei_new]) < 1e-15 )
        {
#ifdef IGL_OUTER_HULL_DEBUG
        //cout<<"Next facet: "<<(f+1)<<" --> "<<(nf+1)<<", |"<<
        //  di[EMAP(e)][diIM(e)]<<" - "<<di[EMAP(e)][nfei_new]<<"| = "<<
        //    abs(di[EMAP(e)][diIM(e)] - di[EMAP(e)][nfei_new])
        //    <<endl;
#endif

          // Only use this face if not already seen
          if(!FH[nf])
          {
            nfei = nfei_new;
          //} else {
          //    std::cout << "skipping face " << nfei_new << " because it is seen before"
          //        << std::endl;
          }
          break;
        //} else {
        //    std::cout << di[EMAP(e)][diIM(e)].transpose() << std::endl;
        //    std::cout << di[EMAP(e)][diIM(nfei_new)].transpose() << std::endl;
        //    std::cout << "skipping face " << nfei_new << " with identical dihedral angle"
        //        << std::endl;
        }
//#ifdef IGL_OUTER_HULL_DEBUG
//        cout<<"Skipping co-planar facet: "<<(f+1)<<" --> "<<(nf+1)<<endl;
//#endif
      }

      int max_ne = -1;
      //// Loop over and find max dihedral angle
      //typename DerivedV::Scalar max_di = -1;
      //for(const auto & ne : neighbors)
      //{
      //  const int nf = ne%m;
      //  if(nf == f)
      //  {
      //    continue;
      //  }
      //  // Corner of neighbor
      //  const int nc = ne/m;
      //  // Is neighbor oriented consistently with (flipped) f?
      //  //const int ns = F(nf,(nc+1)%3);
      //  const int nd = F(nf,(nc+2)%3);
      //  const bool cons = (flip(f)?fd:fs) == nd;
      //  // Normal after possibly flipping to match flip or orientation of f
      //  const auto & nN = (cons? (flip(f)?-1:1.) : (flip(f)?1.:-1.) )*N.row(nf);
      //  // Angle between n and f
      //  const auto & ndi = M_PI - atan2( fN.cross(nN).dot(eV), fN.dot(nN));
      //  if(ndi>=max_di)
      //  {
      //    max_ne = ne;
      //    max_di = ndi;
      //  }
      //}

      ////cout<<(max_ne != max_ne_2)<<" =?= "<<e_cons<<endl;
      //if(max_ne != max_ne_2)
      //{
      //  cout<<(f+1)<<" ---> "<<(max_ne%m)+1<<" != "<<(max_ne_2%m)+1<<" ... "<<e_cons<<" "<<flip(f)<<endl;
      //  typename DerivedV::Scalar max_di = -1;
      //  for(size_t nei = 0;nei<neighbors.size();nei++)
      //  {
      //    const auto & ne = neighbors[nei];
      //    const int nf = ne%m;
      //    if(nf == f)
      //    {
      //      cout<<"  "<<(ne%m)+1<<":\t"<<0<<"\t"<<di[EMAP[e]][nei]<<" "<<diIM(ne)<<endl;
      //      continue;
      //    }
      //    // Corner of neighbor
      //    const int nc = ne/m;
      //    // Is neighbor oriented consistently with (flipped) f?
      //    //const int ns = F(nf,(nc+1)%3);
      //    const int nd = F(nf,(nc+2)%3);
      //    const bool cons = (flip(f)?fd:fs) == nd;
      //    // Normal after possibly flipping to match flip or orientation of f
      //    const auto & nN = (cons? (flip(f)?-1:1.) : (flip(f)?1.:-1.) )*N.row(nf);
      //    // Angle between n and f
      //    const auto & ndi = M_PI - atan2( fN.cross(nN).dot(eV), fN.dot(nN));
      //    cout<<"  "<<(ne%m)+1<<":\t"<<ndi<<"\t"<<di[EMAP[e]][nei]<<" "<<diIM(ne)<<endl;
      //    if(ndi>=max_di)
      //    {
      //      max_ne = ne;
      //      max_di = ndi;
      //    }
      //  }
      //}
      if(nfei >= 0)
      {
        max_ne = uE2E[EMAP(e)][nfei];
      }

      if(max_ne>=0)
      {
        // face of neighbor
        const int nf = max_ne%m;
#ifdef IGL_OUTER_HULL_DEBUG
        if(!FH[nf])
        {
          // first time seeing face
          cout<<(f+1)<<" --> "<<(nf+1)<<endl;
        }
#endif
        FH[nf] = true;
        //std::cout << "face " << face_count++ << ": " << nf << std::endl;
        //std::cout << "f " << F.row(nf).array()+1 << std::endl;
        FHcount++;
        // corner of neighbor
        const int nc = max_ne/m;
        const int nd = F(nf,(nc+2)%3);
        const bool cons = (flip(f)?fd:fs) == nd;
        flip(nf) = (cons ? flip(f) : !flip(f));
        //cout<<"flip("<<nf<<") = "<<(flip(nf)?"true":"false")<<endl;
        const int ne1 = nf+((nc+1)%3)*m;
        const int ne2 = nf+((nc+2)%3)*m;
        if(!EH[ne1])
        {
          Q.push(ne1);
        }
        if(!EH[ne2])
        {
          Q.push(ne2);
        }
      }
    }

    {
      vG[id].resize(FHcount,3);
      vJ[id].resize(FHcount,1);
      //nG += FHcount;
      size_t h = 0;
      assert(counts(id) == IM.rows());
      for(int i = 0;i<counts(id);i++)
      {
        const size_t f = IM(i);
        //if(f_flip)
        //{
        //  flip(f) = !flip(f);
        //}
        if(FH[f])
        {
          vG[id].row(h) = (flip(f)?F.row(f).reverse().eval():F.row(f));
          vJ[id](h,0) = f;
          h++;
        }
      }
      assert((int)h == FHcount);
    }
  }

  // Is A inside B? Assuming A and B are consistently oriented but closed and
  // non-intersecting.
  const auto & is_component_inside_other = [](
    const Eigen::MatrixXd & V,
    const MatrixXV & BC,
    const MatrixXG & A,
    const MatrixXJ & AJ,
    const MatrixXG & B)->bool
  {
    const auto & bounding_box = [](
      const Eigen::MatrixXd & V,
      const MatrixXG & F)->
      Eigen::MatrixXd
    {
      Eigen::MatrixXd BB(2,3);
      BB<<
         1e26,1e26,1e26,
        -1e26,-1e26,-1e26;
      const size_t m = F.rows();
      for(size_t f = 0;f<m;f++)
      {
        for(size_t c = 0;c<3;c++)
        {
          const auto & vfc = V.row(F(f,c));
          BB.row(0) = BB.row(0).array().min(vfc.array()).eval();
          BB.row(1) = BB.row(1).array().max(vfc.array()).eval();
        }
      }
      return BB;
    };
    // A lot of the time we're dealing with unrelated, distant components: cull
    // them.
    Eigen::MatrixXd ABB = bounding_box(V,A);
    Eigen::MatrixXd BBB = bounding_box(V,B);
    if( (BBB.row(0)-ABB.row(1)).maxCoeff()>0  ||
        (ABB.row(0)-BBB.row(1)).maxCoeff()>0 )
    {
      // bounding boxes do not overlap
      return false;
    }
    ////////////////////////////////////////////////////////////////////////
    // POTENTIAL ROBUSTNESS WEAK AREA
    ////////////////////////////////////////////////////////////////////////
    //

    // winding_number_3 expects colmajor
    // q could be so close (<~1e-15) to B that the winding number is not a robust way to
    // determine inside/outsideness. We could try to find a _better_ q which is
    // farther away, but couldn't they all be bad?
    double q[3] = {
        CGAL::to_double(BC(AJ(0), 0)),
        CGAL::to_double(BC(AJ(0), 1)),
        CGAL::to_double(BC(AJ(0), 2)) };
    // In a perfect world, it's enough to test a single point.
    double w;
    winding_number_3(
      V.data(),V.rows(),
      B.data(),B.rows(),
      q,1,&w);
    return w > 0.5 || w < -0.5;
  };

  Eigen::MatrixXd Vcol(V.rows(), V.cols());
  for (size_t i=0; i<(size_t)V.rows(); i++) {
      for (size_t j=0; j<(size_t)V.cols(); j++) {
          Vcol(i, j) = CGAL::to_double(V(i, j));
      }
  }

  // Reject components which are completely inside other components
  vector<bool> keep(ncc,true);
  size_t nG = 0;
  // This is O( ncc * ncc * m)
  for(size_t id = 0;id<ncc;id++)
  {
    for(size_t oid = 0;oid<ncc;oid++)
    {
      if(id == oid)
      {
        continue;
      }
      const bool inside = is_component_inside_other(Vcol,BC,vG[id],vJ[id],vG[oid]);
#ifdef IGL_OUTER_HULL_DEBUG
      cout<<id<<" is inside "<<oid<<" ? "<<inside<<endl;
#endif
      keep[id] = keep[id] && !inside;
    }
    if(keep[id])
    {
      nG += vJ[id].rows();
    }
  }

  // collect G and J across components
  G.resize(nG,3);
  J.resize(nG,1);
  {
    size_t off = 0;
    for(Index id = 0;id<(Index)ncc;id++)
    {
      if(keep[id])
      {
        assert(vG[id].rows() == vJ[id].rows());
        G.block(off,0,vG[id].rows(),vG[id].cols()) = vG[id];
        J.block(off,0,vJ[id].rows(),vJ[id].cols()) = vJ[id];
        off += vG[id].rows();
      }
    }
  }
}

template <
  typename DerivedV,
  typename DerivedF,
  typename DerivedG,
  typename DerivedJ,
  typename Derivedflip>
IGL_INLINE void igl::outer_hull(
  const Eigen::PlainObjectBase<DerivedV> & V,
  const Eigen::PlainObjectBase<DerivedF> & F,
  Eigen::PlainObjectBase<DerivedG> & G,
  Eigen::PlainObjectBase<DerivedJ> & J,
  Eigen::PlainObjectBase<Derivedflip> & flip)
{
  Eigen::Matrix<typename DerivedV::Scalar,DerivedF::RowsAtCompileTime,3> N;
  per_face_normals_stable(V,F,N);
  return outer_hull(V,F,N,G,J,flip);
}

#ifdef IGL_STATIC_LIBRARY

#include <igl/barycenter.cpp>
template void igl::barycenter<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3> >(Eigen::PlainObjectBase<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3> >&);

#include <igl/outer_facet.cpp>
template void igl::outer_facet<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<long, -1, 1, 0, -1, 1>, int>(Eigen::PlainObjectBase<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<long, -1, 1, 0, -1, 1> > const&, int&, bool&);

#include <igl/cgal/order_facets_around_edges.cpp>
template std::__1::enable_if<std::is_same<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3>::Scalar, CGAL::Lazy_exact_nt<CGAL::Gmpq> >::value, void>::type igl::order_facets_around_edges<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 2, 0, -1, 2>, Eigen::Matrix<int, -1, 2, 0, -1, 2>, Eigen::Matrix<long, -1, 1, 0, -1, 1>, long, long, bool>(Eigen::PlainObjectBase<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 2, 0, -1, 2> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 2, 0, -1, 2> > const&, Eigen::PlainObjectBase<Eigen::Matrix<long, -1, 1, 0, -1, 1> > const&, std::__1::vector<std::__1::vector<long, std::__1::allocator<long> >, std::__1::allocator<std::__1::vector<long, std::__1::allocator<long> > > > const&, std::__1::vector<std::__1::vector<long, std::__1::allocator<long> >, std::__1::allocator<std::__1::vector<long, std::__1::allocator<long> > > >&, std::__1::vector<std::__1::vector<bool, std::__1::allocator<bool> >, std::__1::allocator<std::__1::vector<bool, std::__1::allocator<bool> > > >&);

// Explicit template specialization
template void igl::outer_hull<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<long, -1, 1, 0, -1, 1>, Eigen::Matrix<bool, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> >&, Eigen::PlainObjectBase<Eigen::Matrix<long, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<bool, -1, 1, 0, -1, 1> >&);
template void igl::outer_hull<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&);
template void igl::outer_hull<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<bool, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<bool, -1, 1, 0, -1, 1> >&);
#endif


/******************************************************************************
 * Author:   Laurent Kneip                                                    *
 * Contact:  kneip.laurent@gmail.com                                          *
 * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
 *                                                                            *
 * Redistribution and use in source and binary forms, with or without         *
 * modification, are permitted provided that the following conditions         *
 * are met:                                                                   *
 * * Redistributions of source code must retain the above copyright           *
 *   notice, this list of conditions and the following disclaimer.            *
 * * 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.     *
 * * Neither the name of ANU 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 THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 ANU OR THE CONTRIBUTORS 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.                                                               *
 ******************************************************************************/

#ifndef OPENGV_ABSOLUTE_POSE_MODULES_GPNP4_MODULES_HPP_
#define OPENGV_ABSOLUTE_POSE_MODULES_GPNP4_MODULES_HPP_

#include <stdlib.h>
#include <Eigen/Eigen>
#include <vector>

namespace opengv
{
namespace absolute_pose
{
namespace modules
{
namespace gpnp4
{

void init(
    Eigen::Matrix<double,25,37> & groebnerMatrix,
    const Eigen::Matrix<double,12,1> & a,
    Eigen::Matrix<double,12,1> & n,
    Eigen::Matrix<double,12,1> & m,
    Eigen::Matrix<double,12,1> & k,
    Eigen::Matrix<double,12,1> & l,
    Eigen::Vector3d & c0,
    Eigen::Vector3d & c1,
    Eigen::Vector3d & c2,
    Eigen::Vector3d & c3 );
void compute( Eigen::Matrix<double,25,37> & groebnerMatrix );
void sPolynomial5( Eigen::Matrix<double,25,37> & groebnerMatrix );
void sPolynomial6( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow5_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial7( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow6_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial8( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow7_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial9( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow7_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow8_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow5_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow6_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow7_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow8_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow8_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial10( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow6_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow9_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial11( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow4_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow10_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow4_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial12( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow5_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow11_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial13( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow6_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow4_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow12_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial14( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow5_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow13_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial15( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow14_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow14_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial16( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow13_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow15_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow15_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow15_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial17( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow12_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow16_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow16_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial18( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow14_0001_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow14_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow17_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow17_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial19( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow18_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial20( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow18_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow19_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow19_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial21( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow20_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow20_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial22( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow19_0001_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow19_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow20_0001_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow20_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow21_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow20_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow21_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow21_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow21_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial23( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow20_1100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow21_1100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow22_1100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow19_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow22_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow22_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow22_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial24( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow23_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );

}
}
}
}

#endif /* OPENGV_ABSOLUTE_POSE_MODULES_GPNP4_MODULES_HPP_ */


#include "predictor.h"
#include <Eigen/Unsupported/KroneckerProduct>

namespace statiskit
{
    namespace glm
    {
        ScalarPredictor::ScalarPredictor(const MultivariateSampleSpace& explanatory_space)
        { _explanatory_space = explanatory_space.copy().release(); }

        ScalarPredictor::~ScalarPredictor()
        { delete _explanatory_space; }

        ScalarPredictor::ScalarPredictor(const ScalarPredictor& predictor)
        { _explanatory_space = predictor._explanatory_space->copy().release(); }

        const MultivariateSampleSpace* ScalarPredictor::get_explanatory_space() const
        { return _explanatory_space; }

        CompleteScalarPredictor::CompleteScalarPredictor(const MultivariateSampleSpace& explanatory_space) : CompleteScalarPredictor(explanatory_space, explanatory_space.encode())
        {}

        CompleteScalarPredictor::CompleteScalarPredictor(const MultivariateSampleSpace& explanatory_space, const size_t& size) : ScalarPredictor(explanatory_space)
        {
            alpha = 0.;
            _delta = Eigen::VectorXd::Zero(size);
        }

        CompleteScalarPredictor::CompleteScalarPredictor(const CompleteScalarPredictor& predictor) : ScalarPredictor(predictor)
        {
            alpha = predictor.alpha;
            _delta = predictor._delta;
        }

        double CompleteScalarPredictor::operator() (const MultivariateEvent& event) const
        { return alpha + _explanatory_space->encode(event) * _delta; }

    	void CompleteScalarPredictor::set_beta(const Eigen::VectorXd& beta)
    	{
    		if(beta.rows() != (1 + _delta.rows()) )
    		{ throw statiskit::size_error("beta", beta.rows(), 1+_delta.rows()); }
    		alpha = beta(0);
    		_delta = beta.segment(1, _delta.rows());
    	}

    	size_t CompleteScalarPredictor::size() const
    	{ return (1+_delta.size()); }

    	const Eigen::VectorXd& CompleteScalarPredictor::get_delta() const
    	{ return _delta; }

        void CompleteScalarPredictor::set_delta(const Eigen::VectorXd& delta)
        {
            if(delta.rows() != _delta.rows() )
            { throw statiskit::size_error("delta", delta.rows(), _delta.rows()); }
            _delta = delta;
        }

        std::unique_ptr< ScalarPredictor > CompleteScalarPredictor::copy() const
        { return std::make_unique< CompleteScalarPredictor >(*this); }

        ConstrainedScalarPredictor::ConstrainedScalarPredictor(const MultivariateSampleSpace& explanatory_space, const Eigen::MatrixXd& constraint) : CompleteScalarPredictor(explanatory_space, constraint.cols())
        {
        	if(constraint.rows() != explanatory_space.encode() )
        	{ throw statiskit::size_error("constraint", constraint.rows(), explanatory_space.encode() ); }
        	if(constraint.cols() > constraint.rows())
        	{ throw statiskit::size_error("constraint", constraint.cols(), constraint.rows(), statiskit::size_error::size_type::superior); }
        	_constraint = constraint;
        }

        ConstrainedScalarPredictor::ConstrainedScalarPredictor(const ConstrainedScalarPredictor& predictor) : CompleteScalarPredictor(predictor)
        { _constraint = predictor._constraint; }

        double ConstrainedScalarPredictor::operator() (const MultivariateEvent& event) const
        { return alpha + _explanatory_space->encode(event) * _constraint * _delta; }

        const Eigen::MatrixXd& ConstrainedScalarPredictor::get_constraint() const
        { return _constraint; }

    	void ConstrainedScalarPredictor::set_constraint(const Eigen::MatrixXd& constraint)
    	{
    		if(constraint.rows() != _constraint.rows())
    		{ throw statiskit::size_error("constraint", constraint.rows(), _constraint.rows()); }
    		if(constraint.cols() != _constraint.cols())
    		{ throw statiskit::size_error("constraint", constraint.cols(), _constraint.cols()); }
    		_constraint = constraint;
    	}

        std::unique_ptr< ScalarPredictor > ConstrainedScalarPredictor::copy() const
        { return std::make_unique< ConstrainedScalarPredictor >(*this); }

        VectorPredictor::VectorPredictor(const MultivariateSampleSpace& explanatory_space)
        { _explanatory_space = explanatory_space.copy().release(); }

        VectorPredictor::~VectorPredictor()
        { delete _explanatory_space; }

        VectorPredictor::VectorPredictor(const VectorPredictor& predictor)
        { _explanatory_space = predictor._explanatory_space->copy().release(); }

        const MultivariateSampleSpace* VectorPredictor::get_explanatory_space() const
        { return _explanatory_space; }

        CompleteVectorPredictor::CompleteVectorPredictor(const MultivariateSampleSpace& explanatory_space, const size_t& dimension) : VectorPredictor(explanatory_space)
        {
        	_alpha = Eigen::VectorXd::Zero(dimension);
        	_delta = Eigen::MatrixXd::Zero(dimension, explanatory_space.encode());
        }

        CompleteVectorPredictor::CompleteVectorPredictor(const CompleteVectorPredictor& predictor) : VectorPredictor(predictor)
        {
        	_alpha = predictor._alpha;
        	_delta = predictor._delta;
        }

        Eigen::VectorXd CompleteVectorPredictor::operator() (const MultivariateEvent& event) const
        { return _alpha + _delta * (_explanatory_space->encode(event)).transpose(); }

        size_t CompleteVectorPredictor::size() const
        { return _alpha.rows() + _delta.size(); }

        void CompleteVectorPredictor::set_beta(const Eigen::VectorXd& beta)
    	{
    		if(beta.rows() != _alpha.rows() + _delta.rows() * _delta.cols())
    		{ throw statiskit::size_error("beta", beta.rows(),  _alpha.rows() + _delta.rows() * _delta.cols()); }
    		_alpha = beta.segment(0, _alpha.rows());
    		for(size_t j=0; j<_alpha.rows(); ++j)
            { _delta.block(j, 0, 1, _delta.cols()) = ( beta.segment(_alpha.rows() + j * _delta.cols(),_delta.cols() ) ).transpose(); } //_alpha.rows() + (j+1) * _delta.cols() - 1) ).transpose(); }
    	}

    	const Eigen::VectorXd& CompleteVectorPredictor::get_alpha() const
    	{ return _alpha; }

    	void CompleteVectorPredictor::set_alpha(const Eigen::VectorXd& alpha)
        {
            if(alpha.rows() != _alpha.rows() )
            { throw statiskit::size_error("alpha", alpha.rows(),  _alpha.rows() ); }
            _alpha = alpha;
        }

    	const Eigen::MatrixXd& CompleteVectorPredictor::get_delta() const
    	{ return _delta; }

        void CompleteVectorPredictor::set_delta(const Eigen::MatrixXd& delta)
        {
            if(delta.rows() != _delta.rows() )
            { throw statiskit::size_error("delta nb rows", delta.rows(),  _delta.rows() ); }
            if(delta.cols() != _delta.cols() )
            { throw statiskit::size_error("delta nb cols", delta.cols(),  _delta.cols() ); }
            _delta = delta;
        }

        std::unique_ptr< VectorPredictor > CompleteVectorPredictor::copy() const
        { return std::make_unique< CompleteVectorPredictor >(*this); }

        ProportionalVectorPredictor::ProportionalVectorPredictor(const MultivariateSampleSpace& explanatory_space, const size_t& dimension) : VectorPredictor(explanatory_space)
        {
        	_alpha = Eigen::VectorXd::Zero(dimension);
        	_delta = Eigen::VectorXd::Zero(explanatory_space.encode());
        }

        ProportionalVectorPredictor::ProportionalVectorPredictor(const ProportionalVectorPredictor& predictor) : VectorPredictor(predictor)
        {
         	_alpha = predictor._alpha;
        	_delta = predictor._delta;
        }

        Eigen::VectorXd ProportionalVectorPredictor::operator() (const MultivariateEvent& event) const
        { return _alpha + _explanatory_space->encode(event) * _delta * Eigen::VectorXd::Ones(_alpha.rows()); }

        size_t ProportionalVectorPredictor::size() const
        { return _alpha.rows() + _delta.rows(); }

        void ProportionalVectorPredictor::set_beta(const Eigen::VectorXd& beta)
    	{
    		if(beta.rows() != _alpha.rows() + _delta.rows() * _delta.cols())
    		{ throw statiskit::size_error("beta", beta.rows(),  _alpha.rows() + _delta.rows() * _delta.cols()); }
    		_alpha = beta.segment(0, _alpha.rows() );
    		_delta = beta.segment(_alpha.rows(),  _delta.rows() );
    	}

    	const Eigen::VectorXd& ProportionalVectorPredictor::get_alpha() const
    	{ return _alpha; }

        void ProportionalVectorPredictor::set_alpha(const Eigen::VectorXd& alpha)
        {
            if(alpha.rows() != _alpha.rows() )
            { throw statiskit::size_error("alpha", alpha.rows(),  _alpha.rows() ); }
            _alpha = alpha;
        }

    	const Eigen::VectorXd& ProportionalVectorPredictor::get_delta() const
    	{ return _delta; }

        void ProportionalVectorPredictor::set_delta(const Eigen::VectorXd& delta)
        {
            if(delta.rows() != _delta.rows() )
            { throw statiskit::size_error("delta", delta.rows(),  _delta.rows() ); }
            _delta = delta;
        }

        std::unique_ptr< VectorPredictor > ProportionalVectorPredictor::copy() const
        { return std::make_unique< ProportionalVectorPredictor >(*this); }

        ProportionalVectorPredictor::ProportionalVectorPredictor(const MultivariateSampleSpace& explanatory_space) : VectorPredictor(explanatory_space)
        {}

        ConstrainedVectorPredictor::ConstrainedVectorPredictor(const MultivariateSampleSpace& explanatory_space, const size_t& dimension, const Eigen::MatrixXd& constraint) : ProportionalVectorPredictor(explanatory_space)
        {
        	if(constraint.rows() != ( explanatory_space.encode() * dimension ) )
        	{ throw statiskit::size_error("constraint", constraint.rows(),  explanatory_space.encode() * dimension ); }
        	if(constraint.cols() > constraint.rows())
        	{ throw statiskit::size_error("constraint", constraint.cols(), constraint.rows(), statiskit::size_error::size_type::superior); }
        	_constraint = constraint;
        	_intercept_constraint = Eigen::MatrixXd::Identity(dimension, dimension);
            _alpha = Eigen::VectorXd::Zero(dimension);
            _delta = Eigen::VectorXd::Zero(constraint.cols());
        }

        ConstrainedVectorPredictor::ConstrainedVectorPredictor(const MultivariateSampleSpace& explanatory_space, const Eigen::MatrixXd& constraint, const Eigen::MatrixXd& intercept_constraint) : ProportionalVectorPredictor(explanatory_space)
        {
        	if(constraint.rows() != ( explanatory_space.encode() * intercept_constraint.rows()) )
        	{ throw statiskit::size_error("constraint", constraint.rows(),  explanatory_space.encode() * intercept_constraint.rows()); }
        	if(constraint.cols() >  constraint.rows())
        	{ throw statiskit::size_error("constraint", constraint.cols(), constraint.rows(), statiskit::size_error::size_type::superior); }
        	_constraint = constraint;
        	if(intercept_constraint.cols() > intercept_constraint.rows() )
        	{ throw statiskit::size_error("intercept_constraint", intercept_constraint.cols(), intercept_constraint.rows(), statiskit::size_error::size_type::superior); }
        	_intercept_constraint = intercept_constraint;
            _alpha = Eigen::VectorXd::Zero(intercept_constraint.cols());
            _delta = Eigen::VectorXd::Zero(constraint.cols());
        }

        ConstrainedVectorPredictor::ConstrainedVectorPredictor(const MultivariateSampleSpace& explanatory_space, const size_t& dimension, const Indices& proportional) : ConstrainedVectorPredictor(explanatory_space, dimension, partial_proportional_constraint(explanatory_space, dimension, proportional))
        {}

        ConstrainedVectorPredictor::ConstrainedVectorPredictor(const ConstrainedVectorPredictor& predictor) : ProportionalVectorPredictor(predictor)
        {
        	_constraint = predictor._constraint;
        	_intercept_constraint = predictor._intercept_constraint;
        }

        Eigen::VectorXd ConstrainedVectorPredictor::operator() (const MultivariateEvent& event) const
        {
            Eigen::VectorXd xt = _explanatory_space->encode(event).transpose();
            Eigen::MatrixXd Identity = Eigen::MatrixXd::Identity(_intercept_constraint.rows(), _intercept_constraint.rows());
            return _intercept_constraint * _alpha + Eigen::kroneckerProduct(Identity, xt) * _constraint * _delta;
        }

        const Eigen::MatrixXd& ConstrainedVectorPredictor::get_constraint() const
        { return _constraint; }

    	void ConstrainedVectorPredictor::set_constraint(const Eigen::MatrixXd& constraint)
    	{
    		if(constraint.rows() != _constraint.rows())
    		{ throw statiskit::size_error("constraint", constraint.rows(), _constraint.rows()); }
    		if(constraint.cols() != _constraint.cols())
    		{ throw statiskit::size_error("constraint", constraint.cols(), _constraint.cols()); }
    		_constraint = constraint;
    	}

        const Eigen::MatrixXd& ConstrainedVectorPredictor::get_intercept_constraint() const
        { return _intercept_constraint; }

    	void ConstrainedVectorPredictor::set_intercept_constraint(const Eigen::MatrixXd& intercept_constraint)
    	{
    		if(intercept_constraint.rows() != _intercept_constraint.rows())
    		{ throw statiskit::size_error("intercept_constraint", intercept_constraint.rows(), _intercept_constraint.rows()); }
    		if(intercept_constraint.cols() != _intercept_constraint.cols())
    		{ throw statiskit::size_error("intercept_constraint", intercept_constraint.cols(), _intercept_constraint.cols()); }
    		_intercept_constraint = intercept_constraint;
    	}

        std::unique_ptr< VectorPredictor > ConstrainedVectorPredictor::copy() const
        { return std::make_unique< ConstrainedVectorPredictor >(*this); }

        Eigen::MatrixXd ConstrainedVectorPredictor::partial_proportional_constraint(const MultivariateSampleSpace& explanatory_space, const size_t& dimension, const Indices& proportional)
        {
            Index nb_cols=0, nb_block_rows=0, current_row = 0, current_col=0;
            std::vector< Index > rows, cols;
            for(Index i=0; i<explanatory_space.size(); ++i)
            {
                if(explanatory_space.get(i)->get_outcome() == outcome_type::CATEGORICAL)
                {
                    Index K = static_cast< const CategoricalSampleSpace* >( explanatory_space.get(i) )->get_cardinality();
                    rows.push_back(K-1);
                }
                else
                { rows.push_back(1); }
                nb_block_rows += rows.back();

                if(proportional.find(i) == proportional.end()) // complete
                { cols.push_back(rows.back() * dimension); }
                else
                { cols.push_back(rows.back()); }
                nb_cols += cols.back();
            }
            Eigen::MatrixXd constraint = Eigen::MatrixXd::Zero(nb_block_rows*dimension, nb_cols);
            for(Index i=0; i<explanatory_space.size(); ++i)
            {
                Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(rows[i], rows[i]);
                if(proportional.find(i) == proportional.end())
                {
                    for(Index j=0; j<dimension; ++j)
                    {
                        Eigen::RowVectorXd indicator = Eigen::RowVectorXd::Zero(dimension);
                        indicator[j] = 1;
                        constraint.block(j*nb_block_rows + current_row, current_col, rows[i], cols[i]) = Eigen::kroneckerProduct(indicator, identity);
                    }
                }
                else
                {
                    for(Index j=0; j<dimension; ++j)
                    {constraint.block(j*nb_block_rows + current_row, current_col, rows[i], cols[i]) = identity; }
                }
                current_col += cols[i];
                current_row += rows[i];
            }
            return constraint;
        }

        Eigen::MatrixXd ConstrainedVectorPredictor::partial_proportional_constraint(const UnivariateConditionalData& data, const Indices& proportional)
        {
            Index J = static_cast< const CategoricalSampleSpace* >(data.get_response()->get_sample_space())->get_cardinality();
            const MultivariateData* _data = data.get_explanatories();
            const MultivariateSampleSpace* explanatory_space = _data->get_sample_space();
            return partial_proportional_constraint(*explanatory_space, J-1, proportional);
        }
    }
}


// Copyright 2021, Autonomous Space Robotics Lab (ASRL)
//
// 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.

/**
 * \file cloud.hpp
 * \brief PointXYZ, PointXY, PointCloud class definition
 *
 * \author Hugues Thomas, Autonomous Space Robotics Lab (ASRL)
 */
#pragma once

#include <stdio.h>
#include <string.h>
#include <algorithm>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <map>
#include <numeric>
#include <unordered_map>
#include <vector>

#include <time.h>

#include <Eigen/Dense>

#include <vtr_lidar/npm_ply/ply_file_in.h>
#include <vtr_lidar/npm_ply/ply_file_out.h>
#include <vtr_lidar/npm_ply/ply_types.h>

namespace vtr {
namespace lidar {

//------------------------------------------------------------------------------------------------------------
// Point class
// ***********
//
//------------------------------------------------------------------------------------------------------------

class PointXYZ {
 public:
  using Vector3fMap = Eigen::Map<Eigen::Vector3f>;
  using Vector3fMapConst = const Eigen::Map<const Eigen::Vector3f>;
  // Elements
  // ********
  union {
    struct {
      float x;
      float y;
      float z;
    };
    float data[3];
  };

  // Methods
  // *******

  // Constructor
  PointXYZ(float x0 = 0, float y0 = 0, float z0 = 0) : x(x0), y(y0), z(z0) {}

  Vector3fMap getVector3fMap() { return (Vector3fMap(data)); }
  Vector3fMapConst getVector3fMap() const { return (Vector3fMapConst(data)); }

  // array type accessor
  float operator[](int i) const {
    if (i == 0)
      return x;
    else if (i == 1)
      return y;
    else
      return z;
  }

  // opperations
  float dot(const PointXYZ P) const { return x * P.x + y * P.y + z * P.z; }

  float sq_norm() const { return x * x + y * y + z * z; }

  PointXYZ cross(const PointXYZ P) const {
    return PointXYZ(y * P.z - z * P.y, z * P.x - x * P.z, x * P.y - y * P.x);
  }

  PointXYZ& operator+=(const PointXYZ& P) {
    x += P.x;
    y += P.y;
    z += P.z;
    return *this;
  }

  PointXYZ& operator-=(const PointXYZ& P) {
    x -= P.x;
    y -= P.y;
    z -= P.z;
    return *this;
  }

  PointXYZ& operator*=(const float& a) {
    x *= a;
    y *= a;
    z *= a;
    return *this;
  }
};

// Point Opperations
// *****************

inline PointXYZ operator+(const PointXYZ A, const PointXYZ B) {
  return PointXYZ(A.x + B.x, A.y + B.y, A.z + B.z);
}

inline PointXYZ operator-(const PointXYZ A, const PointXYZ B) {
  return PointXYZ(A.x - B.x, A.y - B.y, A.z - B.z);
}

inline PointXYZ operator*(const PointXYZ P, const float a) {
  return PointXYZ(P.x * a, P.y * a, P.z * a);
}

inline PointXYZ operator*(const float a, const PointXYZ P) {
  return PointXYZ(P.x * a, P.y * a, P.z * a);
}

inline PointXYZ operator/(const PointXYZ P, const float a) {
  return PointXYZ(P.x / a, P.y / a, P.z / a);
}

inline PointXYZ operator/(const float a, const PointXYZ P) {
  return PointXYZ(P.x / a, P.y / a, P.z / a);
}

inline std::ostream& operator<<(std::ostream& os, const PointXYZ P) {
  return os << "[" << P.x << ", " << P.y << ", " << P.z << "]";
}

inline bool operator==(const PointXYZ A, const PointXYZ B) {
  return A.x == B.x && A.y == B.y && A.z == B.z;
}

inline PointXYZ floor(const PointXYZ P) {
  return PointXYZ(std::floor(P.x), std::floor(P.y), std::floor(P.z));
}

PointXYZ max_point(const std::vector<PointXYZ>& points);
PointXYZ min_point(const std::vector<PointXYZ>& points);
PointXYZ max_point(const PointXYZ A, const PointXYZ B);
PointXYZ min_point(const PointXYZ A, const PointXYZ B);

//------------------------------------------------------------------------------------------------------------
// Point class 2D
// **************
//
//------------------------------------------------------------------------------------------------------------

class PointXY {
 public:
  // Elements
  // ********

  float x, y;

  // Methods
  // *******

  // Constructor
  PointXY() {
    x = 0;
    y = 0;
  }
  PointXY(float x0, float y0) {
    x = x0;
    y = y0;
  }
  PointXY(PointXYZ P) {
    x = P.x;
    y = P.y;
  }

  // array type accessor
  float operator[](int i) const {
    if (i == 0)
      return x;
    else
      return y;
  }

  // opperations
  float dot(const PointXY P) const { return x * P.x + y * P.y; }

  float sq_norm() const { return x * x + y * y; }

  float cross(const PointXY P) const { return x * P.y - y * P.x; }

  PointXY& operator+=(const PointXY& P) {
    x += P.x;
    y += P.y;
    return *this;
  }

  PointXY& operator-=(const PointXY& P) {
    x -= P.x;
    y -= P.y;
    return *this;
  }

  PointXY& operator*=(const float& a) {
    x *= a;
    y *= a;
    return *this;
  }
};

// Point Opperations
// *****************

inline PointXY operator+(const PointXY A, const PointXY B) {
  return PointXY(A.x + B.x, A.y + B.y);
}

inline PointXY operator-(const PointXY A, const PointXY B) {
  return PointXY(A.x - B.x, A.y - B.y);
}

inline PointXY operator*(const PointXY P, const float a) {
  return PointXY(P.x * a, P.y * a);
}

inline PointXY operator*(const float a, const PointXY P) {
  return PointXY(P.x * a, P.y * a);
}

inline PointXY operator/(const PointXY P, const float a) {
  return PointXY(P.x / a, P.y / a);
}

inline PointXY operator/(const float a, const PointXY P) {
  return PointXY(P.x / a, P.y / a);
}

inline std::ostream& operator<<(std::ostream& os, const PointXY P) {
  return os << "[" << P.x << ", " << P.y << "]";
}

inline bool operator==(const PointXY A, const PointXY B) {
  return A.x == B.x && A.y == B.y;
}

inline PointXY floor(const PointXY P) {
  return PointXY(std::floor(P.x), std::floor(P.y));
}

//------------------------------------------------------------------------------------------------------------
// Pointcloud class
// ****************
//
//------------------------------------------------------------------------------------------------------------

struct PointCloud {
  std::vector<PointXYZ> pts;

  // Must return the number of data points
  inline size_t kdtree_get_point_count() const { return pts.size(); }

  // Returns the dim'th component of the idx'th point in the class:
  // Since this is inlined and the "dim" argument is typically an immediate
  // value, the
  //  "if/else's" are actually solved at compile time.
  inline float kdtree_get_pt(const size_t idx, const size_t dim) const {
    if (dim == 0)
      return pts[idx].x;
    else if (dim == 1)
      return pts[idx].y;
    else
      return pts[idx].z;
  }

  // Optional bounding-box computation: return false to default to a standard
  // bbox computation loop.
  //   Return true if the BBOX was already computed by the class and returned in
  //   "bb" so it can be avoided to redo it again. Look at bb.size() to find out
  //   the expected dimensionality (e.g. 2 or 3 for point clouds)
  template <class BBOX>
  bool kdtree_get_bbox(BBOX& /* bb */) const {
    return false;
  }
};

// Utility function for pointclouds
template <typename T1, typename T2>
void filterVector(std::vector<T1>& vec, std::vector<T2>& scores,
                  T2 filter_value) {
  // Remove every element whose score is < filter_value
  auto vec_address = vec.data();
  vec.erase(std::remove_if(vec.begin(), vec.end(),
                           [&scores, vec_address, filter_value](const T1& f) {
                             return scores[(size_t)(&f - vec_address)] <
                                    filter_value;
                           }),
            vec.end());
}

template <typename T>
void filterVector(std::vector<T>& vec, T filter_value) {
  vec.erase(std::remove_if(
                vec.begin(), vec.end(),
                [filter_value](const float s) { return s < filter_value; }),
            vec.end());
}

template <typename T>
void filterFloatVector(std::vector<T>& vec, std::vector<float>& scores,
                       float filter_value) {
  // Remove every element whose score is < filter_value
  auto vec_address = vec.data();
  vec.erase(std::remove_if(vec.begin(), vec.end(),
                           [&scores, vec_address, filter_value](const T& f) {
                             return scores[(size_t)(&f - vec_address)] <
                                    filter_value;
                           }),
            vec.end());
}

void filterPointCloud(std::vector<PointXYZ>& pts, std::vector<float>& scores,
                      float filter_value);
void filterFloatVector(std::vector<float>& vec, float filter_value);

// PLY reading/saving functions
void save_cloud(std::string dataPath, std::vector<PointXYZ>& points,
                std::vector<PointXYZ>& normals, std::vector<float>& features);
void save_cloud(std::string dataPath, std::vector<PointXYZ>& points,
                std::vector<float>& features);
void save_cloud(std::string dataPath, std::vector<PointXYZ>& points,
                std::vector<PointXYZ>& normals);
void save_cloud(std::string dataPath, std::vector<PointXYZ>& points);

void load_cloud(std::string& dataPath, std::vector<PointXYZ>& points);

void load_cloud(std::string& dataPath, std::vector<PointXYZ>& points,
                std::vector<float>& float_scalar,
                std::string& float_scalar_name, std::vector<int>& int_scalar,
                std::string& int_scalar_name);

void load_cloud_normals(std::string& dataPath, std::vector<PointXYZ>& points,
                        std::vector<PointXYZ>& normals,
                        std::vector<float>& float_scalar,
                        std::string& float_scalar_name,
                        std::vector<int>& int_scalar,
                        std::string& int_scalar_name);

//------------------------------------------------------------------------------------------------------------
// Plane3D class
// *************
//
//------------------------------------------------------------------------------------------------------------

class Plane3D {
 public:
  // Elements
  // ********

  // The plane is define by the equation a*x + b*y + c*z = d. The values (a, b,
  // c) are stored in a PointXYZ called u.
  PointXYZ u;
  float d;

  // Methods
  // *******

  // Constructor
  Plane3D() {
    u.x = 1;
    u.y = 0;
    u.z = 0;
    d = 0;
  }
  Plane3D(const float a0, const float b0, const float c0, const float d0) {
    u.x = a0;
    u.y = b0;
    u.z = c0;
    d = d0;
  }
  Plane3D(const PointXYZ P0, const PointXYZ N0) {
    // Init with point and normal
    u = N0;
    d = N0.dot(P0);
  }
  Plane3D(const PointXYZ A, const PointXYZ B, const PointXYZ C) {
    // Init with three points
    u = (B - A).cross(C - A);
    d = u.dot(A);
  }

  // Method getting distance to one point
  float point_distance(const PointXYZ P) {
    return std::abs((u.dot(P) - d) / std::sqrt(u.sq_norm()));
  }

  // Method getting square distance to one point
  float point_sq_dist(const PointXYZ P) {
    float tmp = u.dot(P) - d;
    return tmp * tmp / u.sq_norm();
  }

  // Method getting distances to some points
  void point_distances(std::vector<PointXYZ>& points,
                       std::vector<float>& distances) {
    if (distances.size() != points.size())
      distances = std::vector<float>(points.size());
    size_t i = 0;
    float inv_norm_u = 1 / std::sqrt(u.sq_norm());
    for (auto& p : points) {
      distances[i] = std::abs((u.dot(p) - d) * inv_norm_u);
      i++;
    }
  }

  int in_range(std::vector<PointXYZ>& points, float threshold) {
    int count = 0;
    float inv_norm_u = 1 / std::sqrt(u.sq_norm());
    for (auto& p : points) {
      if (std::abs((u.dot(p) - d) * inv_norm_u) < threshold) count++;
    }
    return count;
  }
};

}  // namespace lidar
}  // namespace vtr


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file  CombinedImuFactor.cpp
 *  @author Luca Carlone
 *  @author Stephen Williams
 *  @author Richard Roberts
 *  @author Vadim Indelman
 *  @author David Jensen
 *  @author Frank Dellaert
 *  @author Varun Agrawal
 **/

#include <gtsam/navigation/CombinedImuFactor.h>
#include <boost/serialization/export.hpp>

/* External or standard includes */
#include <ostream>

namespace gtsam {

using namespace std;

//------------------------------------------------------------------------------
// Inner class PreintegrationCombinedParams
//------------------------------------------------------------------------------
void PreintegrationCombinedParams::print(const string& s) const {
  PreintegrationParams::print(s);
  cout << "biasAccCovariance:\n[\n" << biasAccCovariance << "\n]"
       << endl;
  cout << "biasOmegaCovariance:\n[\n" << biasOmegaCovariance << "\n]"
       << endl;
  cout << "biasAccOmegaInt:\n[\n" << biasAccOmegaInt << "\n]"
       << endl;
}

//------------------------------------------------------------------------------
bool PreintegrationCombinedParams::equals(const PreintegratedRotationParams& other,
                                  double tol) const {
  auto e = dynamic_cast<const PreintegrationCombinedParams*>(&other);
  return e != nullptr && PreintegrationParams::equals(other, tol) &&
         equal_with_abs_tol(biasAccCovariance, e->biasAccCovariance,
                            tol) &&
         equal_with_abs_tol(biasOmegaCovariance, e->biasOmegaCovariance,
                            tol) &&
         equal_with_abs_tol(biasAccOmegaInt, e->biasAccOmegaInt, tol);
}

//------------------------------------------------------------------------------
// Inner class PreintegratedCombinedMeasurements
//------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::print(const string& s) const {
  PreintegrationType::print(s);
  cout << "  preintMeasCov [ " << preintMeasCov_ << " ]" << endl;
}

//------------------------------------------------------------------------------
bool PreintegratedCombinedMeasurements::equals(
    const PreintegratedCombinedMeasurements& other, double tol) const {
  return PreintegrationType::equals(other, tol)
      && equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
}

//------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::resetIntegration() {
  PreintegrationType::resetIntegration();
  preintMeasCov_.setZero();
}

//------------------------------------------------------------------------------
// sugar for derivative blocks
#define D_R_R(H) (H)->block<3,3>(0,0)
#define D_R_t(H) (H)->block<3,3>(0,3)
#define D_R_v(H) (H)->block<3,3>(0,6)
#define D_t_R(H) (H)->block<3,3>(3,0)
#define D_t_t(H) (H)->block<3,3>(3,3)
#define D_t_v(H) (H)->block<3,3>(3,6)
#define D_v_R(H) (H)->block<3,3>(6,0)
#define D_v_t(H) (H)->block<3,3>(6,3)
#define D_v_v(H) (H)->block<3,3>(6,6)
#define D_a_a(H) (H)->block<3,3>(9,9)
#define D_g_g(H) (H)->block<3,3>(12,12)

//------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::integrateMeasurement(
    const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
  // Update preintegrated measurements.
  Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
  Matrix93 B, C;
  PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);

  // Update preintegrated measurements covariance: as in [2] we consider a first
  // order propagation that can be seen as a prediction phase in an EKF
  // framework. In this implementation, in contrast to [2], we consider the
  // uncertainty of the bias selection and we keep correlation between biases
  // and preintegrated measurements

  // Single Jacobians to propagate covariance
  // TODO(frank): should we not also account for bias on position?
  Matrix3 theta_H_biasOmega = -C.topRows<3>();
  Matrix3 vel_H_biasAcc = -B.bottomRows<3>();

  // overall Jacobian wrt preintegrated measurements (df/dx)
  Eigen::Matrix<double, 15, 15> F;
  F.setZero();
  F.block<9, 9>(0, 0) = A;
  F.block<3, 3>(0, 12) = theta_H_biasOmega;
  F.block<3, 3>(6, 9) = vel_H_biasAcc;
  F.block<6, 6>(9, 9) = I_6x6;

  // propagate uncertainty
  // TODO(frank): use noiseModel routine so we can have arbitrary noise models.
  const Matrix3& aCov = p().accelerometerCovariance;
  const Matrix3& wCov = p().gyroscopeCovariance;
  const Matrix3& iCov = p().integrationCovariance;

  // first order uncertainty propagation
  // Optimized matrix multiplication   (1/dt) * G * measurementCovariance *
  // G.transpose()
  Eigen::Matrix<double, 15, 15> G_measCov_Gt;
  G_measCov_Gt.setZero(15, 15);

  // BLOCK DIAGONAL TERMS
  D_t_t(&G_measCov_Gt) = dt * iCov;
  D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc
      * (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0))
      * (vel_H_biasAcc.transpose());
  D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega
      * (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3))
      * (theta_H_biasOmega.transpose());
  D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
  D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;

  // OFF BLOCK DIAGONAL TERMS
  Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0)
      * theta_H_biasOmega.transpose();
  D_v_R(&G_measCov_Gt) = temp;
  D_R_v(&G_measCov_Gt) = temp.transpose();
  preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
}

//------------------------------------------------------------------------------
// CombinedImuFactor methods
//------------------------------------------------------------------------------
CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j,
    Key vel_j, Key bias_i, Key bias_j,
    const PreintegratedCombinedMeasurements& pim) :
    Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
        pose_j, vel_j, bias_i, bias_j), _PIM_(pim) {
}

//------------------------------------------------------------------------------
gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {
  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
      gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}

//------------------------------------------------------------------------------
void CombinedImuFactor::print(const string& s,
    const KeyFormatter& keyFormatter) const {
  cout << (s.empty() ? s : s + "\n") << "CombinedImuFactor("
       << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ","
       << keyFormatter(this->key3()) << "," << keyFormatter(this->key4()) << ","
       << keyFormatter(this->key5()) << "," << keyFormatter(this->key6())
       << ")\n";
  _PIM_.print("  preintegrated measurements:");
  this->noiseModel_->print("  noise model: ");
}

//------------------------------------------------------------------------------
bool CombinedImuFactor::equals(const NonlinearFactor& other, double tol) const {
  const This* e = dynamic_cast<const This*>(&other);
  return e != nullptr && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol);
}

//------------------------------------------------------------------------------
Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
    const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
    const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
    boost::optional<Matrix&> H3, boost::optional<Matrix&> H4,
    boost::optional<Matrix&> H5, boost::optional<Matrix&> H6) const {

  // error wrt bias evolution model (random walk)
  Matrix6 Hbias_i, Hbias_j;
  Vector6 fbias = traits<imuBias::ConstantBias>::Between(bias_j, bias_i,
      H6 ? &Hbias_j : 0, H5 ? &Hbias_i : 0).vector();

  Matrix96 D_r_pose_i, D_r_pose_j, D_r_bias_i;
  Matrix93 D_r_vel_i, D_r_vel_j;

  // error wrt preintegrated measurements
  Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j,
      bias_i, H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0,
      H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0);

  // if we need the jacobians
  if (H1) {
    H1->resize(15, 6);
    H1->block<9, 6>(0, 0) = D_r_pose_i;
    // adding: [dBiasAcc/dPi ; dBiasOmega/dPi]
    H1->block<6, 6>(9, 0).setZero();
  }
  if (H2) {
    H2->resize(15, 3);
    H2->block<9, 3>(0, 0) = D_r_vel_i;
    // adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
    H2->block<6, 3>(9, 0).setZero();
  }
  if (H3) {
    H3->resize(15, 6);
    H3->block<9, 6>(0, 0) = D_r_pose_j;
    // adding: [dBiasAcc/dPj ; dBiasOmega/dPj]
    H3->block<6, 6>(9, 0).setZero();
  }
  if (H4) {
    H4->resize(15, 3);
    H4->block<9, 3>(0, 0) = D_r_vel_j;
    // adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
    H4->block<6, 3>(9, 0).setZero();
  }
  if (H5) {
    H5->resize(15, 6);
    H5->block<9, 6>(0, 0) = D_r_bias_i;
    // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i]
    H5->block<6, 6>(9, 0) = Hbias_i;
  }
  if (H6) {
    H6->resize(15, 6);
    H6->block<9, 6>(0, 0).setZero();
    // adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j]
    H6->block<6, 6>(9, 0) = Hbias_j;
  }

  // overall error
  Vector r(15);
  r << r_Rpv, fbias; // vector of size 15
  return r;
}

//------------------------------------------------------------------------------
std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) {
  f._PIM_.print("combined preintegrated measurements:\n");
  os << "  noise model sigmas: " << f.noiseModel_->sigmas().transpose();
  return os;
}
}
 /// namespace gtsam

/// Boost serialization export definition for derived class
BOOST_CLASS_EXPORT_IMPLEMENT(gtsam::PreintegrationCombinedParams);


#include <iostream>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>
#include "netsne.h"
#include <armadillo>

using namespace std;
namespace po = boost::program_options;
namespace fsys = boost::filesystem;

bool load_data(string infile_X, mat &X, int &num_instances, int &num_features) {

  FILE *fp = fopen(infile_X.c_str(), "rb");
	if (fp == NULL) {
		cout << "Error: could not open data file " << infile_X << endl;
		return false;
	}

  uint64_t ret;
	ret = fread(&num_instances, sizeof(int), 1, fp);
	ret = fread(&num_features, sizeof(int), 1, fp);

  X.set_size(num_features, num_instances);

  uint64_t nelem = (uint64_t)num_instances * num_features;

  size_t batch_size = 1e8;
  double *ptr = X.memptr();
  ret = 0;
  for (uint64_t remaining = nelem; remaining > 0; remaining -= batch_size) {
    if (remaining < batch_size) {
      batch_size = remaining;
    }
    ret += fread(ptr, sizeof(double), batch_size, fp);
    ptr += batch_size;
  }

  if (ret != nelem) {
    cout << "Error: reading input returned incorrect number of elements (" << ret
         << ", expected " << nelem << ")" << endl;
    return false;
  }

	fclose(fp);

	return true;
}

int main(int argc, char **argv) {
  // Declare the supported options
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help", "produce help message")
    ("input-X", po::value<string>()->value_name("FILE")->default_value("data.dat"), "name of binary input file containing data feature matrix (see prepare_input.m)")
    ("input-P", po::value<string>()->value_name("FILE")->default_value("P.dat"), "name of binary input file containing P matrix (see ComputeP)")
    ("input-Y", po::value<string>()->value_name("FILE"), "if this option is provided, net-SNE will train to match the provided embedding instead of using the P matrix")
    ("out-dir", po::value<string>()->value_name("DIR")->default_value("out"), "where to create output files; directory will be created if it does not exist")
    ("out-dim", po::value<int>()->value_name("NUM")->default_value(2), "number of output dimensions")
    ("max-iter", po::value<int>()->value_name("NUM")->default_value(1000), "maximum number of iterations")
    ("rand-seed", po::value<int>()->value_name("NUM")->default_value(-1), "seed for random number generator; to use current time as seed set it to -1")
    ("theta", po::value<double>()->value_name("NUM")->default_value(0.5, "0.5"), "a value between 0 and 1 that controls the accuracy-efficiency tradeoff in SPTree for gradient computation; 0 means exact")
    ("learn-rate", po::value<double>()->value_name("NUM")->default_value(0.02, "0.02"), "learning rate for gradient steps")
    ("mom-init", po::value<double>()->value_name("NUM")->default_value(0.5, "0.5"), "initial momentum between 0 and 1")
    ("mom-final", po::value<double>()->value_name("NUM")->default_value(0.8, "0.8"), "final momentum between 0 and 1 (switch point controlled by --mom-switch-iter)")
    ("mom-switch-iter", po::value<int>()->value_name("NUM")->default_value(250), "duration (number of iterations) of initial momentum")
    ("early-exag-iter", po::value<int>()->value_name("NUM")->default_value(250), "duration (number of iterations) of early exaggeration")
    ("num-local-sample", po::value<int>()->value_name("NUM")->default_value(20), "number of local samples for each data point in the mini-batch")
    ("batch-frac", po::value<double>()->value_name("NUM")->default_value(0.1, "0.1"), "fraction of data to sample for mini-batch")
    ("min-sample-Z", po::value<double>()->value_name("NUM")->default_value(0.1, "0.1"), "minimum fraction of data to use for approximating the normalization factor Z in the gradient")
    ("l2-reg", po::value<double>()->value_name("NUM")->default_value(0, "0"), "L2 regularization parameter")
    ("init-model-prefix", po::value<string>()->value_name("STR"), "prefix of model files for initialization")
    ("step-method", po::value<string>()->value_name("STR")->default_value("adam"), "gradient step schedule; 'adam', 'mom' (momentum), 'mom_gain' (momentum with gains), 'fixed'")
    ("num-input-feat", po::value<int>()->value_name("NUM"), "if set, use only the first NUM features for the embedding function")
    ("init-map", po::bool_switch()->default_value(false), "output initial mapping for the entire data")
    ("num-layers", po::value<int>()->value_name("NUM")->default_value(2), "number of layers in the neural network")
    ("num-units", po::value<int>()->value_name("NUM")->default_value(50), "number of units for each layer in the neural network")
    ("act-fn", po::value<string>()->value_name("STR")->default_value("relu"), "activation function of the neural network; 'sigmoid' or 'relu'")
    ("test-model", po::bool_switch()->default_value(false), "if set, use the model provided with --init-model-prefix and visualize the entire data set then terminate without training")
    ("no-target", po::bool_switch()->default_value(false), "if this option is provided (ignored if --test-model is not set), then only the new embedding is printed, without the objective value")
    ("perm-iter", po::value<int>()->value_name("NUM")->default_value(INT_MAX, "INT_MAX"), "After every NUM iterations, permute the ordering of data points for fast mini-batching")
    ("cache-iter", po::value<int>()->value_name("NUM")->default_value(INT_MAX, "INT_MAX"), "After every NUM iterations, write intermediary embeddings and parameters to disk. Final embedding is always reported.")
    ("no-sgd", po::bool_switch()->default_value(false), "if set, do not use SGD acceleration; equivalent to t-SNE with an additional backpropagation step to train a neural network. Effective for small datasets")
    //("batch-norm", po::bool_switch()->default_value(false), "turn on batch normalization")
    //("monte-carlo-pos", po::bool_switch()->default_value(false), "use monte-carlo integration for positive gradient term")
    //("match-pos-neg", po::bool_switch()->default_value(false), "compute negative forces for points sampled for positive force")

  ;

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).
                options(desc).run(), vm);
  po::notify(vm);

  if (vm.count("help")) {
    cout << "Usage: RunBhtsne [options]" << endl;
    cout << desc << "\n";
    return 1;
  }

  string infile_P = vm["input-P"].as<string>();
  string infile_X = vm["input-X"].as<string>();
  string outdir = vm["out-dir"].as<string>();

  fsys::path dir(outdir);
  if (fsys::is_directory(dir)) {
    cout << "Error: Output directory (" << outdir << ") already exists" << endl;
    return 1;
  }
  if (fsys::create_directory(dir)) {
    cout << "Output directory created: " << outdir << endl;
  }

  fsys::path paramfile = dir;
  paramfile /= "param.txt";
  ofstream ofs(paramfile.string().c_str());

  NETSNE* netsne = new NETSNE();

  bool use_known_Y = vm.count("input-Y");
  string infile_Y;

  netsne->TEST_RUN = vm["test-model"].as<bool>();
  netsne->NO_TARGET = netsne->TEST_RUN && vm["no-target"].as<bool>();

  if (!netsne->NO_TARGET) {
    if (use_known_Y) {
      infile_Y = vm["input-Y"].as<string>();
      ofs << "input-Y: " << infile_Y << endl;
      cout << "Learning to match the provided embedding: " << infile_Y << endl;
    } else {
      ofs << "input-P: " << infile_P << endl;
    }
  }
  ofs << "input-X: " << infile_X << endl;
  ofs << "out-dir: " << fsys::canonical(dir).string() << endl;

  netsne->BATCH_FRAC = vm["batch-frac"].as<double>(); ofs << "batch-frac: " << netsne->BATCH_FRAC << endl;
  netsne->N_SAMPLE_LOCAL = vm["num-local-sample"].as<int>(); ofs << "num-local-sample: " << netsne->N_SAMPLE_LOCAL << endl;
  netsne->MIN_SAMPLE_Z = vm["min-sample-Z"].as<double>(); ofs << "min-sample-Z: " << netsne->MIN_SAMPLE_Z << endl;
  netsne->STOP_LYING = vm["early-exag-iter"].as<int>(); ofs << "early-exag-iter: " << netsne->STOP_LYING << endl;
  netsne->STEP_METHOD = vm["step-method"].as<string>(); ofs << "step-method: " << netsne->STEP_METHOD << endl;
  netsne->LEARN_RATE = vm["learn-rate"].as<double>(); ofs << "learn-rate: " <<  netsne->LEARN_RATE << endl;
  netsne->L2_REG = vm["l2-reg"].as<double>(); ofs << "l2-reg: " <<  netsne->L2_REG << endl;
  netsne->SGD_FLAG = !vm["no-sgd"].as<bool>(); ofs << "sgd: " << netsne->SGD_FLAG << endl;
  netsne->PERM_ITER = vm["perm-iter"].as<int>(); ofs << "perm-iter: " << netsne->PERM_ITER << endl;
  netsne->CACHE_ITER = vm["cache-iter"].as<int>(); ofs << "cache-iter: " << netsne->CACHE_ITER << endl;
  //netsne->BATCH_NORM = vm["batch-norm"].as<bool>(); ofs << "batch-norm: " << netsne->BATCH_NORM << endl;
  //netsne->MONTE_CARLO_POS = vm["monte-carlo-pos"].as<bool>(); ofs << "monte-carlo-pos: " << netsne->MONTE_CARLO_POS << endl;
  //netsne->MATCH_POS_NEG = vm["match-pos-neg"].as<bool>(); ofs << "match-pos-neg: " << netsne->MATCH_POS_NEG << endl;

  netsne->MODEL_PREFIX_FLAG = vm.count("init-model-prefix");
  if (netsne->MODEL_PREFIX_FLAG) {
    netsne->MODEL_PREFIX = vm["init-model-prefix"].as<string>();
  }
  ofs << "init-model-prefix: " << netsne->MODEL_PREFIX << endl;

  if (netsne->TEST_RUN) {
    netsne->COMPUTE_INIT = true;

    if (!netsne->MODEL_PREFIX_FLAG) {
      cout << "Error: if --test-model is set, then --init-model-prefix must be provided; see --help" << endl;
      return 1;
    }
  } else {
    netsne->COMPUTE_INIT = vm["init-map"].as<bool>();
  }
  ofs << "init-map: " << netsne->COMPUTE_INIT << endl;

  if (!netsne->MODEL_PREFIX_FLAG) {
    netsne->NUM_LAYERS = vm["num-layers"].as<int>(); ofs << "num-layers: " << netsne->NUM_LAYERS << endl;
    netsne->NUM_UNITS = vm["num-units"].as<int>(); ofs << "num-units: " << netsne->NUM_UNITS << endl;
    netsne->ACT_FN = vm["act-fn"].as<string>(); ofs << "act-fn: " << netsne->ACT_FN << endl;
  }

  if (netsne->STEP_METHOD == "mom" || netsne->STEP_METHOD == "mom_gain") {
    netsne->MOM_SWITCH_ITER = vm["mom-switch-iter"].as<int>(); ofs << "mom-switch-iter: " << netsne->MOM_SWITCH_ITER << endl;
    netsne->MOM_INIT = vm["mom-init"].as<double>(); ofs << "mom-init: " << netsne->MOM_INIT << endl;
    netsne->MOM_FINAL = vm["mom-final"].as<double>(); ofs << "mom-final: " << netsne->MOM_FINAL << endl;
  }

  int no_dims = vm["out-dim"].as<int>(); ofs << "out-dim: " << no_dims << endl;
  double theta = vm["theta"].as<double>(); ofs << "theta: " << theta << endl;
  int rand_seed = vm["rand-seed"].as<int>(); ofs << "rand-seed: " << rand_seed << endl;
  int max_iter = vm["max-iter"].as<int>(); ofs << "max-iter: " << max_iter << endl;

  ofs.close();

  if (vm.count("help")) {
    cout << "Usage: RunBhtsne [options]" << endl;
    cout << desc << "\n";
    return 1;
  }

  if (netsne->STEP_METHOD != "adam" && netsne->STEP_METHOD != "mom" && netsne->STEP_METHOD != "mom_gain"
      && netsne->STEP_METHOD != "fixed") {
    cout << "Error: Unrecognized --step-method argument " << netsne->STEP_METHOD << "; see --help" << endl;
    return 1;
  }

  if (netsne->ACT_FN != "sigmoid" && netsne->ACT_FN != "relu") {
    cout << "Error: Unrecognized --act-fn argument " << netsne->ACT_FN << "; see --help" << endl;
    return 1;
  }

  int num_input_feat;
  if (vm.count("num-input-feat")) {
    num_input_feat = vm["num-input-feat"].as<int>();
  } else {
    num_input_feat = INT_MAX;
  }

  cout << "Loading input features ... ";
  mat X;
  int num_instances;
  int num_features;
	if (!load_data(infile_X, X, num_instances, num_features)) {
    return 1;
  }
  cout << endl;

  if (X.n_rows > num_input_feat) {
    cout << "Truncating to top " << num_input_feat << " features" << endl;
    X = X.head_rows(num_input_feat);
  }

  cout << "Data feature matrix is " << X.n_rows << " by " << X.n_cols << endl;

  int N;
  unsigned int *row_P = NULL;
  unsigned int *col_P = NULL;
  double *val_P = NULL;
  mat target_Y;

  if (netsne->NO_TARGET) {

    cout << "No target provided" << endl;
    N = X.n_cols;

  } else if (use_known_Y) {

    cout << "Loading target Y ... ";
    target_Y.load(infile_Y, arma_ascii);
    target_Y = target_Y.t();
    cout << "done" << endl;

    N = target_Y.n_cols;

    if (N != X.n_cols) {
      cout << "Error: Y matrix dimensions (" << N << ") do not match with X matrix (" << X.n_cols << ")" << endl;
      return 1;
    }

  } else {

    cout << "Loading input similarities ... ";
    if (!netsne->load_P(infile_P, N, &row_P, &col_P, &val_P)) {
      cout << "Error: failed to load P from " << infile_P << endl;
      return 1;
    }
    cout << "done" << endl;

    if (N != X.n_cols) {
      cout << "Error: P matrix dimensions (" << N << ") do not match with X matrix (" << X.n_cols << ")" << endl;
      return 1;
    }

  }

  mat Y(no_dims, N);

  if (!netsne->run(N, row_P, col_P, val_P, target_Y, X, Y, no_dims, theta, rand_seed,
           max_iter, dir)) {
    return 1;
  }

  free(row_P);
  free(col_P);
  free(val_P);

  delete(netsne);

  cout << "Done" << endl;
}