#include "ceres/rotation.h"
#include "ceres/jet.h"
#include <Eigen/Dense>
#include <TooN/TooN.h>
#include <cmath>
#include <libv/lma/color/console.hpp>
#include <libv/lma/string/string_utiliy.hpp>
#include <libv/lma/time/tictoc.hpp>
#include <libv/lma/numeric/ad/rt/ad.hpp>
#include <libv/lma/numeric/ad/ct/adct.hpp>
#include <typeinfo>

  typedef Eigen::Matrix<double,9,1> Camera;
  typedef Eigen::Matrix<double,3,1> Point3d;
  typedef Eigen::Matrix<double,2,1> Point2d;

  template<class T>
  void analytical_derivative(const Camera& camera_, const Point3d& point_, const Eigen::Vector2d& obs, Eigen::Matrix<double,2,12>& j)
  {
    const T camera[3] = {T(camera_[0],0),T(camera_[1],1),T(camera_[2],2)};
    const T point[3] = {T(point_[0],9),T(point_[1],10),T(point_[2],11)};
    T p[3];
    ceres::AngleAxisRotatePoint(camera, point, p);
    p[0] += T(camera_[3],3);
    p[1] += T(camera_[4],4);
    p[2] += T(camera_[5],5);
    auto xp = - p[0] / p[2];
    auto yp = - p[1] / p[2];
    const T l1(camera_[7],7);
    const T l2(camera_[8],8);
    auto r2 = xp*xp + yp*yp;
    auto distortion = T(1.0) + r2  * (l1 + l2  * r2);
    T focal(camera_[6],6);
    auto jacobx = focal * distortion * xp - T(obs.x());
    auto jacoby = focal * distortion * yp - T(obs.y());
    for(int i = 0 ; i < 12 ; ++i)
    {
      j(0,i) = jacobx.v[i];
      j(1,i) = jacoby.v[i];
    }
  }

  template<class T>
  void analytical_derivative2(const Camera& camera_, const Point3d& point_, const Point2d& obs, TooN::Matrix<2,12,double>& j)
  {
    typedef T Scalar;
    const std::array<T,9> c = {
      T(camera_[0],0),T(camera_[1],1),T(camera_[2],2),
      T(camera_[3],3),T(camera_[4],4),T(camera_[5],5),
      T(camera_[6],6),T(camera_[7],7),T(camera_[8],8)};

    const std::array<T,3> pt = {T(point_[0],9),T(point_[1],10),T(point_[2],11)};

    std::array<T,2> p;

    const T theta2 = c[0]*c[0] + c[1]*c[1] + c[2]*c[2];
    if (theta2 > Scalar(std::numeric_limits<double>::epsilon()))
    {
      const T
        theta = sqrt(theta2),
        costheta = cos(theta),
        sintheta = sin(theta),
        theta_inverse = 1.0 / theta,
        w[3] = { c[0] * theta_inverse, c[1] * theta_inverse, c[2] * theta_inverse },
        tmp = (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (Scalar(1.0) - costheta),
        p2 =    pt[2] * costheta + (w[0] * pt[1] - w[1] * pt[0]) * sintheta + w[2] * tmp + c[5];
      p[0] = - (pt[0] * costheta + (w[1] * pt[2] - w[2] * pt[1]) * sintheta + w[0] * tmp + c[3]) / p2;
      p[1] = - (pt[1] * costheta + (w[2] * pt[0] - w[0] * pt[2]) * sintheta + w[1] * tmp + c[4]) / p2;
    }
    else
    {
      const T p2 = pt[2] + c[0] * pt[1] - c[1] * pt[0] + c[5];
      p[0] = - (pt[0] + c[1] * pt[2] - c[2] * pt[1] + c[3]) / p2;
      p[1] = - (pt[1] + c[2] * pt[0] - c[0] * pt[2] + c[4]) / p2;
    }

    const T
      r2 = p[0]*p[0] + p[1]*p[1],
      fx = c[6] * (Scalar(1.0) + r2  * (c[7] + c[8]  * r2));

    const typename T::Array
      jacobx = (fx * p[0] - Scalar(obs.x())).infinite(),
      jacoby = (fx * p[1] - Scalar(obs.y())).infinite();

    for(int i = 0 ; i < 12 ; ++i)
    {
      j(0,i) = jacobx[i];
      j(1,i) = jacoby[i];
    }
  }

  template<class T> auto f(const T& a, const T& b, const T& c)
  {
    return - a * 2.5 * b * a * c * a - 2.0 * a * b * a * b / 10.0 + a /2.0 + a * sqrt(b * c * cos( sin( b * b + c) / a * c / 5.0));
  }

  inline void test_adrt(double x, double y, double z, double d[3], double& r)
  {
    AdRt::Ad<double,3> a(x,0),b(y,1),c(z,2);
    AdRt::Ad<double,3> derivatives = f(a,b,c);
    d[0] = derivatives.infinite[0]; d[1] = derivatives.infinite[1]; d[2] = derivatives.infinite[2];
    r = derivatives.value;
  }

  inline void test_adct(double x, double y, double z, double d[3], double& r)
  {
    adct::Ad<double,3> a(x,0),b(y,1),c(z,2);
    auto expr = f(a,b,c);
    auto derivatives = expr.infinite();
    d[0] = derivatives[0];d[1] = derivatives[1];d[2] = derivatives[2];
    r = expr.value();
  }

  inline void test_jet(double x, double y, double z, double d[3], double& r)
  {
    ceres::Jet<double,3> a(x,0),b(y,1),c(z,2);
    auto derivatives = f(a,b,c);
    d[0] = derivatives.v[0];d[1] = derivatives.v[1];d[2] = derivatives.v[2];
    r = derivatives.a;
  }

int main()
{
  std::cout << std::endl << std::endl;
  double d[3] = {0,0,0};
  double r = 0;

  size_t N = 1000000;

  utils::Tic<true> ticAdrt("Adrt");
  for(size_t i = 0 ; i < N; ++i)
    test_adrt(i,i/2,i/3,d,r);
  ticAdrt.disp();

  std::cout << color.red() << " adrt dx,dy,dz = " << d[0] << "," << d[1] << "," << d[2] << "," << r << color.reset() << std::endl;
  d[0] = 0; d[1] = 0, d[2] = 0;
  r = 0;

  utils::Tic<true> ticAd("AdCt");
  for(size_t i = 0 ; i < N; ++i)
    test_adct(i,i/2,i/3,d,r);
  ticAd.disp();

  std::cout << color.red() << " adct dx,dy,dz = " << d[0] << "," << d[1] << "," << d[2] << "," << r << color.reset() << std::endl;

  d[0] = 0; d[1] = 0, d[2] = 0;
  r = 0;
  utils::Tic<true> ticjet("jet");
  for(size_t i = 0 ; i < N; ++i)
    test_jet(i,i/2,i/3,d,r);
  ticjet.disp();

  std::cout << color.red() << " jet  dx,dy,dz = " << d[0] << "," << d[1] << "," << d[2] << "," << r <<  color.reset() << std::endl;

  Camera cam; cam << 0,0,0,10,10,10,10,10,10;
  Point3d pt; pt << 100,200,150;
  Point2d obs; obs << 100,100;

  {
    Eigen::Matrix<double,2,12> jacob;
    utils::Tic<true> jet_proj("Jet Proj");
    for(size_t i = 0 ; i < N; ++i)
      analytical_derivative<ceres::Jet<double,12>>(cam,pt,obs,jacob);
    jet_proj.disp();
    std::cout << jacob << std::endl;
  }

  {
    TooN::Matrix<2,12,double> jacob;
    utils::Tic<true> ad_proj("Ad Proj");
    for(size_t i = 0 ; i < N; ++i)
      analytical_derivative2<adct::Ad<double,12>>(cam,pt,obs,jacob);
    ad_proj.disp();
    std::cout << jacob << std::endl;
  }
//
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2021 Alec Jacobson <alecjacobson@gmail.com>
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "per_corner_normals.h"
#include "vertex_triangle_adjacency.h"
#include "per_face_normals.h"
#include "PI.h"
#include "parallel_for.h"
#include "doublearea.h"
#include <Eigen/Geometry>

template <
  typename DerivedV,
  typename DerivedF,
  typename DerivedCN>
IGL_INLINE void igl::per_corner_normals(
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedF> & F,
  const typename DerivedV::Scalar corner_threshold_degrees,
  Eigen::PlainObjectBase<DerivedCN> & CN)
{
  Eigen::Matrix<Eigen::Index,Eigen::Dynamic,1> VF,NK;
  vertex_triangle_adjacency(F,V.rows(),VF,NK);
  return per_corner_normals(V,F,corner_threshold_degrees,VF,NK,CN);
}

template <
  typename DerivedV,
  typename DerivedF,
  typename DerivedVF,
  typename DerivedNI,
  typename DerivedCN>
IGL_INLINE void igl::per_corner_normals(
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedF> & F,
  const typename DerivedV::Scalar corner_threshold_degrees,
  const Eigen::MatrixBase<DerivedVF> & VF,
  const Eigen::MatrixBase<DerivedNI> & NI,
  Eigen::PlainObjectBase<DerivedCN> & CN)
{
  typedef typename DerivedV::Scalar Scalar;
  typedef Eigen::Index Index;
  // unit normals
  Eigen::Matrix<Scalar,Eigen::Dynamic,3,Eigen::RowMajor> FN(F.rows(),3);
  // face areas
  Eigen::Matrix<Scalar,Eigen::Dynamic,1> FA(F.rows());
  igl::parallel_for(F.rows(),[&](const Index f)
  {
    const Eigen::Matrix<Scalar,1,3> v10 = V.row(F(f,1))-V.row(F(f,0));
    const Eigen::Matrix<Scalar,1,3> v20 = V.row(F(f,2))-V.row(F(f,0));
    const Eigen::Matrix<Scalar,1,3> n = v10.cross(v20);
    const Scalar a = n.norm();
    FA(f) = a;
    FN.row(f) = n/a;
  },10000);

  // number of faces
  const Index m = F.rows();
  // valence of faces
  const Index n = F.cols();
  assert(n == 3);

  // initialize output to ***zero***
  CN.setZero(m*n,3);

  const Scalar cos_thresh = cos(corner_threshold_degrees*igl::PI/180);
  // loop over faces
  //for(Index i = 0;i<m;i++)
  igl::parallel_for(F.rows(),[&](const Index i)
  {
    // Normal of this face
    const auto & fnhat = FN.row(i);
    // loop over corners
    for(Index j = 0;j<n;j++)
    {
      const auto & v = F(i,j);
      for(int k = NI[v]; k<NI[v+1]; k++)
      {
        const auto & ifn = FN.row(VF[k]);
        // dot product between face's normal and other face's normal
        const Scalar dp = fnhat.dot(ifn);
        // if difference in normal is slight then add to average
        if(dp > cos_thresh)
        {
          // add to running sum
          CN.row(i*n+j) += ifn*FA(VF[k]);
        }
      }
      // normalize to take average
      CN.row(i*n+j).normalize();
    }
  },10000);
}

template <
  typename DerivedV,
  typename DerivedF,
  typename DerivedCI,
  typename DerivedCC,
  typename DerivedCN>
IGL_INLINE void igl::per_corner_normals(
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedF> & F,
  const Eigen::MatrixBase<DerivedCI> & CI,
  const Eigen::MatrixBase<DerivedCC> & CC,
  Eigen::PlainObjectBase<DerivedCN> & CN)
{
  typedef typename DerivedV::Scalar Scalar;
  typedef Eigen::Index Index;
  assert(CC.rows() == F.rows()*3+1);
  // area weighted normals
  Eigen::Matrix<Scalar,Eigen::Dynamic,3,Eigen::RowMajor> FN(F.rows(),3);
  //for(Index f = 0;f<F.rows();f++)
  igl::parallel_for(F.rows(),[&](const Index f)
  {
    const Eigen::Matrix<Scalar,1,3> v10 = V.row(F(f,1))-V.row(F(f,0));
    const Eigen::Matrix<Scalar,1,3> v20 = V.row(F(f,2))-V.row(F(f,0));
    FN.row(f) = v10.cross(v20);
  },10000);

  // number of faces
  const Index m = F.rows();
  // valence of faces
  const Index n = F.cols();
  assert(n == 3);

  // initialize output to ***zero***
  CN.setZero(m*n,3);
  // loop over faces
  igl::parallel_for(m*n,[&](const Index ci)
  {
    for(int k = CC(ci); k<CC(ci+1); k++)
    {
      // add to running sum
      const auto cfk = CI(k);
      CN.row(ci) += FN.row(cfk);
    }
    // normalize to take average
    CN.row(ci).normalize();
  },10000);
}

template <typename DerivedNV, typename DerivedNF, typename DerivedCN>
IGL_INLINE void igl::per_corner_normals(
  const Eigen::MatrixBase<DerivedNV> & NV,
  const Eigen::MatrixBase<DerivedNF> & NF,
  Eigen::PlainObjectBase<DerivedCN> & CN)
{
  const auto m = NF.rows();
  const auto nc = NF.cols();
  CN.resize(m*nc,3);
  for(Eigen::Index i = 0;i<m;i++)
  {
    for(Eigen::Index c = 0;c<nc;c++)
    {
      CN.row(i*nc+c) = NV.row(NF(i,c));
    }
  }
}

template <
  typename DerivedV,
  typename DerivedI,
  typename DerivedC,
  typename DerivedN,
  typename DerivedVV,
  typename DerivedFF,
  typename DerivedJ,
  typename DerivedNN>
IGL_INLINE void igl::per_corner_normals(
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedI> & I,
  const Eigen::MatrixBase<DerivedC> & C,
  const typename DerivedV::Scalar corner_threshold_degrees,
  Eigen::PlainObjectBase<DerivedN>  & N,
  Eigen::PlainObjectBase<DerivedVV> & VV,
  Eigen::PlainObjectBase<DerivedFF> & FF,
  Eigen::PlainObjectBase<DerivedJ>  & J,
  Eigen::PlainObjectBase<DerivedNN> & NN)
{
  const Eigen::Index m = C.size()-1;
  typedef Eigen::Index Index;
  Eigen::MatrixXd FN;
  per_face_normals(V,I,C,FN,VV,FF,J);
  typedef typename DerivedN::Scalar Scalar;
  Eigen::Matrix<Scalar,Eigen::Dynamic,1> AA;
  doublearea(VV,FF,AA);
  // VF[i](j) = p means p is the jth face incident on vertex i
  // to-do micro-optimization to avoid vector<vector>
  std::vector<std::vector<Eigen::Index>> VF(V.rows());
  for(Eigen::Index p = 0;p<m;p++)
  {
    // number of faces/vertices in this simple polygon
    const Index np = C(p+1)-C(p);
    for(Eigen::Index i = 0;i<np;i++)
    {
      VF[I(C(p)+i)].push_back(p);
    }
  }

  N.resize(I.rows(),3);
  for(Eigen::Index p = 0;p<m;p++)
  {
    // number of faces/vertices in this simple polygon
    const Index np = C(p+1)-C(p);
    Eigen::Matrix<Scalar,3,1> fn = FN.row(p);
    for(Eigen::Index i = 0;i<np;i++)
    {
      N.row(C(p)+i).setZero();
      // Loop over faces sharing this vertex
      for(const auto & n : VF[I(C(p)+i)])
      {
        Eigen::Matrix<Scalar,3,1> ifn = FN.row(n);
        // dot product between face's normal and other face's normal
        Scalar dp = fn.dot(ifn);
        if(dp > cos(corner_threshold_degrees*igl::PI/180))
        {
          // add to running sum
          N.row(C(p)+i) += AA(n) * ifn;
        }
      }
      N.row(C(p)+i).normalize();
    }
  }

  // Relies on order of FF
  NN.resize(FF.rows()*3,3);
  {
    Eigen::Index k = 0;
    for(Eigen::Index p = 0;p<m;p++)
    {
      // number of faces/vertices in this simple polygon
      const Index np = C(p+1)-C(p);
      for(Eigen::Index i = 0;i<np;i++)
      {
        assert(FF(k,0) == I(C(p)+((i+0)%np)));
        assert(FF(k,1) == I(C(p)+((i+1)%np)));
        assert(FF(k,2) == V.rows()+p);
        NN.row(k*3+0) = N.row(C(p)+((i+0)%np));
        NN.row(k*3+1) = N.row(C(p)+((i+1)%np));
        NN.row(k*3+2) = FN.row(p);
        k++;
      }
    }
    assert(k == FF.rows());
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::per_corner_normals<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::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::per_corner_normals<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<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::per_corner_normals<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<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar, Eigen::MatrixBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::per_corner_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::per_corner_normals<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<double, -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<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -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> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
// Clang thinks this is the same as the one below, but Windows doesn't?
// template void igl::per_corner_normals<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::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::per_corner_normals<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::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, double, Eigen::PlainObjectBase< Eigen::Matrix<double, -1, -1, 0, -1, -1> > &);
#endif


#pragma once

#include <algorithm>
#include <boost/multi_array.hpp>
#include <cmath>
#include <iostream>
#include <vector>

// ------------------------------------------------------------
#include "aux/hash_specializations.hpp"
#include "laguerren_impl.hpp"

namespace boltzmann {

template <typename NUMERIC>
class LaguerreNKS
{
 public:
  typedef NUMERIC numeric_t;

 public:
  LaguerreNKS(int K)
      : Y_(K + 1)
      , K_(K)
      , is_initialized_(false)
  { }

  void compute(const std::vector<numeric_t>& x);

  /**
   *
   * @param x evaluation points
   * @param n number of evaluation points
   * @param expw evaluation weight: e^(-r^2*expw)
   */
  void compute(const numeric_t* x, unsigned int n, numeric_t expw = numeric_t(0));

  unsigned int get_npoints() const { return Y_[0].shape()[1]; }

 public:
  typedef boost::multi_array<numeric_t, 2> array_t;

 public:
  const NUMERIC* get(unsigned int k, unsigned int alpha) const;
  void info() const;

 private:
  std::vector<array_t> Y_;
  unsigned int K_;
  bool is_initialized_;
};

// ----------------------------------------------------------------------
template <typename NUMERIC>
void
LaguerreNKS<NUMERIC>::compute(const std::vector<numeric_t>& x)
{
  compute(x.data(), x.size());
}

// ----------------------------------------------------------------------
template <typename NUMERIC>
void
LaguerreNKS<NUMERIC>::compute(const numeric_t* x, unsigned int n, numeric_t expw)
{
  std::vector<numeric_t> x2(n);
  std::transform(x, x + n, x2.begin(), [](const numeric_t& v) { return v * v; });
  // L_n-1
  std::vector<numeric_t> Lnm1(n);
  // L_n-2
  std::vector<numeric_t> Lnm2(n);

  for (unsigned int alpha = 0; alpha <= K_; ++alpha) {
    Y_[alpha].resize(boost::extents[K_ / 2 + 1][n]);
// init
#pragma omp parallel for
    for (size_t xi = 0; xi < n; ++xi) {
      numeric_t fexp = 1;
      if (::math::abs(expw) > 1e-16) fexp = ::math::exp(-expw * x2[xi]);
      Y_[alpha][0][xi] = boost::math::laguerren(0, alpha, x2[xi]) * fexp;
      Y_[alpha][1][xi] = boost::math::laguerren(1, alpha, x2[xi]) * fexp;
    }

    for (unsigned int kp = 2; kp <= K_ / 2; ++kp) {
#pragma omp parallel for
      for (size_t xi = 0; xi < n; ++xi) {
        Y_[alpha][kp][xi] = boost::math::laguerren_next(
            kp - 1, alpha, x2[xi], Y_[alpha][kp - 1][xi], Y_[alpha][kp - 2][xi]);
      }
    }
  }

  boost::multi_array<numeric_t, 2> powers_of_x(boost::extents[K_ + 1][n]);

  for (unsigned int xi = 0; xi < n; ++xi) {
    powers_of_x[0][xi] = 1.0;
  }

  for (unsigned int alpha = 1; alpha <= K_; ++alpha) {
    for (unsigned int xi = 0; xi < n; ++xi) {
      powers_of_x[alpha][xi] = x[xi] * powers_of_x[alpha - 1][xi];
    }
  }

  // add factor x^(2j+k%2)
  for (unsigned int alpha = 0; alpha <= K_; ++alpha) {
    for (unsigned int kp = 0; kp <= K_ / 2; ++kp) {
      int j = alpha / 2;
      int k = 2 * kp + 2 * j + alpha % 2;
      for (size_t xi = 0; xi < n; ++xi) {
        Y_[alpha][kp][xi] *= powers_of_x[2 * j + k % 2][xi];
      }
    }
  }

  is_initialized_ = true;
}

// ----------------------------------------------------------------------
template <typename NUMERIC>
const NUMERIC*
LaguerreNKS<NUMERIC>::get(unsigned int k, unsigned int alpha) const
{
  assert(is_initialized_);
  assert(alpha < Y_.size());
  assert(k < Y_[alpha].shape()[0]);
  return Y_[alpha][k].origin();
}

// ----------------------------------------------------------------------
template <typename NUMERIC>
void
LaguerreNKS<NUMERIC>::info() const
{
  unsigned long long int nentries = 0;
  for (unsigned int alpha = 0; alpha < Y_.size(); ++alpha) {
    nentries += Y_[alpha].shape()[0] * Y_[alpha].shape()[1];
  }

  std::cout << " LaguerreN uses " << nentries * sizeof(NUMERIC) / 1e6 << " MB" << std::endl;
}

}  // end namespace boltzmann


/*
 * Driver for ODE integration
 */

#include <iostream>
#include <vector>
#include <cmath>
#include <cassert>
#include <boost/numeric/odeint.hpp>

#include "fast_detector_model.hpp"

/* The type of container used to hold the state vector */
typedef std::vector< double > state_type;

/* the type returned by std::vector.size() */
typedef std::vector< double >::size_type T_vecsize;

/* copy a carray into a vector */
std::vector< double >
carray_to_vector(const double *data, int N)
{
    std::vector< double > vec;
    vec.assign(data, data+N);
    return vec;
}

/* TODO: doc */
struct linear_interpolator {
    double m_timestep;
    std::vector< double > m_interpolation_values;
    int m_mode;
    T_vecsize N;

    linear_interpolator( double timestep, std::vector< double > intvals, int mode) :
                                    m_timestep(timestep),
                                    m_interpolation_values(intvals),
                                    m_mode(mode)
    {
        N = intvals.size();
        /*
        std::cout << "linear interpolator init\n";
        for(int ii=0; ii<10; ii++)
        {
            std::cout << ii << '\t' << intvals[ii] << std::endl;
        }
        */

    }

    double operator() ( const double t )
    {
        double p, i1, i2, w1, w2, r;
        T_vecsize ii1, ii2;
        //linear interpolation
        p = t/m_timestep;
        if (p <= 0)
        {
            r = m_interpolation_values[0];
        }
        else if (p >= N-1)
        {
            r = m_interpolation_values[N-1];
        }
        else
        {
            /* piecewise-constant - return Y0 for t < 0 between t0 and t1
                                           Y1 for t0 <= t < t1 , etc*/
            if(m_mode == 0)
            {
                i1 = floor(p);
                ii1 = (T_vecsize)i1 + 1;
                r = m_interpolation_values[ii1];
            }
            /* linear interpolation */
            else if (m_mode == 1)
            {
                i1 = floor(p);
                i2 = ceil(p);
                w2 = (p-i1);
                w1 = 1.0-w2;
                ii1 = (T_vecsize)i1;
                ii2 = (T_vecsize)i2;
                /*
                std::cout << "p:" << p << " t:" << t << "i1:" << i1 << " i2:" << i2 << std::endl;
                std::cout << w1 << " " << w2 << std::endl;
                std::cout << m_interpolation_values[ii1] << " " << m_interpolation_values[ii2] << std::endl;
                */
                //std::cout << ii1 << " " << ii2 << std::endl;
                r = w1*m_interpolation_values[ii1] + w2*m_interpolation_values[ii2];
            }
            /* invalid mode TODO: how to flag the error? */
            else
            {
                assert(false);
                r = INFINITY;
            }
        }
        return r;
    }
};

// calculate Na, Nb, Nc from generated code (copied from generated.c)
void calc_NaNbNc(double t, double Nrn, double lamp,
                 double &Na, double &Nb, double &Nc)
{
    using namespace std; //for math functions pow, exp
    Na = Nrn*lamrn/(lama + lamp) - Nrn*lamrn*exp(-t*(lama + lamp))/(lama +
            lamp);
    Nb = Nrn*lama*lamrn/(lama*lamb + pow(lamp, 2) + lamp*(lama + lamb)) -
            Nrn*lama*lamrn*exp(-t*(lamb + lamp))/(lama*lamb - pow(lamb, 2) +
            lamp*(lama - lamb)) + Nrn*lama*lamrn*exp(-t*(lama + lamp))/
            (pow(lama, 2) - lama*lamb + lamp*(lama - lamb));
    Nc = Nrn*lama*lamb*lamrn/(lama*lamb*lamc + pow(lamp, 3) + pow(lamp, 2)*
            (lama + lamb + lamc) + lamp*(lama*lamb + lamc*(lama + lamb))) -
            Nrn*lama*lamb*lamrn*exp(-t*(lamc + lamp))/(lama*lamb*lamc +
            pow(lamc, 3) - pow(lamc, 2)*(lama + lamb) + lamp*(lama*lamb +
            pow(lamc, 2) - lamc*(lama + lamb))) + Nrn*lama*lamb*lamrn*
            exp(-t*(lamb + lamp))/(lama*pow(lamb, 2) - pow(lamb, 3) -
            lamc*(lama*lamb - pow(lamb, 2)) + lamp*(lama*lamb - pow(lamb, 2)
            - lamc*(lama - lamb))) - Nrn*lama*lamb*lamrn*exp(-t*(lama + lamp))
            /(pow(lama, 3) - pow(lama, 2)*lamb - lamc*(pow(lama, 2) - lama*lamb)
             + lamp*(pow(lama, 2) - lama*lamb - lamc*(lama - lamb)));
    return;
}

// Compute the steady-state solution for the state variable Y
// returns Yss : state_type( [Nrnd, Nrn, Fa, Fb, Fc] )

state_type calc_steady_state(double Nrn, double Q, double rs, double lamp,
                      double V_tank, double recoil_prob, double eff)
{
    double tt, Na, Nb, Nc, Fa, Fb, Fc;
    state_type Yss;
    double Acc_counts;
    // transit time assuming plug flow in the tank
    tt = V_tank / Q;
    calc_NaNbNc(tt, Nrn, lamp, Na, Nb, Nc);

    // expressions based on these lines from detector_state_rate_of_change
    // dFadt = Q*rs*Na - Fa*lama
    // dFbdt = Q*rs*Nb - Fb*lamb + Fa*lama * (1.0-recoil_prob)
    // dFcdt = Q*rs*Nc - Fc*lamc + Fb*lamb
    Fa = Na*Q*rs/lama;
    Fb = (Q*rs*Nb + Fa*lama * (1.0-recoil_prob)) / lamb;
    Fc = (Q*rs*Nc + Fb*lamb) / lamc;

    // accumulated counts, in this case over one second
    Acc_counts = eff*(Fa*lama + Fc*lamc);
    // pack output into vector
    Yss.push_back(Nrn);
    Yss.push_back(Nrn);
    Yss.push_back(Nrn);
    Yss.push_back(Fa);
    Yss.push_back(Fb);
    Yss.push_back(Fc);
    Yss.push_back(Acc_counts);
    return Yss;
}

//[ rhs_class
/* The rhs of x' = f(x) defined as a class */
struct two_filter_detector {

    linear_interpolator m_bc;
    linear_interpolator m_airt;
    // parameters
    double Q;
    double rs;
    double lamp;
    double eff;
    double Q_external;
    double V_delay;
    double V_delay_2;
    double V_tank;
    double t_delay;
    double recoil_prob;
    double cal_source_strength;
    double cal_begin;
    double cal_duration;
    double inj_source_strength;
    double inj_begin;
    double inj_duration;

    two_filter_detector( linear_interpolator bc,
                         linear_interpolator airt,
                         double * parameters ) : m_bc(bc), m_airt(airt)
    {
        // store parameters
        Q = *parameters++;
        rs = *parameters++;
        lamp = *parameters++;
        eff = *parameters++;
        Q_external = *parameters++;
        V_delay = *parameters++;
        V_delay_2 = *parameters++;
        V_tank = *parameters++;
        t_delay = *parameters++;
        recoil_prob = *parameters++;
        cal_source_strength = *parameters++;
        cal_begin = *parameters++;
        cal_duration = *parameters++;
        inj_source_strength = *parameters++;
        inj_begin = *parameters++;
        inj_duration = *parameters++;

        /*
        std::cout << "Q: " << Q << std::endl
                  << "rs: " << rs << std::endl
                  << "lamp: " << lamp << std::endl
                  << "eff: " << eff << std::endl
                  << "Q_external: " << Q_external << std::endl
                  << "V_delay: " << V_delay << std::endl
                  << "V_delay_2: " << V_delay_2 << std::endl
                  << "V_tank: " << V_tank << std::endl
                  << "t_delay: " << t_delay << std::endl
                  << "recoil_prob: " << recoil_prob << std::endl;
        */
        /*
        std::cout << "t/60\tT\tdTdt\tRn\n";
        for (double t=-60; t<600; t+=60)
        {
            const double delt = 60;
            const double T = m_airt(t);
            double dTdt = (T - m_airt(t-delt)) / delt;
            std::cout << t/60.0 << '\t' << T << '\t' << dTdt << '\t'
                      << m_bc(t)
                      << std::endl;
        }
        */
    }

    void operator() ( const state_type &x , state_type &dxdt , const double t )
    {
        double Nrnd, Nrnd2, Nrn, Fa, Fb, Fc;
        double Nrn_inj; //radon concentration from source injected at inlet
        double Nrn_cal; //radon concentration from calibration source
        double dNrnddt, dNrnd2dt, dNrndt, dFadt, dFbdt, dFcdt, dAcc_countsdt;
        double tt, Na, Nb, Nc;
        // boundary condition
        double Nrn_ext = m_bc(t - t_delay);
        // temperature and temperature change
        double T = m_airt(t - t_delay);
        // for computing temerature rate of change by finite-difference
        const double delt = 60;
        double dTdt = (T - m_airt(t-t_delay-delt)) / delt;
        // copied from theoretical_model.py:detector_state_rate_of_change...
        // unpack state vector
        Nrnd = x[IDX_Nrnd];
        Nrnd2 = x[IDX_Nrnd2];
        Nrn = x[IDX_Nrn];
        Fa = x[IDX_Fa];
        Fb = x[IDX_Fb];
        Fc = x[IDX_Fc];
        // The radon concentration flowing into the inlet needs to be
        // bumped up if the injection source is active
        bool inj_is_active = inj_source_strength > 0.0
                                && (t - t_delay) > inj_begin
                                && (t - t_delay) <= inj_begin+inj_duration;
        if(inj_is_active)
        {
            Nrn_inj = inj_source_strength / Q_external;
        }
        else
        {
            Nrn_inj = 0;
        }

        // The radon concentration flowing into the main tank needs to be
        // bumped up if the calibration source is active
        bool cal_is_active = cal_source_strength > 0.0
                                && (t - t_delay) > cal_begin
                                && (t - t_delay) <= cal_begin+cal_duration;
        if(cal_is_active)
        {
            Nrn_cal = cal_source_strength / Q_external;
        }
        else
        {
            Nrn_cal = 0;
        }

        // make sure that we can't have V_delay_2 > 0 when V_delay == 0
        if (V_delay == 0 && V_delay_2 > 0)
        {
          V_delay = V_delay_2;
          V_delay_2 = 0;
        }

        // effect of delay and tank volumes (allow V_delay to be zero)
        if (V_delay == 0.0) // no delay tanks
        {
            dNrndt = Q_external / V_tank * (Nrn_ext + Nrn_cal + Nrn_inj - Nrn)
                     - Nrn*lamrn;
            // Nrnd,Nrnd2 become unimportant, but we need to do something with them
            // so just apply the same equation as for Nrn
            dNrnddt = Q_external / V_tank * (Nrn_ext + Nrn_cal + Nrn_inj - Nrnd)
                      - Nrnd*lamrn;
            dNrnd2dt = Q_external / V_tank * (Nrn_ext + Nrn_cal + Nrn_inj - Nrnd2)
                                - Nrnd2*lamrn;
        }
        else if (V_delay > 0.0 && V_delay_2 == 0.0) //one delay tank
        {
          dNrnddt = Q_external / V_delay * (Nrn_ext + Nrn_inj - Nrnd)
                                                                - Nrnd*lamrn;
          dNrndt = Q_external / V_tank * (Nrnd + Nrn_cal - Nrn) - Nrn*lamrn;
          // unused, but apply same eqn as delay tank 1
          dNrnd2dt = Q_external / V_delay * (Nrn_ext + Nrn_inj - Nrnd2)
                                                               - Nrnd2*lamrn;
        }
        else // two delay tanks
        {
            dNrnddt = Q_external / V_delay * (Nrn_ext + Nrn_inj - Nrnd)
                                                                  - Nrnd*lamrn;
            dNrnd2dt = Q_external / V_delay_2 * (Nrnd - Nrnd2) - Nrnd2*lamrn;
            dNrndt = Q_external / V_tank * (Nrnd2 + Nrn_cal - Nrn) - Nrn*lamrn;
        }

        // effect of temperature changes causing the tank to 'breathe'
        dNrndt -= Nrnd * dTdt/T;
        // Na, Nb, Nc from steady-state in tank
        // transit time assuming plug flow in the tank
        tt = V_tank / Q;
        calc_NaNbNc(tt, Nrn, lamp, Na, Nb, Nc);
        // compute rate of change of each state variable
        dFadt = Q*rs*Na - Fa*lama;
        dFbdt = Q*rs*Nb - Fb*lamb + Fa*lama * (1.0-recoil_prob);
        dFcdt = Q*rs*Nc - Fc*lamc + Fb*lamb;
        dAcc_countsdt = eff*(Fa*lama + Fc*lamc);
        // pack into dxdt
        dxdt[IDX_Nrnd] = dNrnddt;
        dxdt[IDX_Nrnd2] = dNrnd2dt;
        dxdt[IDX_Nrn] = dNrndt;
        dxdt[IDX_Fa] = dFadt;
        dxdt[IDX_Fb] = dFbdt;
        dxdt[IDX_Fc] = dFcdt;
        dxdt[IDX_Acc_counts] = dAcc_countsdt;
        return;
    }
};
//]

//[ integrate_observer
class push_back_state_and_time
{
    std::vector< state_type >& m_states;
    std::vector< double >& m_times;

    public:
    push_back_state_and_time( std::vector< state_type > &states ,
                              std::vector< double > &times )
    : m_states( states ) , m_times( times ) { }

    void operator()( const state_type &x , double t )
    {
        m_states.push_back( x );
        m_times.push_back( t );
        // std::cout << t << std::endl;
    }

    double getval(T_vecsize idx_time, T_vecsize idx_statevec)
    {
        return m_states[idx_time][idx_statevec];
    }
};
//]

/* Interface function */
int integrate_radon_detector(int N_times,
                             double timestep,
                             int interpolation_mode,
                             double *external_radon_conc,
                             double *airt,
                             double *initial_state,
                             double *state_history,
                             double *parameters)
{
    using namespace boost::numeric::odeint;

    // initialise the boundary conditions object
    std::vector< double > intvals;
    intvals = carray_to_vector(external_radon_conc, N_times);
    linear_interpolator boundary_conditions(timestep, intvals, interpolation_mode);

    // initialise the air temperature object (always use linear intrpolation)
    std::vector< double > airt_intvals;
    airt_intvals = carray_to_vector(airt, N_times);
    linear_interpolator boundary_conditions_airt(timestep, airt_intvals, 1);

    // copy initial state into a state_type object
    state_type x0 = carray_to_vector(initial_state, NUM_STATE_VARIABLES);
    double t0 = 0.0;
    double t1 = timestep*N_times;

    // initialise the RHS of the system of equations
    two_filter_detector system_of_equations(boundary_conditions,
                                            boundary_conditions_airt,
                                            parameters);

    // initialise the stepper (integrator)
    // ref: http://www.boost.org/doc/libs/1_57_0/libs/numeric/odeint/doc/html/boost_numeric_odeint/odeint_in_detail/steppers.html
    // (it would be nicer to use 'auto' for the type, but we'd need extra compiler flags)

    // stepper options: http://www.boost.org/doc/libs/1_57_0/libs/numeric/odeint/doc/html/boost_numeric_odeint/odeint_in_detail/steppers.html#boost_numeric_odeint.odeint_in_detail.steppers.stepper_overview

    //// OPTION 1: dense output stepper (step size can be larger than output grid)
    //typedef boost::numeric::odeint::result_of::make_dense_output<
    //    runge_kutta_dopri5< state_type > >::type dense_stepper_type;
    //dense_stepper_type stepper = make_dense_output( 1.0e-6 , 1.0e-5 ,
    //                                    runge_kutta_dopri5< state_type >() );

    // OPTION 2: controlled stepper (error is controlled, but dt must be chosen
    // such that the stepper ends up at exactly the endpoint)
    typedef boost::numeric::odeint::result_of::make_controlled<
        runge_kutta_dopri5< state_type > >::type controlled_stepper_type;
    controlled_stepper_type stepper = make_controlled( 1.0e-6 , 1.0e-5 ,
                                        runge_kutta_dopri5< state_type >() );

    //initialise observer (i.e. container for recording output at each step)
    std::vector< state_type > obs_x_vec;
    std::vector< double > obs_times_vec;
    obs_x_vec.reserve(N_times);
    obs_times_vec.reserve(N_times);
    push_back_state_and_time observer(obs_x_vec, obs_times_vec);

    // integrate the system of odes
    // ref: http://www.boost.org/doc/libs/1_57_0/libs/numeric/odeint/doc/html/boost_numeric_odeint/odeint_in_detail/integrate_functions.html
    boost::numeric::odeint::integrate_const(stepper, system_of_equations,
                                            x0, t0, t1, timestep, observer);

    //copy the integration history from the observer into the output array
    double *state_iter = state_history;
    for (int ii=0; ii<N_times; ii++){
        for (int jj=0; jj<NUM_STATE_VARIABLES; jj++){
            *state_iter = observer.getval(ii,jj);
            state_iter++;
        }
    }

    return 0;
}

/* Function for testing the linear interpolation class */
double linear_interpolation(double xi, int N, double timestep, const double *y, const int mode)
{
    assert(mode == 0 || mode == 1);
    std::vector< double > intvals;
    intvals = carray_to_vector(y, N);
    /*
    for(int ii=0; ii<N; ii++){
        std::cout << intvals[ii] << " ";
    }
    std::cout << std::endl;
    */
    linear_interpolator li(timestep, intvals, mode);
    return li(xi);
}


// Copyright (C) 2014 The Regents of the University of California (Regents).
// 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 The Regents or University of California 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 THE COPYRIGHT HOLDERS OR 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/estimators/estimate_uncalibrated_relative_pose.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <limits>
#include <memory>
#include <vector>

#include "theia/matching/feature_correspondence.h"
#include "theia/sfm/create_and_initialize_ransac_variant.h"
#include "theia/sfm/pose/eight_point_fundamental_matrix.h"
#include "theia/sfm/pose/essential_matrix_utils.h"
#include "theia/sfm/pose/fundamental_matrix_util.h"
#include "theia/sfm/pose/util.h"
#include "theia/sfm/triangulation/triangulation.h"
#include "theia/solvers/estimator.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/util/util.h"

namespace theia {

using Eigen::Matrix3d;
using Eigen::Vector3d;

namespace {

// An estimator for computing the relative pose from 8 feature correspondences
// (via decomposition of the fundamental matrix).
//
// NOTE: Feature correspondences must be in pixel coordinates with the principal
// point removed i.e. principal point at (0, 0). This also assumes negligible
// skew (which is reasonable for most cameras).
class UncalibratedRelativePoseEstimator
    : public Estimator<FeatureCorrespondence, UncalibratedRelativePose> {
 public:
  UncalibratedRelativePoseEstimator() {}

  // 8 correspondences are needed to determine a fundamental matrix and thus a
  // relative pose.
  double SampleSize() const { return 8; }

  // Estimates candidate relative poses from correspondences.
  bool EstimateModel(
      const std::vector<FeatureCorrespondence>& centered_correspondences,
      std::vector<UncalibratedRelativePose>* relative_poses) const {
    std::vector<Eigen::Vector2d> image1_points, image2_points;
    for (int i = 0; i < 8; i++) {
      image1_points.emplace_back(centered_correspondences[i].feature1);
      image2_points.emplace_back(centered_correspondences[i].feature2);
    }

    UncalibratedRelativePose relative_pose;
    if (!NormalizedEightPointFundamentalMatrix(
            image1_points, image2_points, &relative_pose.fundamental_matrix)) {
      return false;
    }

    // Only consider fundamental matrices that we can decompose focal lengths
    // from.
    if (!FocalLengthsFromFundamentalMatrix(
            relative_pose.fundamental_matrix.data(),
            &relative_pose.focal_length1,
            &relative_pose.focal_length2)) {
      return false;
    }

    // TODO(cmsweeney): Should we check if the focal lengths are reasonable?

    // Compose the essential matrix from the fundamental matrix and focal
    // lengths.
    Matrix3d essential_matrix;
    EssentialMatrixFromFundamentalMatrix(
        relative_pose.fundamental_matrix.data(),
        relative_pose.focal_length1,
        relative_pose.focal_length2,
        essential_matrix.data());

    // Normalize the centered_correspondences.
    std::vector<FeatureCorrespondence> normalized_correspondences(
        centered_correspondences.size());
    for (int i = 0; i < centered_correspondences.size(); i++) {
      normalized_correspondences[i].feature1 =
          centered_correspondences[i].feature1 / relative_pose.focal_length1;
      normalized_correspondences[i].feature2 =
          centered_correspondences[i].feature2 / relative_pose.focal_length2;
    }

    GetBestPoseFromEssentialMatrix(essential_matrix,
                                   normalized_correspondences,
                                   &relative_pose.rotation,
                                   &relative_pose.position);
    relative_poses->emplace_back(relative_pose);
    return true;
  }

  // The error for a correspondences given a model. This is the squared sampson
  // error.
  double Error(const FeatureCorrespondence& centered_correspondence,
               const UncalibratedRelativePose& relative_pose) const {
    FeatureCorrespondence normalized_correspondence;
    normalized_correspondence.feature1 =
        centered_correspondence.feature1 / relative_pose.focal_length1;
    normalized_correspondence.feature2 =
        centered_correspondence.feature2 / relative_pose.focal_length2;
    if (!IsTriangulatedPointInFrontOfCameras(normalized_correspondence,
                                             relative_pose.rotation,
                                             relative_pose.position)) {
      return std::numeric_limits<double>::max();
    }

    return SquaredSampsonDistance(relative_pose.fundamental_matrix,
                                  centered_correspondence.feature1,
                                  centered_correspondence.feature2);
  }

 private:
  DISALLOW_COPY_AND_ASSIGN(UncalibratedRelativePoseEstimator);
};

}  // namespace

bool EstimateUncalibratedRelativePose(
    const RansacParameters& ransac_params,
    const RansacType& ransac_type,
    const std::vector<FeatureCorrespondence>& centered_correspondences,
    UncalibratedRelativePose* relative_pose,
    RansacSummary* ransac_summary) {
  UncalibratedRelativePoseEstimator relative_pose_estimator;
  std::unique_ptr<SampleConsensusEstimator<UncalibratedRelativePoseEstimator> >
      ransac = CreateAndInitializeRansacVariant(ransac_type,
                                                ransac_params,
                                                relative_pose_estimator);

  // Estimate essential matrix.
  return ransac->Estimate(centered_correspondences,
                          relative_pose,
                          ransac_summary);
}

}  // namespace theia


//==============================================================================
//         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_POLYNOMIALS_FUNCTIONS_SCALAR_HERMITE_HPP_INCLUDED
#define NT2_POLYNOMIALS_FUNCTIONS_SCALAR_HERMITE_HPP_INCLUDED

#include <nt2/polynomials/functions/hermite.hpp>
#include <nt2/include/functions/scalar/oneplus.hpp>
#include <nt2/include/constants/one.hpp>
#include <boost/dispatch/meta/as_floating.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A1 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( hermite_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_< integer_<A0> >)(scalar_< arithmetic_<A1> >)
                            )
  {

    typedef typename boost::dispatch::meta::as_floating<A1>::type result_type;

    NT2_FUNCTOR_CALL(2)
    {
      return nt2::hermite(a0, result_type(a1));
    }
  };

  /////////////////////////////////////////////////////////////////////////////
  // Implementation when type A1 is floating_
  /////////////////////////////////////////////////////////////////////////////
  BOOST_DISPATCH_IMPLEMENT  (hermite_, tag::cpu_,
                             (A0)(A1),
                             (scalar_< integer_<A0> > )(scalar_< floating_<A1> > )
                            )
  {
    typedef A1 result_type;
    NT2_FUNCTOR_CALL(2)
    {
      A1 p0 = One<A1>();
      if(a0 == 0) return p0;
      A1 p1 = a1+a1;
      A0 c = 1;
      while(c < a0)
      {
        std::swap(p0, p1);
        p1 = hermite_next(c, a1, p0, p1);
        ++c;
      }
      return p1;
    }
  private:
    template <class T, class T1, class T2>
    static inline T
    hermite_next(const uint32_t& n, const T& x, const T1& Hn, const T2& Hnm1)
    {
      return (2 * x * Hn - 2 * n * Hnm1);
    }
  };
} }

#endif


#include <iostream>
#include <Eigen>
#include <fstream>
#include <unistd.h>
#include <cstdlib>
#include "denseBlocksJacobi.h"
#include "denseOverlappingJacobi.h"
#include "denseAsyncBlocksJacobi.h"
#include "denseAsyncOverlappingJacobi.h"
#include "denseAsyncJacobi.h"
#include "denseParallelJacobi.h"
#include "denseSerialJacobi.h"
#include "sparseBlocksJacobi.h"
#include "sparseOverlappingJacobi.h"
#include "sparseAsyncBlocksJacobi.h"
#include "sparseAsyncOverlappingJacobi.h"
#include "sparseAsyncJacobi.h"
#include "sparseParallelJacobi.h"
#include "sparseSerialJacobi.h"
#include "parser.h"
#include "sparseOptimizedBlocksJacobi.h"
#include "sparseOptimizedOverlappingJacobi.h"
#include "denseOptimizedBlocksJacobi.h"
#include "denseOptimizedOverlappingJacobi.h"

using namespace std;
using namespace Eigen;
using namespace Iterative;

auto fromFile = false;
auto debug = false;
auto toCsv = false;
string filename;
string inputMatrix;

ulong matrixSize = 1024;
ulong iterations = 100;
double tolerance = 0.000000001;
int workers = 8;
auto blockSize = 64;

const auto sequential = "sequential";
const auto parallel = "parallel";
const auto parallel_async = "parallel_async";
const auto blocks = "blocks";
const auto blocks_optimized = "blocks_optimized";
const auto blocks_async = "blocks_async";
const auto overlapping = "overlapping";
const auto overlapping_optimized = "overlapping_optimized";
const auto overlapping_async = "overlapping_async";

enum methods {
    SEQUENTIAL,
    PARALLEL,
    PARALLEL_ASYNC,
    BLOCKS,
    BLOCKS_OPTIMIZED,
    BLOCKS_ASYNC,
    OVERLAPPING,
    OVERLAPPING_OPTIMIZED,
    OVERLAPPING_ASYNC
};

auto method = SEQUENTIAL;
string methodString = sequential;

void parse_args(int argc, char *argv[]);
void write_csv(string fileName, std::chrono::duration<double> time, double error, long iteration);

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

    parse_args(argc, argv);

    Eigen::setNbThreads(workers);
    auto error = 0.;

    chrono::time_point<chrono::high_resolution_clock> start_time;
    chrono::time_point<chrono::high_resolution_clock> end_time;
    ColumnVector<double, Dynamic> x;
    srand(42);

#ifdef DENSE

    Matrix<double, Dynamic, Dynamic> A = Matrix<double, Dynamic, Dynamic>::Random(matrixSize, matrixSize);
    ColumnVector<double, Dynamic> b = ColumnVector<double, Dynamic>::Random(matrixSize);

#pragma omp parallel for schedule(static)
    for (int i = 0; i < A.rows(); ++i) {
        auto value = A.row(i).template lpNorm<1>();
        A(i, i) = value;
    }

    denseSerialJacobi<double, Dynamic> serialJacobi(A, b, iterations, tolerance);
    denseParallelJacobi<double, Dynamic> parallelJacobi(A, b, iterations, tolerance, workers);
    denseAsyncJacobi<double, Dynamic> asyncJacobi(A, b, iterations, tolerance, workers);
    denseBlocksJacobi<double, Dynamic> blocksJacobi(A, b, iterations, tolerance, workers, blockSize);
    denseOptimizedBlocksJacobi<double, Dynamic> optimizedBlocksJacobi(A, b, iterations, tolerance, workers, blockSize);
    denseOverlappingJacobi<double, Dynamic> overlappingJacobi(A, b, iterations, tolerance, workers, blockSize);
    denseOptimizedOverlappingJacobi<double, Dynamic> optimizedOverlappingJacobi(A, b, iterations, tolerance, workers, blockSize);
    denseAsyncBlocksJacobi<double, Dynamic> asyncBlocksJacobi(A, b, iterations, tolerance, workers, blockSize);
    denseAsyncOverlappingJacobi<double, Dynamic> asyncOverlappingJacobi(A, b, iterations, tolerance, workers, blockSize);

#endif

#ifdef SPARSE

        SparseMatrix<double>A;
        if (fromFile){
            ifstream input(inputMatrix);
            read_matrix<double>(A, input);
            input.close();

        } else {

            A.resize(matrixSize, matrixSize);
            typedef Eigen::Triplet<double> T;

            vector<T> triplets;

            for (int i = 0; i < 100*matrixSize; ++i) {
                triplets.emplace_back(T(abs(rand())%matrixSize,abs(rand())%matrixSize,(double)rand()*1000/RAND_MAX));
                if(i<matrixSize)
                    triplets.emplace_back(T(i,i,(double)rand()*1000/RAND_MAX));
            }

            A.setFromTriplets(triplets.begin(),triplets.end());

            triplets.clear();

            #pragma omp parallel for schedule(static)
            for (int i = 0; i < matrixSize; ++i) {
                auto sum=0.;
                for (int j = 0; j < matrixSize; ++j) {
                    sum+=abs(A.coeff(i,j));
                }
                A.coeffRef(i,i) = sum;
            }
        }

        ColumnVector<double, Dynamic> b = ColumnVector<double, Dynamic>::Zero(A.cols());

        if(!fromFile) b.setRandom();

        sparseSerialJacobi<double> serialJacobi(A, b, iterations, tolerance);
        sparseParallelJacobi<double> parallelJacobi(A, b, iterations, tolerance, workers);
        sparseAsyncJacobi<double> asyncJacobi(A, b, iterations, tolerance, workers);
        sparseBlocksJacobi<double> blocksJacobi(A, b, iterations, tolerance, workers, blockSize);
        sparseOptimizedBlocksJacobi<double> optimizedBlocksJacobi(A, b, iterations, tolerance, workers, blockSize);
        sparseOverlappingJacobi<double> overlappingJacobi(A, b, iterations, tolerance, workers, blockSize);
        sparseOptimizedOverlappingJacobi<double> optimizedOverlappingJacobi(A, b, iterations, tolerance, workers, blockSize);
        sparseAsyncBlocksJacobi<double> asyncBlocksJacobi(A, b, iterations, tolerance, workers, blockSize);
        sparseAsyncOverlappingJacobi<double> asyncOverlappingJacobi(A, b, iterations, tolerance, workers, blockSize);

#endif

    auto iterationsPerformed = 0L;

    switch (method){

        case SEQUENTIAL:
            methodString = sequential;
            cout << "Sequential" << endl;
            start_time = Time::now();
            x = serialJacobi.solve();
            end_time = Time::now();
            error = (b-A*x).template lpNorm<1>()/matrixSize;
            cout << "error: " << error << endl;
            std::cout << "time: " << ' ' << dsec(end_time - start_time).count() << std::endl;
            iterationsPerformed = serialJacobi.getIteration();
            break;
        case PARALLEL:
            methodString = parallel;
            cout << "Parallel" << endl;
            start_time = Time::now();
            x = parallelJacobi.solve();
            end_time = Time::now();
            error = (b - A * x).template lpNorm<1>() / matrixSize;
            cout << "error: " << error << endl;
            std::cout << "time: " << ' ' << dsec(end_time - start_time).count() << std::endl;
            iterationsPerformed = parallelJacobi.getIteration();
            break;
        case PARALLEL_ASYNC:
            methodString = parallel_async;
            cout << "Parallel async" << endl;
            start_time = Time::now();
            x = asyncJacobi.solve();
            end_time = Time::now();
            error = (b - A * x).template lpNorm<1>() / matrixSize;
            cout << "error: " << error << endl;
            std::cout << "time: " << ' ' << dsec(end_time - start_time).count() << std::endl;
            iterationsPerformed = asyncJacobi.getIteration();
            break;
        case BLOCKS:
            methodString = blocks;
            cout << "Parallel blocks" << endl;
            start_time = Time::now();
            x = blocksJacobi.solve();
            end_time = Time::now();
            error = (b - A * x).template lpNorm<1>() / matrixSize;
            cout << "error: " << error << endl;
            std::cout << "time: " << ' ' << dsec(end_time - start_time).count() << std::endl;
            iterationsPerformed = blocksJacobi.getIteration();

            break;
        case BLOCKS_OPTIMIZED:
            methodString = blocks_optimized;
            cout << "Parallel Optimized blocks" << endl;
            start_time = Time::now();
            x = optimizedBlocksJacobi.solve();
            end_time = Time::now();
            error = (b - A * x).template lpNorm<1>() / matrixSize;
            cout << "error: " << error << endl;
            std::cout << "time: " << ' ' << dsec(end_time - start_time).count() << std::endl;
            iterationsPerformed = optimizedBlocksJacobi.getIteration();
            break;
        case BLOCKS_ASYNC:
            methodString = blocks_async;
            cout << "Parallel async blocks" << endl;
            start_time = Time::now();
            x = asyncBlocksJacobi.solve();
            end_time = Time::now();
            error = (b - A * x).template lpNorm<1>() / matrixSize;
            cout << "error: " << error << endl;
            std::cout << "time: " << ' ' << dsec(end_time - start_time).count() << std::endl;
            iterationsPerformed = asyncBlocksJacobi.getIteration();

            break;
        case OVERLAPPING:
            methodString = overlapping;
            cout << "Parallel overlapping" << endl;
            start_time = Time::now();
            x = overlappingJacobi.solve();
            end_time = Time::now();
            error = (b - A * x).template lpNorm<1>() / matrixSize;
            cout << "error: " << error << endl;
            std::cout << "time: " << ' ' << dsec(end_time - start_time).count() << std::endl;
            iterationsPerformed = overlappingJacobi.getIteration();
            break;
        case OVERLAPPING_OPTIMIZED:
            methodString = overlapping_optimized;
            cout << "Parallel optimized overlapping" << endl;
            start_time = Time::now();
            x = optimizedOverlappingJacobi.solve();
            end_time = Time::now();
            error = (b - A * x).template lpNorm<1>() / matrixSize;
            cout << "error: " << error << endl;
            std::cout << "time: " << ' ' << dsec(end_time - start_time).count() << std::endl;
            iterationsPerformed = optimizedOverlappingJacobi.getIteration();
            break;
        case OVERLAPPING_ASYNC:
            methodString = overlapping_async;
            cout << "Parallel async overlapping blocks" << endl;
            start_time = Time::now();
            x = asyncOverlappingJacobi.solve();
            end_time = Time::now();
            error = (b - A * x).template lpNorm<1>() / matrixSize;
            cout << "error: " << error << endl;
            std::cout << "time: " << ' ' << dsec(end_time - start_time).count() << std::endl;
            iterationsPerformed = asyncOverlappingJacobi.getIteration();
            break;
    }

    if(toCsv) write_csv(filename, dsec(end_time - start_time), error, iterationsPerformed);

    if(debug) cout << x.transpose() << endl;

//    cerr << "OK" << endl;

    return 0;

}

void parse_args(int argc,  char *argv[]) {

    string arg(argv[1]);
    if (arg == sequential) method = SEQUENTIAL;
    else if (arg == parallel) method = PARALLEL;
    else if (arg == parallel_async) method = PARALLEL_ASYNC;
    else if (arg == blocks) method = BLOCKS;
    else if (arg == blocks_optimized) method = BLOCKS_OPTIMIZED;
    else if (arg == blocks_async) method = BLOCKS_ASYNC;
    else if (arg == overlapping) method = OVERLAPPING;
    else if (arg == overlapping_optimized) method = OVERLAPPING_OPTIMIZED;
    else if (arg == overlapping_async) method = OVERLAPPING_ASYNC;
    else exit(1);

    InputParser input(argc,argv);

    if(input.cmdOptionExists("-w")){

        workers = (int) strtol(input.getCmdOption("-w").c_str(), nullptr, 10);
    }

    if(input.cmdOptionExists("-s")){

        matrixSize = (ulong) strtol(input.getCmdOption("-s").c_str(), nullptr, 10);
    }

    if(input.cmdOptionExists("-i")){

        iterations = (ulong) strtol(input.getCmdOption("-i").c_str(), nullptr, 10);
    }

    if(input.cmdOptionExists("-t")){

        tolerance = stod(input.getCmdOption("-t"));
    }

    if(input.cmdOptionExists("-b")){

        blockSize = (int) strtol(input.getCmdOption("-b").c_str(), nullptr, 10);
    }

    if(input.cmdOptionExists("-p")){
        filename = input.getCmdOption("-p");
        toCsv = true;
        cout << filename << endl;
    }

    if(input.cmdOptionExists("-d")){
        debug = true;
    }

    if(input.cmdOptionExists("-m")){
        inputMatrix = input.getCmdOption("-m");
        fromFile = true;
    }

}

void write_csv(string fileName, std::chrono::duration<double> time, double error, long iteration){

    ifstream test(fileName);
    auto exists = test.good();
    test.close();
    ofstream outFile(fileName, ofstream::out | ofstream::app);

    if(!exists){
        outFile << "algorithm,time,iterations,workers,error,size,blockSize" << endl;
    }

    outFile << methodString << ',' << time.count() << ',' << iteration <<','<< workers << ',' << error << ','
            << matrixSize << ',' << blockSize << endl;

}


#include <cstdlib>
#include <iostream>
#include <complex>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/blas.hpp>
#include <Eigen/Core>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/eigen/vector.hpp>
#include <boost/numeric/bindings/blas/level1.hpp>
#include <boost/numeric/bindings/remove_imaginary.hpp>
#include "random.hpp"

template<typename T>
struct asum_cl {
  static T asum(const T &x) {
    return std::abs(x);
  }
};

template<typename T>
struct asum_cl<std::complex<T>> {
  static T asum(const std::complex<T> &x) {
    return std::abs(x.real())+std::abs(x.imag());
  }
};

template<typename Vec>
int
iamax(const Vec &v) {
  namespace bindings=boost::numeric::bindings;
  typedef typename bindings::value_type<Vec>::type value_type;
  auto i=bindings::begin(v);
  auto i_end=bindings::end(v);
  auto i_res=i;
  for (; i!=i_end; ++i)
    if (asum_cl<value_type>::asum(*i)>asum_cl<value_type>::asum(*i_res))
      i_res=i;
  return std::distance(bindings::begin(v), i_res);
}

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;

typedef double real;
typedef std::complex<real> complex;

int main(int argc, char *argv[]) {
  {
    typedef ublas::vector<real> vector;
    typedef ublas::matrix<real> matrix;
    typedef vector::size_type size_type;
    rand_normal<real>::reset();
    size_type n=8;
    vector v(n);
    for (size_type i=0; i<n; ++i)
      v(i)=rand_normal<real>::get();
    std::cout << "ublas using vectors : iamax(v) = " << iamax(v) << '\n'
	      << "blas using vectors  : iamax(v) = " << blas::iamax(v) << '\n';
    matrix M(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i)
    	M(i, j)=0;
    ublas::matrix_column<matrix> mc(M, 2);
    ublas::matrix_row<matrix> mr(M, 3);
    mc=v;
    std::cout << "blas using cols     : iamax(v) = " << blas::iamax(mc) << '\n';
    mr=v;
    std::cout << "blas using rows     : iamax(v) = " << blas::iamax(mr) << '\n';
  }
  {
    typedef ublas::vector<complex> vector;
    typedef ublas::matrix<complex> matrix;
    typedef vector::size_type size_type;
    rand_normal<complex>::reset();
    size_type n=8;
    vector v(n);
    for (size_type i=0; i<n; ++i)
      v(i)=rand_normal<complex>::get();
    std::cout << "ublas using vectors : iamax(v) = " << iamax(v) << '\n'
	      << "blas using vectors  : iamax(v) = " << blas::iamax(v) << '\n';
    matrix M(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i)
    	M(i, j)=0;
    ublas::matrix_column<matrix> mc(M, 2);
    ublas::matrix_row<matrix> mr(M, 3);
    mc=v;
    std::cout << "blas using cols     : iamax(v) = " << blas::iamax(mc) << '\n';
    mr=v;
    std::cout << "blas using rows     : iamax(v) = " << blas::iamax(mr) << '\n';
  }
  {
    typedef Eigen::Matrix<real, Eigen::Dynamic, 1> vector;
    typedef Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> matrix;
    typedef int size_type;
    rand_normal<real>::reset();
    size_type n=8;
    vector v(n);
    for (size_type i=0; i<n; ++i)
      v(i)=rand_normal<real>::get();
    std::cout << "eigen using vectors : iamax(v) = " << iamax(v) << '\n'
	      << "blas using vectors  : iamax(v) = " << blas::iamax(v) << '\n';
    matrix M(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i)
    	M(i, j)=0;
    auto mc=M.col(2);
    auto mr=M.row(3);
    mc=v;
    std::cout << "blas using cols     : iamax(v) = " << blas::iamax(mc) << '\n';
    mr=v;
    std::cout << "blas using rows     : iamax(v) = " << blas::iamax(mr) << '\n';
  }
  {
    typedef Eigen::Matrix<complex, Eigen::Dynamic, 1> vector;
    typedef Eigen::Matrix<complex, Eigen::Dynamic, Eigen::Dynamic> matrix;
    typedef int size_type;
    rand_normal<complex>::reset();
    size_type n=8;
    vector v(n);
    for (size_type i=0; i<n; ++i)
      v(i)=rand_normal<complex>::get();
    std::cout << "eigen using vectors : iamax(v) = " << iamax(v) << '\n'
	      << "blas using vectors  : iamax(v) = " << blas::iamax(v) << '\n';
    matrix M(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i)
    	M(i, j)=0;
    auto mc=M.col(2);
    auto mr=M.row(3);
    mc=v;
    std::cout << "blas using cols     : iamax(v) = " << blas::iamax(mc) << '\n';
    mr=v;
    std::cout << "blas using rows     : iamax(v) = " << blas::iamax(mr) << '\n';
  }
  return EXIT_SUCCESS;
}


#ifndef TVMTL_MANIFOLD_GRASSMANN_HPP
#define TVMTL_MANIFOLD_GRASSMANN_HPP

#include <cmath>
#include <complex>
#include <iostream>
#include <functional>

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/SVD>
#include <Eigen/QR>
#include <unsupported/Eigen/MatrixFunctions>
#include <unsupported/Eigen/KroneckerProduct>

//own includes
#include "enumerators.hpp"
#include "matrix_utils.hpp"

namespace tvmtl {

// Specialization GRASSMANN
template <int N, int P>
struct Manifold< GRASSMANN, N, P> {

    public:
	static const MANIFOLD_TYPE MyType;
	static const int manifold_dim ;
	static const int value_dim; // TODO: maybe rename to embedding_dim

	static const bool non_isometric_embedding;

	// Scalar type of manifold
	//typedef double scalar_type;
	typedef double scalar_type;
	typedef double dist_type;
	typedef std::complex<double> complex_type;
	typedef std::vector<double> weight_list;

	// Value Typedef
	typedef Eigen::Matrix< scalar_type, N, P>				value_type;
	typedef value_type&							ref_type;
	typedef const value_type&						cref_type;
	typedef std::vector<value_type, Eigen::aligned_allocator<value_type> >	value_list;

	// Tangent space typedefs
	typedef Eigen::Matrix <scalar_type, N * P, P * (N - P) > tm_base_type;
	typedef tm_base_type& tm_base_ref_type;

	// Derivative Typedefs
	typedef value_type			     deriv1_type;
	typedef deriv1_type&			     deriv1_ref_type;

	typedef Eigen::Matrix<scalar_type, N*P, N*P>				deriv2_type;
	typedef deriv2_type&							deriv2_ref_type;
	typedef	Eigen::Matrix<scalar_type, P * (N - P), P * (N - P) >		restricted_deriv2_type;

	// Helper Types
	typedef Eigen::PermutationMatrix<N * P, N * P, int> perm_type;

	inline static perm_type ConstructPermutationMatrix();

	// Manifold distance functions (for IRLS)
	inline static dist_type dist_squared(cref_type x, cref_type y);
	inline static dist_type distPF_squared(cref_type x, cref_type y );
	inline static dist_type distGeod_squared( cref_type x, cref_type y );

	inline static void deriv1x_dist_squared(cref_type x, cref_type y, deriv1_ref_type result);
	inline static void deriv1y_dist_squared(cref_type x, cref_type y, deriv1_ref_type result);

	inline static void deriv2xx_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);
	inline static void deriv2xy_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);
	inline static void deriv2yy_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);

	static const perm_type permutation_matrix;

	// Manifold exponentials und logarithms ( for Proximal point)
	template <typename DerivedX, typename DerivedY>
	inline static void exp(const Eigen::MatrixBase<DerivedX>& x, const Eigen::MatrixBase<DerivedY>& y, Eigen::MatrixBase<DerivedX>& result);
	inline static void log(cref_type x, cref_type y, ref_type result);

	inline static void convex_combination(cref_type x, cref_type y, double t, ref_type result);

	// Implementations of the Karcher mean
	// Slow list version
	inline static void karcher_mean(ref_type x, const value_list& v, double tol=1e-10, int maxit=15);
	inline static void weighted_karcher_mean(ref_type x, const weight_list& w, const value_list& v, double tol=1e-10, int maxit=15);
	// Variadic templated version
	template <typename V, class... Args>
	inline static void karcher_mean(V& x, const Args&... args);
	template <typename V>
	inline static void variadic_karcher_mean_gradient(V& x, const V& y);
	template <typename V, class... Args>
	inline static void variadic_karcher_mean_gradient(V& x, const V& y1, const Args&... args);

	// Basis transformation for restriction to tangent space
	inline static void tangent_plane_base(cref_type x, tm_base_ref_type result);

	// Projections
	inline static void horizontal_space_projector(cref_type x, ref_type a);
	inline static void projector(ref_type x);

	// Interpolation pre- and postprocessing
	inline static void interpolation_preprocessing(ref_type x) {};
	inline static void interpolation_postprocessing(ref_type x) {};

};

/*-----IMPLEMENTATION GRASSMANN----------*/

// Static constants, Outside definition to avoid linker error

template <int N, int P>
const MANIFOLD_TYPE Manifold <GRASSMANN, N, P>::MyType = GRASSMANN;

template <int N, int P>
const int Manifold <GRASSMANN, N, P>::manifold_dim = (N - P) * P;

template <int N, int P>
const int Manifold <GRASSMANN, N, P>::value_dim = N * P;

template <int N, int P>
const bool Manifold <GRASSMANN, N, P>::non_isometric_embedding = false;

// PermutationMatrix
template <int N, int P>
typename Manifold < GRASSMANN, N, P>::perm_type Manifold<GRASSMANN, N, P>::ConstructPermutationMatrix(){

    Eigen::Matrix<int, N * P, 1> indices;
    indices.setZero();

    int inc = 0;
    for(int i=1; i< indices.size(); ++i){
	if(i % 2 == 0)
	    inc = -P;
	else
	    inc = N;
	indices(i) = indices(i-1) + inc;
    }

    perm_type Perm(indices);
    return Perm.transpose();
}

template <int N, int P>
const typename Manifold < GRASSMANN, N, P>::perm_type Manifold<GRASSMANN, N, P>::permutation_matrix = ConstructPermutationMatrix();

// Squared GRASSMANN distance function
template <int N, int P>
inline typename Manifold <GRASSMANN, N, P>::dist_type Manifold <GRASSMANN, N, P>::dist_squared( cref_type x, cref_type y ){
    return distPF_squared(x,y);
}

template <int N, int P>
inline typename Manifold <GRASSMANN, N, P>::dist_type Manifold <GRASSMANN, N, P>::distGeod_squared( cref_type x, cref_type y ){

    // Geodesic distance
     Eigen::JacobiSVD<Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> > svd(x.transpose() * y, Eigen::ComputeThinU | Eigen::ComputeThinV);
     Eigen::Matrix<scalar_type, P, 1> sv= svd.singularValues();
     for(int i=0; i<P; ++i){
	if(sv(i)>=1.0)
	    sv(i)=0;
	else if(sv(i)<=-1.0)
	    sv(i)=M_PI;
	else
	    sv(i)=std::acos(sv(i));
    }
    #ifdef TVMTL_MANIFOLD_DEBUG_GRASSMANN
     std::cout << sv << std::endl;
    #endif

    return sv.squaredNorm();

}

template <int N, int P>
inline typename Manifold <GRASSMANN, N, P>::dist_type Manifold <GRASSMANN, N, P>::distPF_squared( cref_type x, cref_type y ){

    // Projection F-distance;
    return 0.5 * (x * x.transpose() - y * y.transpose()).squaredNorm();
}

// Derivative of Squared GRASSMANN distance w.r.t. first argument
template <int N, int P>
inline void Manifold <GRASSMANN, N, P>::deriv1x_dist_squared( cref_type x, cref_type y, deriv1_ref_type result){
    result = 2.0 * (x * x.transpose() - y * y.transpose()) * x;
}
// Derivative of Squared GRASSMANN distance w.r.t. second argument
template <int N, int P>
inline void Manifold <GRASSMANN, N, P>::deriv1y_dist_squared( cref_type x, cref_type y, deriv1_ref_type result){
    result = 2.0 * (y * y.transpose() - x * x.transpose()) * y;
}

// Second Derivative of Squared GRASSMANN distance w.r.t first argument
template <int N, int P>
inline void Manifold <GRASSMANN, N, P>::deriv2xx_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    deriv2_type XtXId, IdXXtmYYt, XtXP;
    XtXId = Eigen::kroneckerProduct(x.transpose() * x, Eigen::Matrix<scalar_type, N, N>::Identity());
    IdXXtmYYt = Eigen::kroneckerProduct(Eigen::Matrix<scalar_type, P, P>::Identity(), x * x.transpose() - y * y.transpose());
    XtXP = Eigen::kroneckerProduct(x.transpose(), x) * permutation_matrix;
    result = 2.0 * (XtXId + XtXP + IdXXtmYYt);
}
// Second Derivative of Squared GRASSMANN distance w.r.t first and second argument
template <int N, int P>
inline void Manifold <GRASSMANN, N, P>::deriv2xy_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    result = -2.0 * (Eigen::kroneckerProduct(x.transpose() * y, Eigen::Matrix<scalar_type, N, N>::Identity()) + Eigen::kroneckerProduct(x.transpose(), y) * permutation_matrix);
}
// Second Derivative of Squared GRASSMANN distance w.r.t second argument
template <int N, int P>
inline void Manifold <GRASSMANN, N, P>::deriv2yy_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    deriv2xx_dist_squared(y, x, result);
}

// Exponential and Logarithm Map
template <int N, int P>
template <typename DerivedX, typename DerivedY>
inline void Manifold <GRASSMANN, N, P>::exp(const Eigen::MatrixBase<DerivedX>& x, const Eigen::MatrixBase<DerivedY>& y, Eigen::MatrixBase<DerivedX>& result){
    Eigen::JacobiSVD<Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> > svd(y, Eigen::ComputeThinU | Eigen::ComputeThinV);
    value_type temp_result = x * svd.matrixV() * svd.singularValues().array().cos().matrix().asDiagonal() * svd.matrixV().transpose() + svd.matrixU() * svd.singularValues().array().sin().matrix().asDiagonal() * svd.matrixV().transpose();
    // Reorthonormalization
    Eigen::HouseholderQR<value_type> qr(temp_result);
    result = qr.householderQ() * value_type::Identity();
}

template <int N, int P>
inline void Manifold <GRASSMANN, N, P>::log(cref_type x, cref_type y, ref_type result){
    Eigen::Matrix<scalar_type, P, P> YtX = y.transpose() * x;
    Eigen::Matrix<scalar_type, P, N> At = y.transpose() - YtX * x.transpose();

    value_type B = YtX.householderQr().solve(At).transpose();

    Eigen::JacobiSVD<Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> > svd(B, Eigen::ComputeThinU | Eigen::ComputeThinV);

    #ifdef TVMTL_MANIFOLD_DEBUG_GRASSMANN
        std::cout << "\n\nB\n" << B << std::endl;
	std::cout << "U\n" << svd.matrixU() << std::endl;
	std::cout << "S\n" << svd.singularValues() << std::endl;
	std::cout << "Vt\n" << svd.matrixV().transpose() << std::endl;
    #endif

    result = svd.matrixU() * svd.singularValues().unaryExpr(std::function<scalar_type(scalar_type)>((scalar_type(*)(scalar_type))&std::atan)).asDiagonal() * svd.matrixV().transpose();
}

// Tangent Plane restriction
template <int N, int P>
inline void Manifold <GRASSMANN, N, P>::tangent_plane_base(cref_type x, tm_base_ref_type result){
    //value_type Hproj = (Eigen::Matrix<scalar_type, N, N>::Identity() - x * x.transpose()) * x; // x - xx^tx = x - x = 0 TODO: Check of that makes sense
    //Eigen::JacobiSVD<value_type> svd(Hproj);

    // Compute X_orth by SVD
   /*
    Eigen::JacobiSVD<value_type> svd(x);
    Eigen::Matrix<scalar_type, N, N - P> xorth = svd.matrixU().rightCols(N-P);
    */

    //Compute X_orth by QR
    Eigen::HouseholderQR<value_type> qr(x);
    Eigen::Matrix<scalar_type, N, N> Q = qr.householderQ();
    Eigen::Matrix<scalar_type, N, N - P> xorth = Q.rightCols(N-P);

    int k = 0;
    for(int r = 0; r < N - P; r++)
	for(int c = 0; c < P; c++ ){
	    Eigen::Matrix<scalar_type, N, P> T = Eigen::Matrix<scalar_type, N, P>::Zero();
	    T.col(c) = xorth.col(r);
	    result.col(k) = Eigen::Map<Eigen::VectorXd>(T.data(), T.size());
	    ++k;
	}

}

template <int N, int P>
inline void Manifold <GRASSMANN, N, P>::horizontal_space_projector(cref_type x, ref_type a){
	    a = a - x * x.transpose() * a;
}

// Convex geodesic combinations
template <int N, int P>
inline void Manifold <GRASSMANN, N, P>::convex_combination(cref_type x, cref_type y, double t, ref_type result){
    value_type l;
    log(x, y, l);
    exp(x, l * t, result);
}

// Karcher mean implementations
template <int N, int P>
inline void Manifold<GRASSMANN, N, P>::karcher_mean(ref_type x, const value_list& v, double tol, int maxit){
    value_type L, temp;

    int k = 0;
    double error = 0.0;
    do{
	scalar_type m1 = x.sum();
	L = value_type::Zero();
	for(int i = 0; i < v.size(); ++i){
	    log(x, v[i], temp);
	    L += temp;
	}
	exp(x, 1.0 / v.size() * L, temp);
	x = temp;
	error = std::abs(x.sum() - m1);
	++k;
    } while(error > tol && k < maxit);

}

template <int N, int P>
inline void Manifold<GRASSMANN, N, P>::weighted_karcher_mean(ref_type x, const weight_list& w, const value_list& v, double tol, int maxit){
    value_type L, temp;

    int k = 0;
    double error = 0.0;
    do{
	scalar_type m1 = x.sum();
	L = value_type::Zero();
	for(int i = 0; i < v.size(); ++i){
	    log(x, v[i], temp);
	    L += w[i] * temp;
	}
	exp(x, 1.0 / v.size() * L, temp);
	x = temp;
	error = std::abs(x.sum() - m1);
	++k;
    } while(error > tol && k < maxit);

}

template <int N, int P>
template <typename V, class... Args>
inline void Manifold<GRASSMANN, N, P>::karcher_mean(V& x, const Args&... args){
    V temp, sum;

    int numArgs = sizeof...(args);
    int k = 0;
    double error = 0.0;
    double tol = 1e-10;
    int maxit = 15;
    do{
	scalar_type m1 = x.sum();
	sum = x;
	variadic_karcher_mean_gradient(sum, args...);
	exp(x, 1.0 / numArgs * sum, temp);
	x = temp;
	error = std::abs(x.sum() - m1);
	++k;
    } while(error > tol && k < maxit);
}

template <int N, int P>
template <typename V>
inline void Manifold<GRASSMANN, N, P>::variadic_karcher_mean_gradient(V& x, const V& y){
    V temp;
    log(x, y, temp);
    x = temp;
}

template <int N, int P>
template <typename V, class... Args>
inline void Manifold<GRASSMANN, N, P>::variadic_karcher_mean_gradient(V& x, const V& y1, const Args& ... args){
    V temp1, temp2;
    temp2 = x;

    log(x, y1, temp1);

    variadic_karcher_mean_gradient(temp2, args...);
    temp1 += temp2;
    x = temp1;
}

template <int N, int P>
inline void Manifold <GRASSMANN, N, P>::projector(ref_type x){
	    Eigen::HouseholderQR<value_type> qr(x);
	    value_type thinQ = qr.householderQ() * value_type::Identity();
	    x = thinQ;
}

} // end namespace tvmtl

#endif


/**
 * @file burgersequation_main.cc
 * @brief NPDE homework BurgersEquation code
 * @author Oliver Rietmann
 * @date 15.04.2019
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Core>
#include <cstdlib>
#include <fstream>
#include <iostream>

#include "burgersequation.h"

int main() {
  /* SAM_LISTING_BEGIN_1 */
  const unsigned int N = 100;
  Eigen::VectorXd x = Eigen::VectorXd::LinSpaced(N + 1, -1.0, 4.0);
  Eigen::VectorXd mu03 = BurgersEquation::solveBurgersGodunov(0.3, N);
  Eigen::VectorXd mu30 = BurgersEquation::solveBurgersGodunov(3.0, N);

  // Write the solutions to a file that can be used for plotting.
  //====================
  // Your code goes here

  //====================
  /* SAM_LISTING_END_1 */

  /* SAM_LISTING_BEGIN_2 */
  Eigen::Matrix<double, 3, 4> result = BurgersEquation::numexpBurgersGodunov();

  // Write the result to a file that can be used for plotting.
  //====================
  // Your code goes here
  //====================
  /* SAM_LISTING_END_2 */

  return 0;
}


#include <NTL/mat_GF2E.h>
#include <NTL/vec_GF2XVec.h>
#include <NTL/vec_long.h>
#include <NTL/BasicThreadPool.h>

NTL_START_IMPL

//===========================================================

#define PAR_THRESH (40000.0)

static double
GF2E_SizeInWords()
{
   return GF2E::WordLength();
}

static
void mul_aux(Mat<GF2E>& X, const Mat<GF2E>& A, const Mat<GF2E>& B)
{
   long n = A.NumRows();
   long l = A.NumCols();
   long m = B.NumCols();

   if (l != B.NumRows())
      LogicError("matrix mul: dimension mismatch");

   X.SetDims(n, m);

   GF2Context GF2_context;
   GF2_context.save();
   GF2EContext GF2E_context;
   GF2E_context.save();
   double sz = GF2E_SizeInWords();

   bool seq = (double(n)*double(l)*double(m)*sz*sz < PAR_THRESH);

   NTL_GEXEC_RANGE(seq, m, first, last)
   NTL_IMPORT(n)
   NTL_IMPORT(l)
   NTL_IMPORT(m)

   GF2_context.restore();
   GF2E_context.restore();

   long i, j, k;
   GF2X acc, tmp;

   Vec<GF2E> B_col;
   B_col.SetLength(l);

   for (j = first; j < last; j++) {
      for (k = 0; k < l; k++) B_col[k] = B[k][j];

      for (i = 0; i < n; i++) {
         clear(acc);
         for (k = 0; k < l; k++) {
            mul(tmp, rep(A[i][k]), rep(B_col[k]));
            add(acc, acc, tmp);
         }
         conv(X[i][j], acc);
      }
   }

   NTL_GEXEC_RANGE_END
}

void mul(mat_GF2E& X, const mat_GF2E& A, const mat_GF2E& B)
{
   if (&X == &A || &X == &B) {
      mat_GF2E tmp;
      mul_aux(tmp, A, B);
      X = tmp;
   }
   else
      mul_aux(X, A, B);
}

void inv(GF2E& d, Mat<GF2E>& X, const Mat<GF2E>& A)
{
   long n = A.NumRows();

   if (A.NumCols() != n)
      LogicError("inv: nonsquare matrix");

   if (n == 0) {
      set(d);
      X.SetDims(0, 0);
      return;
   }

   const GF2XModulus& G = GF2E::modulus();

   GF2X t1, t2;
   GF2X pivot;
   GF2X pivot_inv;

   Vec< GF2XVec > M;
   // scratch space

   M.SetLength(n);
   for (long i = 0; i < n; i++) {
      M[i].SetSize(n, 2*GF2E::WordLength());
      for (long j = 0; j < n; j++) {
         M[i][j] = rep(A[i][j]);
      }
   }

   GF2X det;
   det = 1;

   Vec<long> P;
   P.SetLength(n);
   for (long k = 0; k < n; k++) P[k] = k;
   // records swap operations

   GF2Context GF2_context;
   GF2_context.save();
   double sz = GF2E_SizeInWords();

   bool seq = double(n)*double(n)*sz*sz < PAR_THRESH;

   bool pivoting = false;

   for (long k = 0; k < n; k++) {

      long pos = -1;

      for (long i = k; i < n; i++) {
         rem(pivot, M[i][k], G);
         if (pivot != 0) {
            InvMod(pivot_inv, pivot, G);
            pos = i;
            break;
         }
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
            P[k] = pos;
            pivoting = true;
         }

         MulMod(det, det, pivot, G);

         {
            // multiply row k by pivot_inv
            GF2X *y = &M[k][0];
            for (long j = 0; j < n; j++) {
               rem(t2, y[j], G);
               MulMod(y[j], t2, pivot_inv, G);
            }
            y[k] = pivot_inv;
         }

         NTL_GEXEC_RANGE(seq, n, first, last)
         NTL_IMPORT(n)
         NTL_IMPORT(k)

         GF2_context.restore();

         GF2X *y = &M[k][0];
         GF2X t1, t2;

         for (long i = first; i < last; i++) {
            if (i == k) continue; // skip row k

            GF2X *x = &M[i][0];
            rem(t1, x[k], G);
            negate(t1, t1);
            x[k] = 0;
            if (t1 == 0) continue;

            // add t1 * row k to row i
            for (long j = 0; j < n; j++) {
               mul(t2, y[j], t1);
               add(x[j], x[j], t2);
            }
         }
         NTL_GEXEC_RANGE_END
      }
      else {
         clear(d);
         return;
      }
   }

   if (pivoting) {
      // pivot colums, using reverse swap sequence

      for (long i = 0; i < n; i++) {
         GF2X *x = &M[i][0];

         for (long k = n-1; k >= 0; k--) {
            long pos = P[k];
            if (pos != k) swap(x[pos], x[k]);
         }
      }
   }

   X.SetDims(n, n);
   for (long i = 0; i < n; i++)
      for (long j = 0; j < n; j++)
         conv(X[i][j], M[i][j]);

   conv(d, det);
}

static
void solve_impl(GF2E& d, Vec<GF2E>& X,
                const Mat<GF2E>& A, const Vec<GF2E>& b, bool trans)

{
   long n = A.NumRows();
   if (A.NumCols() != n)
      LogicError("solve: nonsquare matrix");

   if (b.length() != n)
      LogicError("solve: dimension mismatch");

   if (n == 0) {
      set(d);
      X.SetLength(0);
      return;
   }

   GF2X t1, t2;

   const GF2XModulus& G = GF2E::modulus();

   Vec< GF2XVec > M;

   M.SetLength(n);

   for (long i = 0; i < n; i++) {
      M[i].SetSize(n+1, 2*GF2E::WordLength());

      if (trans)
         for (long j = 0; j < n; j++) M[i][j] = rep(A[j][i]);
      else
         for (long j = 0; j < n; j++) M[i][j] = rep(A[i][j]);

      M[i][n] = rep(b[i]);
   }

   GF2X det;
   set(det);

   GF2Context GF2_context;
   GF2_context.save();
   double sz = GF2E_SizeInWords();

   for (long k = 0; k < n; k++) {
      long pos = -1;
      for (long i = k; i < n; i++) {
         rem(t1, M[i][k], G);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1)) {
            pos = i;
         }
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
         }

         MulMod(det, det, M[k][k], G);

         // make M[k, k] == -1 mod G, and make row k reduced

         InvMod(t1, M[k][k], G);
         negate(t1, t1);
         for (long j = k+1; j <= n; j++) {
            rem(t2, M[k][j], G);
            MulMod(M[k][j], t2, t1, G);
         }

         bool seq =
            double(n-(k+1))*(n-(k+1))*sz*sz < PAR_THRESH;

         NTL_GEXEC_RANGE(seq, n-(k+1), first, last)
         NTL_IMPORT(n)
         NTL_IMPORT(k)

         GF2_context.restore();

         GF2X t1, t2;

         for (long ii = first; ii < last; ii++) {
            long i = ii + k+1;

            // M[i] = M[i] + M[k]*M[i,k]

            t1 = M[i][k];   // this is already reduced

            GF2X *x = M[i].elts() + (k+1);
            GF2X *y = M[k].elts() + (k+1);

            for (long j = k+1; j <= n; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(*x, *x, t2);
            }
         }

         NTL_GEXEC_RANGE_END

      }
      else {
         clear(d);
         return;
      }
   }

   X.SetLength(n);
   for (long i = n-1; i >= 0; i--) {
      clear(t1);
      for (long j = i+1; j < n; j++) {
         mul(t2, rep(X[j]), M[i][j]);
         add(t1, t1, t2);
      }
      sub(t1, t1, M[i][n]);
      conv(X[i], t1);
   }

   conv(d, det);
}

void solve(GF2E& d, Vec<GF2E>& x,
               const Mat<GF2E>& A, const Vec<GF2E>& b)
{
   solve_impl(d, x, A, b, true);
}

void solve(GF2E& d, const Mat<GF2E>& A,
               Vec<GF2E>& x, const Vec<GF2E>& b)
{
   solve_impl(d, x, A, b, false);
}

long gauss(Mat<GF2E>& M_in, long w)
{
   GF2X t1, t2;
   GF2X piv;

   long n = M_in.NumRows();
   long m = M_in.NumCols();

   if (w < 0 || w > m)
      LogicError("gauss: bad args");

   const GF2XModulus& G = GF2E::modulus();

   Vec< GF2XVec > M;

   M.SetLength(n);
   for (long i = 0; i < n; i++) {
      M[i].SetSize(m, 2*GF2E::WordLength());
      for (long j = 0; j < m; j++) {
         M[i][j] = rep(M_in[i][j]);
      }
   }

   GF2Context GF2_context;
   GF2_context.save();
   double sz = GF2E_SizeInWords();

   long l = 0;
   for (long k = 0; k < w && l < n; k++) {

      long pos = -1;
      for (long i = l; i < n; i++) {
         rem(t1, M[i][k], G);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1)) {
            pos = i;
         }
      }

      if (pos != -1) {
         swap(M[pos], M[l]);

         InvMod(piv, M[l][k], G);
         negate(piv, piv);

         for (long j = k+1; j < m; j++) {
            rem(M[l][j], M[l][j], G);
         }

         bool seq =
            double(n-(l+1))*double(m-(k+1))*sz*sz < PAR_THRESH;

         NTL_GEXEC_RANGE(seq, n-(l+1), first, last)
         NTL_IMPORT(m)
         NTL_IMPORT(k)
         NTL_IMPORT(l)

         GF2_context.restore();

         GF2X t1, t2;

         for (long ii = first; ii < last; ii++) {
            long i = ii + l+1;

            // M[i] = M[i] + M[l]*M[i,k]*piv

            MulMod(t1, M[i][k], piv, G);

            clear(M[i][k]);

            GF2X *x = M[i].elts() + (k+1);
            GF2X *y = M[l].elts() + (k+1);

            for (long j = k+1; j < m; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(t2, t2, *x);
               *x = t2;
            }
         }

         NTL_GEXEC_RANGE_END

         l++;
      }
   }

   for (long i = 0; i < n; i++)
      for (long j = 0; j < m; j++)
         conv(M_in[i][j], M[i][j]);

   return l;
}

long gauss(Mat<GF2E>& M)
{
   return gauss(M, M.NumCols());
}

void image(Mat<GF2E>& X, const Mat<GF2E>& A)
{
   Mat<GF2E> M;
   M = A;
   long r = gauss(M);
   M.SetDims(r, M.NumCols());
   X = M;
}

void kernel(Mat<GF2E>& X, const Mat<GF2E>& A)
{
   long m = A.NumRows();
   long n = A.NumCols();

   const GF2XModulus& G = GF2E::modulus();

   Mat<GF2E> M;

   transpose(M, A);
   long r = gauss(M);

   if (r == 0) {
      ident(X, m);
      return;
   }

   X.SetDims(m-r, m);

   if (m-r == 0 || m == 0) return;

   Vec<long> D;
   D.SetLength(m);
   for (long j = 0; j < m; j++) D[j] = -1;

   Vec<GF2E> inverses;
   inverses.SetLength(m);

   for (long i = 0, j = -1; i < r; i++) {
      do {
         j++;
      } while (IsZero(M[i][j]));

      D[j] = i;
      inv(inverses[j], M[i][j]);
   }

   GF2EContext GF2E_context;
   GF2E_context.save();
   GF2Context GF2_context;
   GF2_context.save();
   double sz = GF2E_SizeInWords();

   bool seq =
      double(m-r)*double(r)*double(r)*sz*sz < PAR_THRESH;

   NTL_GEXEC_RANGE(seq, m-r, first, last)
   NTL_IMPORT(m)
   NTL_IMPORT(r)

   GF2_context.restore();
   GF2E_context.restore();

   GF2X t1, t2;
   GF2E T3;

   for (long k = first; k < last; k++) {
      Vec<GF2E>& v = X[k];
      long pos = 0;
      for (long j = m-1; j >= 0; j--) {
         if (D[j] == -1) {
            if (pos == k)
               set(v[j]);
            else
               clear(v[j]);
            pos++;
         }
         else {
            long i = D[j];

            clear(t1);

            for (long s = j+1; s < m; s++) {
               mul(t2, rep(v[s]), rep(M[i][s]));
               add(t1, t1, t2);
            }

            conv(T3, t1);
            mul(T3, T3, inverses[j]);
            negate(v[j], T3);
         }
      }
   }

   NTL_GEXEC_RANGE_END
}

void determinant(GF2E& d, const Mat<GF2E>& M_in)
{
   GF2X t1, t2;

   const GF2XModulus& G = GF2E::modulus();

   long n = M_in.NumRows();

   if (M_in.NumCols() != n)
      LogicError("determinant: nonsquare matrix");

   if (n == 0) {
      set(d);
      return;
   }

   Vec< GF2XVec > M;

   M.SetLength(n);
   for (long i = 0; i < n; i++) {
      M[i].SetSize(n, 2*GF2E::WordLength());
      for (long j = 0; j < n; j++) {
         M[i][j] = rep(M_in[i][j]);
      }
   }

   GF2X det;
   set(det);

   GF2Context GF2_context;
   GF2_context.save();
   double sz = GF2E_SizeInWords();

   for (long k = 0; k < n; k++) {
      long pos = -1;
      for (long i = k; i < n; i++) {
         rem(t1, M[i][k], G);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1))
            pos = i;
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
         }

         MulMod(det, det, M[k][k], G);

         // make M[k, k] == -1 mod G, and make row k reduced

         InvMod(t1, M[k][k], G);
         negate(t1, t1);
         for (long j = k+1; j < n; j++) {
            rem(t2, M[k][j], G);
            MulMod(M[k][j], t2, t1, G);
         }

         bool seq =
            double(n-(k+1))*(n-(k+1))*sz*sz < PAR_THRESH;

         NTL_GEXEC_RANGE(seq, n-(k+1), first, last)
         NTL_IMPORT(n)
         NTL_IMPORT(k)

         GF2_context.restore();

         GF2X t1, t2;

         for (long ii = first; ii < last; ii++) {
            long i = ii + k+1;

            // M[i] = M[i] + M[k]*M[i,k]

            t1 = M[i][k];   // this is already reduced

            GF2X *x = M[i].elts() + (k+1);
            GF2X *y = M[k].elts() + (k+1);

            for (long j = k+1; j < n; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(*x, *x, t2);
            }
         }

         NTL_GEXEC_RANGE_END

      }
      else {
         clear(d);
         return;
      }
   }

   conv(d, det);
}

//===========================================================

void add(mat_GF2E& X, const mat_GF2E& A, const mat_GF2E& B)
{
   long n = A.NumRows();
   long m = A.NumCols();

   if (B.NumRows() != n || B.NumCols() != m)
      LogicError("matrix add: dimension mismatch");

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         add(X(i,j), A(i,j), B(i,j));
}

static
void mul_aux(vec_GF2E& x, const mat_GF2E& A, const vec_GF2E& b)
{
   long n = A.NumRows();
   long l = A.NumCols();

   if (l != b.length())
      LogicError("matrix mul: dimension mismatch");

   x.SetLength(n);

   long i, k;
   GF2X acc, tmp;

   for (i = 1; i <= n; i++) {
      clear(acc);
      for (k = 1; k <= l; k++) {
         mul(tmp, rep(A(i,k)), rep(b(k)));
         add(acc, acc, tmp);
      }
      conv(x(i), acc);
   }
}

void mul(vec_GF2E& x, const mat_GF2E& A, const vec_GF2E& b)
{
   if (&b == &x || A.alias(x)) {
      vec_GF2E tmp;
      mul_aux(tmp, A, b);
      x = tmp;
   }
   else
      mul_aux(x, A, b);
}

static
void mul_aux(vec_GF2E& x, const vec_GF2E& a, const mat_GF2E& B)
{
   long n = B.NumRows();
   long l = B.NumCols();

   if (n != a.length())
      LogicError("matrix mul: dimension mismatch");

   x.SetLength(l);

   long i, k;
   GF2X acc, tmp;

   for (i = 1; i <= l; i++) {
      clear(acc);
      for (k = 1; k <= n; k++) {
         mul(tmp, rep(a(k)), rep(B(k,i)));
         add(acc, acc, tmp);
      }
      conv(x(i), acc);
   }
}

void mul(vec_GF2E& x, const vec_GF2E& a, const mat_GF2E& B)
{
   if (&a == &x) {
      vec_GF2E tmp;
      mul_aux(tmp, a, B);
      x = tmp;
   }
   else
      mul_aux(x, a, B);
}

void ident(mat_GF2E& X, long n)
{
   X.SetDims(n, n);
   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i == j)
            set(X(i, j));
         else
            clear(X(i, j));
}

long IsIdent(const mat_GF2E& A, long n)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i != j) {
            if (!IsZero(A(i, j))) return 0;
         }
         else {
            if (!IsOne(A(i, j))) return 0;
         }

   return 1;
}

void transpose(mat_GF2E& X, const mat_GF2E& A)
{
   long n = A.NumRows();
   long m = A.NumCols();

   long i, j;

   if (&X == & A) {
      if (n == m)
         for (i = 1; i <= n; i++)
            for (j = i+1; j <= n; j++)
               swap(X(i, j), X(j, i));
      else {
         mat_GF2E tmp;
         tmp.SetDims(m, n);
         for (i = 1; i <= n; i++)
            for (j = 1; j <= m; j++)
               tmp(j, i) = A(i, j);
         X.kill();
         X = tmp;
      }
   }
   else {
      X.SetDims(m, n);
      for (i = 1; i <= n; i++)
         for (j = 1; j <= m; j++)
            X(j, i) = A(i, j);
   }
}

void mul(mat_GF2E& X, const mat_GF2E& A, const GF2E& b_in)
{
   GF2E b = b_in;
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         mul(X[i][j], A[i][j], b);
}

void mul(mat_GF2E& X, const mat_GF2E& A, GF2 b)
{
   X = A;
   if (b == 0)
      clear(X);
}

void diag(mat_GF2E& X, long n, const GF2E& d_in)
{
   GF2E d = d_in;
   X.SetDims(n, n);
   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i == j)
            X(i, j) = d;
         else
            clear(X(i, j));
}

long IsDiag(const mat_GF2E& A, long n, const GF2E& d)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i != j) {
            if (!IsZero(A(i, j))) return 0;
         }
         else {
            if (A(i, j) != d) return 0;
         }

   return 1;
}

long IsZero(const mat_GF2E& a)
{
   long n = a.NumRows();
   long i;

   for (i = 0; i < n; i++)
      if (!IsZero(a[i]))
         return 0;

   return 1;
}

void clear(mat_GF2E& x)
{
   long n = x.NumRows();
   long i;
   for (i = 0; i < n; i++)
      clear(x[i]);
}

mat_GF2E operator+(const mat_GF2E& a, const mat_GF2E& b)
{
   mat_GF2E res;
   add(res, a, b);
   NTL_OPT_RETURN(mat_GF2E, res);
}

mat_GF2E operator*(const mat_GF2E& a, const mat_GF2E& b)
{
   mat_GF2E res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(mat_GF2E, res);
}

mat_GF2E operator-(const mat_GF2E& a, const mat_GF2E& b)
{
   mat_GF2E res;
   sub(res, a, b);
   NTL_OPT_RETURN(mat_GF2E, res);
}

mat_GF2E operator-(const mat_GF2E& a)
{
   mat_GF2E res;
   negate(res, a);
   NTL_OPT_RETURN(mat_GF2E, res);
}

vec_GF2E operator*(const mat_GF2E& a, const vec_GF2E& b)
{
   vec_GF2E res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(vec_GF2E, res);
}

vec_GF2E operator*(const vec_GF2E& a, const mat_GF2E& b)
{
   vec_GF2E res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(vec_GF2E, res);
}

void inv(mat_GF2E& X, const mat_GF2E& A)
{
   GF2E d;
   inv(d, X, A);
   if (d == 0) ArithmeticError("inv: non-invertible matrix");
}

void power(mat_GF2E& X, const mat_GF2E& A, const ZZ& e)
{
   if (A.NumRows() != A.NumCols()) LogicError("power: non-square matrix");

   if (e == 0) {
      ident(X, A.NumRows());
      return;
   }

   mat_GF2E T1, T2;
   long i, k;

   k = NumBits(e);
   T1 = A;

   for (i = k-2; i >= 0; i--) {
      sqr(T2, T1);
      if (bit(e, i))
         mul(T1, T2, A);
      else
         T1 = T2;
   }

   if (e < 0)
      inv(X, T1);
   else
      X = T1;
}

void random(mat_GF2E& x, long n, long m)
{
   x.SetDims(n, m);
   for (long i = 0; i < n; i++) random(x[i], m);
}

NTL_END_IMPL


#pragma once

#include <vector>
#include <boost/math/differentiation/autodiff.hpp>

namespace hmc {

template<class MODEL, class T>
void force_ad_1d(const MODEL& model, const std::vector<T>& x, std::vector<T> *f) {
  auto x0_fvar = boost::math::differentiation::make_fvar<double, 1>(x[0]);
  auto p = model.potential(x0_fvar);
  (*f)[0] = -p.derivative(1);
}

template<class MODEL, class T>
void force_ad_2d(const MODEL& model, const std::vector<T>& x, std::vector<T> *f) {
  auto const fvar = boost::math::differentiation::make_ftuple<double, 1, 1>(x[0], x[1]);
  auto const& x0_fvar = std::get<0>(fvar);
  auto const& x1_fvar = std::get<1>(fvar);
  auto p = model.potential(x0_fvar, x1_fvar);
  (*f)[0] = -p.derivative(1, 0);
  (*f)[1] = -p.derivative(0, 1);
}

}