// $Id$
//
//  Copyright (C) 2003-2008 Greg Landrum and Rational Discovery LLC
//
//   @@ All Rights Reserved @@
//  This file is part of the RDKit.
//  The contents are covered by the terms of the BSD license
//  which is included in the file license.txt, found at the root
//  of the RDKit source tree.
//
#include <GraphMol/ROMol.h>
#include <GraphMol/Atom.h>
#include <GraphMol/Bond.h>
#include <GraphMol/BondIterators.h>
#include <GraphMol/MolOps.h>
#include <memory>
#include <boost/shared_array.hpp>
#include <algorithm>
#include <cstring>

namespace RDKit {

const int LOCAL_INF = (int)1e8;

// local utility namespace
namespace {
/* ----------------------------------------------

This implements the Floyd-Warshall all-pairs-shortest-paths
algorithm as described on pg 564 of the White Book (Cormen, Leiserson, Rivest)

Arguments:
adjMat: the adjacency matrix.  This is overwritten with the shortest
paths, should be dim x dim
dim: the size of adjMat
pathMat: the path matrix, should be dim x dim

-----------------------------------------------*/
template <class T>
void FloydWarshall(int dim, T *adjMat, int *pathMat) {
  int k, i, j;
  T *currD, *lastD, *tTemp;
  int *currP, *lastP, *iTemp;

  currD = new T[dim * dim];
  currP = new int[dim * dim];
  lastD = new T[dim * dim];
  lastP = new int[dim * dim];

  memcpy(static_cast<void *>(lastD), static_cast<void *>(adjMat),
         dim * dim * sizeof(T));

  // initialize the paths
  for (i = 0; i < dim; i++) {
    int itab = i * dim;
    for (j = 0; j < dim; j++) {
      if (i == j || adjMat[itab + j] == LOCAL_INF) {
        pathMat[itab + j] = -1;
      } else {
        pathMat[itab + j] = i;
      }
    }
  }
  memcpy(static_cast<void *>(lastP), static_cast<void *>(pathMat),
         dim * dim * sizeof(int));

  for (k = 0; k < dim; k++) {
    int ktab = k * dim;
    for (i = 0; i < dim; i++) {
      int itab = i * dim;
      for (j = 0; j < dim; j++) {
        T v1 = lastD[itab + j];
        T v2 = lastD[itab + k] + lastD[ktab + j];
        if (v1 <= v2) {
          currD[itab + j] = v1;
          currP[itab + j] = lastP[itab + j];
        } else {
          currD[itab + j] = v2;
          currP[itab + j] = lastP[ktab + j];
        }
      }
    }
    tTemp = currD;
    currD = lastD;
    lastD = tTemp;

    iTemp = currP;
    currP = lastP;
    lastP = iTemp;
  }
  memcpy(static_cast<void *>(adjMat), static_cast<void *>(lastD),
         dim * dim * sizeof(T));
  memcpy(static_cast<void *>(pathMat), static_cast<void *>(lastP),
         dim * dim * sizeof(int));

  delete[] currD;
  delete[] currP;
  delete[] lastD;
  delete[] lastP;
}

template <class T>
void FloydWarshall(int dim, T *adjMat, int *pathMat,
                   const std::vector<int> &activeAtoms) {
  T *currD, *lastD, *tTemp;
  int *currP, *lastP, *iTemp;

  currD = new T[dim * dim];
  currP = new int[dim * dim];
  lastD = new T[dim * dim];
  lastP = new int[dim * dim];

  memcpy(static_cast<void *>(lastD), static_cast<void *>(adjMat),
         dim * dim * sizeof(T));

  // initialize the paths
  for (auto ai : activeAtoms) {
    int itab = ai * dim;
    for (int activeAtom : activeAtoms) {
      if (ai == activeAtom || adjMat[itab + activeAtom] == LOCAL_INF) {
        pathMat[itab + activeAtom] = -1;
      } else {
        pathMat[itab + activeAtom] = ai;
      }
    }
  }
  memcpy(static_cast<void *>(lastP), static_cast<void *>(pathMat),
         dim * dim * sizeof(int));

  for (auto ak : activeAtoms) {
    int ktab = ak * dim;
    for (auto ai : activeAtoms) {
      int itab = ai * dim;
      for (int activeAtom : activeAtoms) {
        T v1 = lastD[itab + activeAtom];
        T v2 = lastD[itab + ak] + lastD[ktab + activeAtom];
        if (v1 <= v2) {
          currD[itab + activeAtom] = v1;
          currP[itab + activeAtom] = lastP[itab + activeAtom];
        } else {
          currD[itab + activeAtom] = v2;
          currP[itab + activeAtom] = lastP[ktab + activeAtom];
        }
      }
    }
    tTemp = currD;
    currD = lastD;
    lastD = tTemp;

    iTemp = currP;
    currP = lastP;
    lastP = iTemp;
  }
  memcpy(static_cast<void *>(adjMat), static_cast<void *>(lastD),
         dim * dim * sizeof(T));
  memcpy(static_cast<void *>(pathMat), static_cast<void *>(lastP),
         dim * dim * sizeof(int));

  delete[] currD;
  delete[] currP;
  delete[] lastD;
  delete[] lastP;
}
}  // end of local utility namespace

namespace MolOps {
double *getDistanceMat(const ROMol &mol, bool useBO, bool useAtomWts,
                       bool force, const char *propNamePrefix) {
  std::string propName;
  boost::shared_array<double> sptr;
  if (propNamePrefix) {
    propName = propNamePrefix;
  } else {
    propName = "";
  }
  propName += "DistanceMatrix";
  // make sure we don't use the nonBO cache for the BO matrix and vice versa:
  if (useBO) {
    propName += "BO";
  }
  if (!force && mol.hasProp(propName)) {
    mol.getProp(propName, sptr);
    return sptr.get();
  }
  int nAts = mol.getNumAtoms();
  auto *dMat = new double[nAts * nAts];
  int i, j;
  // initialize off diagonals to LOCAL_INF and diagonals to 0
  for (i = 0; i < nAts * nAts; i++) {
    dMat[i] = LOCAL_INF;
  }
  for (i = 0; i < nAts; i++) {
    dMat[i * nAts + i] = 0.0;
  }

  ROMol::EDGE_ITER firstB, lastB;
  boost::tie(firstB, lastB) = mol.getEdges();
  while (firstB != lastB) {
    const Bond* bond = mol[*firstB];
    i = bond->getBeginAtomIdx();
    j = bond->getEndAtomIdx();
    double contrib;
    if (useBO) {
      if (!bond->getIsAromatic()) {
        contrib = 1. / bond->getBondTypeAsDouble();
      } else {
        contrib = 2. / 3.;
      }
    } else {
      contrib = 1.0;
    }
    dMat[i * nAts + j] = contrib;
    dMat[j * nAts + i] = contrib;
    ++firstB;
  }

  auto *pathMat = new int[nAts * nAts];
  memset(static_cast<void *>(pathMat), 0, nAts * nAts * sizeof(int));
  FloydWarshall(nAts, dMat, pathMat);

  if (useAtomWts) {
    for (i = 0; i < nAts; i++) {
      int anum = mol.getAtomWithIdx(i)->getAtomicNum();
      dMat[i * nAts + i] = 6.0 / anum;
    }
  }
  sptr.reset(dMat);
  mol.setProp(propName, sptr, true);
  boost::shared_array<int> iSptr(pathMat);
  mol.setProp(propName + "_Paths", iSptr, true);

  return dMat;
};

double *getDistanceMat(const ROMol &mol, const std::vector<int> &activeAtoms,
                       const std::vector<const Bond *> &bonds, bool useBO,
                       bool useAtomWts) {
  const int nAts = rdcast<int>(activeAtoms.size());

  auto *dMat = new double[nAts * nAts];
  int i, j;
  // initialize off diagonals to LOCAL_INF and diagonals to 0
  for (i = 0; i < nAts * nAts; i++) {
    dMat[i] = LOCAL_INF;
  }
  for (i = 0; i < nAts; i++) {
    dMat[i * nAts + i] = 0.0;
  }

  for (auto bond : bonds) {
    i = rdcast<int>(std::find(activeAtoms.begin(), activeAtoms.end(),
                              static_cast<int>(bond->getBeginAtomIdx())) -
                    activeAtoms.begin());
    j = rdcast<int>(std::find(activeAtoms.begin(), activeAtoms.end(),
                              static_cast<int>(bond->getEndAtomIdx())) -
                    activeAtoms.begin());
    double contrib;
    if (useBO) {
      if (!bond->getIsAromatic()) {
        contrib = 1. / bond->getBondTypeAsDouble();
      } else {
        contrib = 2. / 3.;
      }
    } else {
      contrib = 1.0;
    }
    dMat[i * nAts + j] = contrib;
    dMat[j * nAts + i] = contrib;
  }

  auto *pathMat = new int[nAts * nAts];
  memset(static_cast<void *>(pathMat), 0, nAts * nAts * sizeof(int));
  FloydWarshall(nAts, dMat, pathMat);
  delete[] pathMat;

  if (useAtomWts) {
    for (i = 0; i < nAts; i++) {
      int anum = mol.getAtomWithIdx(activeAtoms[i])->getAtomicNum();
      dMat[i * nAts + i] = 6.0 / anum;
    }
  }
  return dMat;
};

// NOTE: do *not* delete results
double *getAdjacencyMatrix(const ROMol &mol, bool useBO, int emptyVal,
                           bool force, const char *propNamePrefix,
                           const boost::dynamic_bitset<> *bondsToUse) {
  std::string propName;
  boost::shared_array<double> sptr;
  if (propNamePrefix) {
    propName = propNamePrefix;
  } else {
    propName = "";
  }
  propName += "AdjacencyMatrix";
  if (!force && mol.hasProp(propName)) {
    mol.getProp(propName, sptr);
    return sptr.get();
  }

  int nAts = mol.getNumAtoms();
  auto *res = new double[nAts * nAts];
  memset(static_cast<void *>(res), emptyVal, nAts * nAts * sizeof(double));

  for (ROMol::ConstBondIterator bondIt = mol.beginBonds();
       bondIt != mol.endBonds(); bondIt++) {
    if (bondsToUse && !(*bondsToUse)[(*bondIt)->getIdx()]) {
      continue;
    }
    if (!useBO) {
      int beg = (*bondIt)->getBeginAtomIdx();
      int end = (*bondIt)->getEndAtomIdx();
      res[beg * nAts + end] = 1;
      res[end * nAts + beg] = 1;
    } else {
      int begIdx = (*bondIt)->getBeginAtomIdx();
      int endIdx = (*bondIt)->getEndAtomIdx();
      Atom const *beg = mol.getAtomWithIdx(begIdx);
      Atom const *end = mol.getAtomWithIdx(endIdx);
      res[begIdx * nAts + endIdx] = (*bondIt)->getValenceContrib(beg);
      res[endIdx * nAts + begIdx] = (*bondIt)->getValenceContrib(end);
    }
  }
  sptr.reset(res);
  mol.setProp(propName, sptr, true);

  return res;
};

INT_LIST getShortestPath(const ROMol &mol, int aid1, int aid2) {
  int nats = mol.getNumAtoms();
  RANGE_CHECK(0, aid1, nats - 1);
  RANGE_CHECK(0, aid2, nats - 1);
  CHECK_INVARIANT(aid1 != aid2, "");

  INT_VECT pred(nats, -1);  // set all atoms to unprocessed state
  pred[aid1] = -2;          // marks begin
  pred[aid2] = -3;          // marks end

  std::deque<int> bfsQ;

  bfsQ.push_back(aid1);
  bool done = false;
  ROMol::ADJ_ITER nbrIdx, endNbrs;
  while ((!done) && (bfsQ.size() > 0)) {
    int curAid = bfsQ.front();
    boost::tie(nbrIdx, endNbrs) =
        mol.getAtomNeighbors(mol.getAtomWithIdx(curAid));
    while (!done && nbrIdx != endNbrs) {
      switch (pred[*nbrIdx]) {
        case -1:
          pred[*nbrIdx] = curAid;
          bfsQ.push_back(rdcast<int>(*nbrIdx));
          break;
        case -3:  // end found
          pred[*nbrIdx] = curAid;
          done = true;
          break;
        default:  // already processed (or begin)
          break;
      }
      ++nbrIdx;
    }
    bfsQ.pop_front();
  }

  INT_LIST res;
  if (done) {
    done = false;
    int prev = aid2;
    res.push_back(aid2);
    while (!done) {
      prev = pred[prev];
      if (prev != aid1) {
        res.push_front(prev);
      } else {
        done = true;
      }
    }
    res.push_front(aid1);
  }
  return res;
}

double *get3DDistanceMat(const ROMol &mol, int confId, bool useAtomWts,
                         bool force, const char *propNamePrefix) {
  const Conformer &conf = mol.getConformer(confId);
  std::string propName;
  boost::shared_array<double> sptr;
  if (propNamePrefix) {
    propName = propNamePrefix;
  } else {
    propName = "_";
  }
  if (propName != "") {
    propName += "3DDistanceMatrix_Conf" + std::to_string(conf.getId());
    if (!force && mol.hasProp(propName)) {
      mol.getProp(propName, sptr);
      return sptr.get();
    }
  }

  unsigned int nAts = mol.getNumAtoms();
  auto *dMat = new double[nAts * nAts];

  for (unsigned int i = 0; i < nAts; ++i) {
    if (useAtomWts) {
      dMat[i * nAts + i] = 6.0 / mol.getAtomWithIdx(i)->getAtomicNum();
    } else {
      dMat[i * nAts + i] = 0.0;
    }
    for (unsigned int j = i + 1; j < nAts; ++j) {
      double dist = (conf.getAtomPos(i) - conf.getAtomPos(j)).length();
      dMat[i * nAts + j] = dist;
      dMat[j * nAts + i] = dist;
    }
  }

  if (propName != "") {
    sptr.reset(dMat);
    mol.setProp(propName, sptr, true);
  }
  return dMat;
}
}  // end of namespace MolOps
}  // end of namespace RDKit


#include "drakeGeometryUtil.h"
#include <iostream>
#include <cmath>
#include <limits>
#include <stdexcept>
#include <Eigen/Sparse>

using namespace Eigen;

double angleDiff(double phi1, double phi2)
{
  double d = phi2-phi1;
  if(d>0.0)
  {
    d = fmod(d+M_PI,2*M_PI)-M_PI;
  }
  else
  {
    d = fmod(d-M_PI,2*M_PI)+M_PI;
  }
  return d;
}

Vector4d quatConjugate(const Eigen::Vector4d& q)
{
  Vector4d q_conj;
  q_conj << q(0), -q(1), -q(2), -q(3);
  return q_conj;
}

Eigen::Matrix4d dquatConjugate()
{
  Matrix4d dq_conj = Matrix4d::Identity();
  dq_conj(1, 1) = -1.0;
  dq_conj(2, 2) = -1.0;
  dq_conj(3, 3) = -1.0;
  return dq_conj;
}

Eigen::Vector4d quatProduct(const Eigen::Vector4d& q1, const Eigen::Vector4d& q2)
{
  double w1 = q1(0);
  double w2 = q2(0);
  const auto& v1 = q1.tail<3>();
  const auto& v2 = q2.tail<3>();
  Vector4d r;
  r << w1 * w2 - v1.dot(v2), v1.cross(v2) + w1 * v2 + w2 * v1;
  return r;
}

Eigen::Matrix<double, 4, 8> dquatProduct(const Eigen::Vector4d& q1, const Eigen::Vector4d& q2)
{
  double w1 = q1(0);
  double w2 = q2(0);
  const auto& v1 = q1.tail<3>();
  const auto& v2 = q2.tail<3>();

  Matrix<double, 4, 8> dr;
  dr.row(0) << w2, -v2.transpose(), w1, -v1.transpose();
  dr.row(1) << q2(1), q2(0), q2(3), -q2(2), q1(1), q1(0), -q1(3), q1(2);
  dr.row(2) << q2(2), -q2(3), q2(0), q2(1), q1(2), q1(3), q1(0), -q1(1);
  dr.row(3) << q2(3), q2(2), -q2(1), q2(0), q1(3), -q1(2), q1(1), q1(0);
  return dr;
}

Eigen::Vector3d quatRotateVec(const Eigen::Vector4d& q, const Eigen::Vector3d& v)
{
  Vector4d v_quat;
  v_quat << 0, v;
  Vector4d q_times_v = quatProduct(q, v_quat);
  Vector4d q_conj = quatConjugate(q);
  Vector4d v_rot = quatProduct(q_times_v, q_conj);
  Vector3d r = v_rot.bottomRows<3>();
  return r;
}

Eigen::Matrix<double, 3, 7> dquatRotateVec(const Eigen::Vector4d& q, const Eigen::Vector3d& v)
{
  Matrix<double, 4, 7> dq;
  dq << Matrix4d::Identity(), MatrixXd::Zero(4, 3);
  Matrix<double, 4, 7> dv = Matrix<double, 4, 7>::Zero();
  dv.bottomRightCorner<3, 3>() = Matrix3d::Identity();
  Matrix<double, 8, 7> dqdv;
  dqdv << dq, dv;

  Vector4d v_quat;
  v_quat << 0, v;
  Vector4d q_times_v = quatProduct(q, v_quat);
  Matrix<double, 4, 8> dq_times_v_tmp = dquatProduct(q, v_quat);
  Matrix<double, 4, 7> dq_times_v = dq_times_v_tmp * dqdv;

  Matrix<double, 4, 7> dq_conj = dquatConjugate() * dq;
  Matrix<double, 8, 7> dq_times_v_dq_conj;
  dq_times_v_dq_conj << dq_times_v, dq_conj;
  Matrix<double, 4, 8> dv_rot_tmp = dquatProduct(q_times_v, quatConjugate(q));
  Matrix<double, 4, 7> dv_rot = dv_rot_tmp * dq_times_v_dq_conj;
  Eigen::Matrix<double, 3, 7> dr = dv_rot.bottomRows(3);
  return dr;
}

Eigen::Vector4d quatDiff(const Eigen::Vector4d& q1, const Eigen::Vector4d& q2)
{
  return quatProduct(quatConjugate(q1), q2);
}

Eigen::Matrix<double, 4, 8> dquatDiff(const Eigen::Vector4d& q1, const Eigen::Vector4d& q2)
{
  auto dr = dquatProduct(quatConjugate(q1), q2);
  dr.block<4, 3>(0, 1) = -dr.block<4, 3>(0, 1);
  return dr;
}

double quatDiffAxisInvar(const Eigen::Vector4d& q1, const Eigen::Vector4d& q2, const Eigen::Vector3d& u)
{
  Vector4d r = quatDiff(q1, q2);
  double e = -2.0 + 2 * r(0) * r(0) + 2 * pow(u(0) * r(1) + u(1) * r(2) + u(2) * r(3), 2);
  return e;
}

Eigen::Matrix<double, 1, 11> dquatDiffAxisInvar(const Eigen::Vector4d& q1, const Eigen::Vector4d& q2, const Eigen::Vector3d& u)
{
  Vector4d r = quatDiff(q1, q2);
  Matrix<double, 4, 8> dr = dquatDiff(q1, q2);
  Matrix<double, 1, 11> de;
  const auto& rvec = r.tail<3>();
  de << 4.0 * r(0) * dr.row(0) + 4.0 * u.transpose() * rvec *u.transpose() * dr.block<3, 8>(1, 0), 4.0 * u.transpose() * rvec * rvec.transpose();
  return de;
}

double quatNorm(const Eigen::Vector4d& q)
{
  return std::acos(q(0));
}

Eigen::Vector4d slerp(const Eigen::Vector4d& q1, const Eigen::Vector4d& q2, double interpolation_parameter)
{
  /*
   * Q = slerp(q1, q2, f) Spherical linear interpolation between two quaternions
   *   This function uses the implementation given in Algorithm 8 of [1].
   *
   * @param q1   Initial quaternion (w, x, y, z)
   * @param q2   Final quaternion (w, x, y, z)
   * @param f    Interpolation parameter between 0 and 1 (inclusive)
   * @retval Q   Interpolated quaternion(s). 4-by-1 vector.
   *
   * [1] Kuffner, J.J., "Effective sampling and distance metrics for 3D rigid
   * body path planning," Robotics and Automation, 2004. Proceedings. ICRA '04.
   * 2004 IEEE International Conference on , vol.4, no., pp.3993,3998 Vol.4,
   * April 26-May 1, 2004
   * doi: 10.1109/ROBOT.2004.1308895
   */

  // Compute the quaternion inner product
  double lambda = (q1.transpose() * q2).value();
  int q2_sign;
  if (lambda < 0.0) {
    // The quaternions are pointing in opposite directions, so use the equivalent alternative representation for q2
    lambda = -lambda;
    q2_sign = -1;
  }
  else {
    q2_sign = 1;
  }

  // Calculate interpolation factors
  // TODO: do we really want an epsilon so small?
  double r, s;
  if (std::abs(1.0 - lambda) < std::numeric_limits<double>::epsilon()) {
    // The quaternions are nearly parallel, so use linear interpolation
    r = 1.0 - interpolation_parameter;
    s = interpolation_parameter;
  }
  else {
    double alpha = std::acos(lambda);
    double gamma = 1.0 / std::sin(alpha);
    r = std::sin((1.0 - interpolation_parameter) * alpha) * gamma;
    s = std::sin(interpolation_parameter * alpha) * gamma;
  }

  Vector4d ret = q1 * r;
  ret += q2_sign * q2 * s;
  return ret;
}

Vector4d uniformlyRandomAxisAngle(std::default_random_engine& generator)
{
  std::normal_distribution<double> normal;
  std::uniform_real_distribution<double> uniform(-M_PI, M_PI);
  double angle = uniform(generator);
  Vector3d axis = Vector3d(normal(generator), normal(generator), normal(generator));
  axis.normalize();
  Vector4d a;
  a << axis, angle;
  return a;
}

Vector4d uniformlyRandomQuat(std::default_random_engine& generator)
{
  return axis2quat(uniformlyRandomAxisAngle(generator));
}

Eigen::Matrix3d uniformlyRandomRotmat(std::default_random_engine& generator)
{
  return axis2rotmat(uniformlyRandomAxisAngle(generator));
}

Eigen::Vector3d uniformlyRandomRPY(std::default_random_engine& generator)
{
  return axis2rpy(uniformlyRandomAxisAngle(generator));
}

DLLEXPORT int rotationRepresentationSize(int rotation_type)
{
  switch (rotation_type) {
    case 0:
      return 0;
      break;
    case 1:
      return 3;
      break;
    case 2:
      return 4;
      break;
    default:
      throw std::runtime_error("rotation representation type not recognized");
  }
}

Matrix3d rotz(double theta) {
  // returns 3D rotation matrix (about the z axis)
  Matrix3d M;
  double c=cos(theta);
  double s=sin(theta);
  M << c,-s, 0,
      s, c, 0,
      0, 0, 1;
  return M;
}

void rotz(double theta, Matrix3d &M, Matrix3d &dM, Matrix3d &ddM)
{
  double c=cos(theta), s=sin(theta);
  M << c,-s,0, s,c,0, 0,0,1;
  dM << -s,-c,0, c,-s,0, 0,0,0;
  ddM << -c,s,0, -s,-c,0, 0,0,0;
}

DLLEXPORT GradientVar<double,3,1> quat2expmap(const Ref<const Vector4d> &q, int gradient_order)
{
  double t = sqrt(1-q(0)*q(0));
  bool is_degenerate=(t*t<std::numeric_limits<double>::epsilon());
  double s = is_degenerate?2.0:2.0*acos(q(0))/t;
  GradientVar<double,3,1> ret(3,1,4,gradient_order);
  ret.value() = s*q.tail(3);
  if(gradient_order>0)
  {
    ret.gradient().value() = Matrix<double,3,4>::Zero();
    double dsdq1 = is_degenerate?0.0: (-2*t+2*acos(q(0))*q(0))/pow(t,3);
    ret.gradient().value().col(0) = q.tail(3)*dsdq1;
    ret.gradient().value().block(0,1,3,3) = Matrix3d::Identity()*s;
  }
  else if(gradient_order>1)
  {
    throw std::runtime_error("gradient_order>1 is not supported in quat2expmap");
  }
  return ret;
}

DLLEXPORT GradientVar<double,3,1> flipExpmap(const Ref<const Vector3d> &expmap, int gradient_order)
{
  if(gradient_order>1)
  {
    throw std::runtime_error("gradient_order>1 is not supported in flipExpmap");
  }
  double expmap_norm = expmap.norm();
  bool is_degenerate=(expmap_norm<std::numeric_limits<double>::epsilon());
  GradientVar<double,3,1> ret(3,1,3,gradient_order);
  Matrix3d eye3 = Matrix3d::Identity();
  if(is_degenerate)
  {
    ret.value() = expmap;
    if(gradient_order>0)
    {
      ret.gradient().value() = eye3;
    }
  }
  else
  {
    ret.value() = expmap-expmap/expmap_norm*2*M_PI;
    if(gradient_order>0)
    {
      ret.gradient().value() = eye3-(expmap_norm*expmap_norm*eye3-expmap*expmap.transpose())/pow(expmap_norm,3)*2*M_PI;
    }
  }
  return ret;
}

DLLEXPORT GradientVar<double, 3,1> unwrapExpmap(const Ref<const Vector3d> & expmap1, const Ref<const Vector3d> &expmap2, int gradient_order)
{
  auto expmap2_flip = flipExpmap(expmap2,gradient_order);
  double distance1 = (expmap1-expmap2).squaredNorm();
  double distance2 = (expmap1-expmap2_flip.value()).squaredNorm();
  if(distance1>distance2)
  {
    return expmap2_flip;
  }
  else
  {
    GradientVar<double,3,1> ret(3,1,3,gradient_order);
    ret.value() = expmap2;
    if(gradient_order>0)
    {
      ret.gradient().value() = Matrix3d::Identity();
    }
    return ret;
  }
}

void quat2expmapSequence(const Ref<const Matrix<double,4,Dynamic>> &quat, const Ref<const Matrix<double,4,Dynamic>> &quat_dot, Ref<Matrix<double,3,Dynamic>> expmap, Ref<Matrix<double,3,Dynamic>> expmap_dot)
{
  DenseIndex N = quat.cols();
  if(quat_dot.cols() != N)
  {
    throw std::runtime_error("quat_dot must have the same number of columns as quat in quat2expmapSequence");
  }
  expmap.resize(3,N);
  expmap_dot.resize(3,N);
  for(int i = 0;i<N;i++)
  {
    auto expmap_grad = quat2expmap(quat.col(i),1);
    expmap.col(i) = expmap_grad.value();
    expmap_dot.col(i) = expmap_grad.gradient().value()*quat_dot.col(i);
    if(i>=1)
    {
      auto closest_grad = closestExpmap(expmap.col(i-1),expmap.col(i),1);
      expmap.col(i) = closest_grad.value();
      expmap_dot.col(i) = closest_grad.gradient().value()*expmap_dot.col(i);
    }
  }
}

DLLEXPORT GradientVar<double, 3,1> closestExpmap(const Ref<const Vector3d> & expmap1, const Ref<const Vector3d> &expmap2, int gradient_order)
{
  if (gradient_order>1) {
    throw std::runtime_error("closestExpmap only supports first order gradient");
  }
  double expmap1_norm = expmap1.norm();
  double expmap2_norm = expmap2.norm();
  GradientVar<double, 3, 1> ret(3,1,3,gradient_order);
  if (expmap2_norm < std::numeric_limits<double>::epsilon()) {
    if (expmap1_norm > std::numeric_limits<double>::epsilon()) {
      Vector3d expmap1_axis = expmap1/expmap1_norm;
      int expmap1_round = static_cast<int>(expmap1_norm/(2*M_PI) + 0.5);
      ret.value() = expmap1_axis*expmap1_round*2*M_PI;
      if(ret.hasGradient()) {
        ret.gradient().value() = Matrix3d::Zero();
      }
      return ret;
    }
    else {
      ret.value() = expmap2;
      if (ret.hasGradient()) {
        ret.gradient().value() = Matrix3d::Identity();
      }
    }
  }
  else {
    Vector3d expmap2_axis = expmap2/expmap2_norm;
    Matrix3d dexpmap2_axis_dexpmap2 = (expmap2_norm*Matrix3d::Identity() - expmap2*expmap2.transpose()/expmap2_norm)/pow(expmap2_norm,2);
    double expmap2_closest_k = (expmap2_axis.transpose()*expmap1 - expmap2_norm)/(2*M_PI);
    int expmap2_closest_k1;
    int expmap2_closest_k2;
    if (expmap2_closest_k>0) {
      expmap2_closest_k1 = (int) expmap2_closest_k;
    }
    else {
      expmap2_closest_k1 = (int) expmap2_closest_k - 1;
    }
    expmap2_closest_k2 = expmap2_closest_k1 + 1;
    Vector3d expmap2_closest1 = expmap2 + 2*expmap2_closest_k1*M_PI*expmap2_axis;
    Vector3d expmap2_closest2 = expmap2 + 2*expmap2_closest_k2*M_PI*expmap2_axis;
    if ((expmap2_closest1 - expmap1).norm() < (expmap2_closest2 - expmap1).norm()) {
      ret.value() = expmap2_closest1;
      if (ret.hasGradient()) {
        ret.gradient().value() = Matrix3d::Identity() + 2*dexpmap2_axis_dexpmap2*(double)expmap2_closest_k1*M_PI;
      }
      return ret;
    }
    else {
      ret.value() = expmap2_closest2;
      if (ret.hasGradient()) {
        ret.gradient().value() = Matrix3d::Identity() + 2*dexpmap2_axis_dexpmap2*(double)expmap2_closest_k2*M_PI;
      }
      return ret;
    }
  }
  return ret;
}


#include <iostream>
#include <Eigen/Dense>

#include "LSLOpt/BFGS.hpp"
#include "ModelSystem.hpp"

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

  // start with gamma = 67.5 deg
  Eigen::VectorXd x0 = Eigen::VectorXd::Constant(1, 3*M_PI/8);

  LSLOpt::OptimizationParameters<double> params
      = LSLOpt::getOptimizationParameters<double>();
  LSLOpt::OstreamOutput output{LSLOpt::OutputLevel::Status, std::cout};

  auto result = LSLOpt::lsl_bfgs(modelSystem, x0, params, output);

  // access the optimal parameters
  std::cout << "gamma: " << result.x[0] << std::endl;
  // access the final function value
  std::cout << "score: " << result.function_value << std::endl;

  return 0;
}


#include <bitset>
#include <sstream>
#include <string>
#include <thread>
#include <boost/beast/core.hpp>
#include <boost/beast/websocket.hpp>
#include <boost/asio/ip/tcp.hpp>
#include <math_nerd/hill_cipher.h>

#include "file_handler.h"

namespace beast = boost::beast;
namespace http = beast::http;
namespace websocket = beast::websocket;
namespace net = boost::asio;
namespace mod = math_nerd::int_mod;
namespace matrix = math_nerd::matrix_t;
namespace hc = math_nerd::hill_cipher;

using tcp = net::ip::tcp;

auto operator<<(std::ostream &os, hc::hill_key key)->std::ostream &;

auto do_session(tcp::socket socket) -> void
{
    websocket::stream<tcp::socket> ws{ std::move(socket) };

    ws.set_option(websocket::stream_base::decorator(
        [](websocket::response_type &res)
        {
            res.set(http::field::server,
                "Hill Cipher");
        }));

    ws.accept();

    auto key_size{ 5 };
    std::unique_ptr<hc::hill_key> key{ new hc::hill_key{ key_size } };

    while( true )
    {
        beast::flat_buffer input;

        ws.read(input);

        auto cmd{ beast::buffers_to_string(input.data()) };

        std::bitset<3> cmd_type{ 0x0 };

        enum command
        {
            generate_key,
            encrypt,
            decrypt
        };

        std::string output;
        output.resize(100);
        output = "Error.";

        switch( cmd[0] )
        {
            case 'g':
            {
                if( cmd[1] >= '0' && cmd[1] <= '9' )
                {
                    key_size = std::max(cmd[1] - '0', 2);
                    cmd_type[command::generate_key] = true;
                }
            }
            break;

            case 'e':
            {
                cmd_type[command::encrypt] = true;
                cmd.erase(0, 1);
            }
            break;

            case 'd':
            {
                cmd_type[command::decrypt] = true;
                cmd.erase(0, 1);
            }
            break;

            default:
            {
                // Empty.
            }
        }

        if( cmd_type[command::generate_key] )
        {
            if( key_size != key->row_count() )
            {
                key.reset(new hc::hill_key{ key_size });
            }

            output = "g" + std::to_string(key_size);

            // Create our (not cryptographically secure) PRNG.
            std::random_device device;
            std::mt19937 rng(device());

            // Key distribution.
            std::uniform_int_distribution<int> key_dist(0, 96);

            // Index distribution for fixing invalid keys.
            std::uniform_int_distribution<int> idx_dist(0, key_size);

            for( auto i{ 0 }; i < key_size; ++i )
            {
                for( auto j{ 0 }; j < key_size; ++j )
                {
                    // Create (not cryptographically secure) random elements.
                    (*key)[i][j] = key_dist(rng);
                }
            }

            while( not hc::is_valid_key((*key)) )
            {   // Randomly touch parts of the key to attempt to fix.
                (*key)[idx_dist(rng)][idx_dist(rng)] += key_dist(rng);
            }

            std::stringstream os;

            os << (*key);

            output += os.str();
        }
        else if( cmd_type[command::encrypt] )
        {
            output = "e" + hc::encrypt((*key), cmd);
        }
        else if( cmd_type[command::decrypt] )
        {
            output = "d" + hc::decrypt((*key), cmd);
        }

        beast::multi_buffer buffer;
        auto msg = net::buffer_copy(buffer.prepare(output.size()), net::buffer(output));
        buffer.commit(msg);

        ws.text(ws.got_text());
        ws.write(buffer.data());
    }
}

auto main() -> int
try
{
    auto const address{ net::ip::make_address("127.0.0.1") };
    auto const port{ static_cast<std::uint16_t>(31337) };

    file_handler file;

    net::io_context ioc{ 1 };

    tcp::acceptor acceptor{ ioc, {address, port} };

    tcp::socket socket{ ioc };

    acceptor.accept(socket);

    do_session(std::move(socket));

    return EXIT_SUCCESS;
}
catch( std::exception const &e )
{
    if( std::string(e.what()) == "The WebSocket stream was gracefully closed at both endpoints" )
    {
        return EXIT_SUCCESS;
    }

    std::cerr << e.what() << std::endl;
    return EXIT_FAILURE;
}

auto operator<<(std::ostream &os, hc::hill_key key) -> std::ostream &
{
    // Starting bracket for key.
    os << "[";

    for( auto i{ 0 }; i < key.row_count(); ++i )
    {
        for( auto j{ 0 }; j < key.column_count(); ++j )
        {
            // Print element.
            os << key[i][j];

            if( j != key.column_count() - 1 )
            {
                // Delimit with comma.
                os << ",";
            }
        }

        if( i != key.row_count() - 1 )
        {   // Delimit row with semicolon.
            os << ";";
        }
    }

    // Ending bracket for key.
    os << "]";

    return os;
}


// Copyright (c) 2018 by University Paris-Est Marne-la-Vallee
// InnerExplicit.hpp
// This file is part of the Garamon for c3ga.
// Authors: Stephane Breuils and Vincent Nozick
// Contact: vincent.nozick@u-pem.fr
//
// Licence MIT
// A a copy of the MIT License is given along with this program

/// \file InnerExplicit.hpp
/// \author Stephane Breuils, Vincent Nozick
/// \brief Explicit precomputed per grades inner products of c3ga.

#ifndef C3GA_INNER_PRODUCT_EXPLICIT_HPP__
#define C3GA_INNER_PRODUCT_EXPLICIT_HPP__
#pragma once

#include <Eigen/Core>

#include "c3ga/Mvec.hpp"
#include "c3ga/Inner.hpp"
#include "c3ga/Constants.hpp"

/*!
 * @namespace c3ga
 */
namespace c3ga {
    template<typename T> class Mvec;

    /// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 0) and mv2 (grade 0).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 0 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 0 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 0
	template<typename T>
	void inner_0_0(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 0) and mv2 (grade 1).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 0 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 1 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void inner_0_1(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1.coeff(0)*mv2;
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 0) and mv2 (grade 2).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 0 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 2 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 2
	template<typename T>
	void inner_0_2(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1.coeff(0)*mv2;
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 0) and mv2 (grade 3).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 0 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 3 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 3
	template<typename T>
	void inner_0_3(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1.coeff(0)*mv2;
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 0) and mv2 (grade 4).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 0 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 4 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 4
	template<typename T>
	void inner_0_4(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1.coeff(0)*mv2;
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 0) and mv2 (grade 5).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 0 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 5 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 5
	template<typename T>
	void inner_0_5(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1.coeff(0)*mv2;
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 1) and mv2 (grade 0).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 1 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 0 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void inner_1_0(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 1) and mv2 (grade 1).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 1 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 1 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 0
	template<typename T>
	void inner_1_1(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(0)*mv2.coeff(4) + mv1.coeff(1)*mv2.coeff(1) + mv1.coeff(2)*mv2.coeff(2) + mv1.coeff(3)*mv2.coeff(3) - mv1.coeff(4)*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 1) and mv2 (grade 2).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 1 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 2 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void inner_1_2(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(3) - mv1.coeff(1)*mv2.coeff(0) - mv1.coeff(2)*mv2.coeff(1) - mv1.coeff(3)*mv2.coeff(2);
		mv3.coeffRef(1) +=  mv1.coeff(0)*mv2.coeff(6) - mv1.coeff(2)*mv2.coeff(4) - mv1.coeff(3)*mv2.coeff(5) - mv1.coeff(4)*mv2.coeff(0);
		mv3.coeffRef(2) +=  mv1.coeff(0)*mv2.coeff(8) + mv1.coeff(1)*mv2.coeff(4) - mv1.coeff(3)*mv2.coeff(7) - mv1.coeff(4)*mv2.coeff(1);
		mv3.coeffRef(3) +=  mv1.coeff(0)*mv2.coeff(9) + mv1.coeff(1)*mv2.coeff(5) + mv1.coeff(2)*mv2.coeff(7) - mv1.coeff(4)*mv2.coeff(2);
		mv3.coeffRef(4) +=  mv1.coeff(1)*mv2.coeff(6) + mv1.coeff(2)*mv2.coeff(8) + mv1.coeff(3)*mv2.coeff(9) - mv1.coeff(4)*mv2.coeff(3);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 1) and mv2 (grade 3).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 1 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 3 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 2
	template<typename T>
	void inner_1_3(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(0)*mv2.coeff(2) + mv1.coeff(2)*mv2.coeff(0) + mv1.coeff(3)*mv2.coeff(1);
		mv3.coeffRef(1) += -mv1.coeff(0)*mv2.coeff(4) - mv1.coeff(1)*mv2.coeff(0) + mv1.coeff(3)*mv2.coeff(3);
		mv3.coeffRef(2) += -mv1.coeff(0)*mv2.coeff(5) - mv1.coeff(1)*mv2.coeff(1) - mv1.coeff(2)*mv2.coeff(3);
		mv3.coeffRef(3) += -mv1.coeff(1)*mv2.coeff(2) - mv1.coeff(2)*mv2.coeff(4) - mv1.coeff(3)*mv2.coeff(5);
		mv3.coeffRef(4) += -mv1.coeff(0)*mv2.coeff(7) + mv1.coeff(3)*mv2.coeff(6) - mv1.coeff(4)*mv2.coeff(0);
		mv3.coeffRef(5) += -mv1.coeff(0)*mv2.coeff(8) - mv1.coeff(2)*mv2.coeff(6) - mv1.coeff(4)*mv2.coeff(1);
		mv3.coeffRef(6) += -mv1.coeff(2)*mv2.coeff(7) - mv1.coeff(3)*mv2.coeff(8) - mv1.coeff(4)*mv2.coeff(2);
		mv3.coeffRef(7) += -mv1.coeff(0)*mv2.coeff(9) + mv1.coeff(1)*mv2.coeff(6) - mv1.coeff(4)*mv2.coeff(3);
		mv3.coeffRef(8) +=  mv1.coeff(1)*mv2.coeff(7) - mv1.coeff(3)*mv2.coeff(9) - mv1.coeff(4)*mv2.coeff(4);
		mv3.coeffRef(9) +=  mv1.coeff(1)*mv2.coeff(8) + mv1.coeff(2)*mv2.coeff(9) - mv1.coeff(4)*mv2.coeff(5);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 1) and mv2 (grade 4).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 1 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 4 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 3
	template<typename T>
	void inner_1_4(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(1) - mv1.coeff(3)*mv2.coeff(0);
		mv3.coeffRef(1) +=  mv1.coeff(0)*mv2.coeff(2) + mv1.coeff(2)*mv2.coeff(0);
		mv3.coeffRef(2) +=  mv1.coeff(2)*mv2.coeff(1) + mv1.coeff(3)*mv2.coeff(2);
		mv3.coeffRef(3) +=  mv1.coeff(0)*mv2.coeff(3) - mv1.coeff(1)*mv2.coeff(0);
		mv3.coeffRef(4) += -mv1.coeff(1)*mv2.coeff(1) + mv1.coeff(3)*mv2.coeff(3);
		mv3.coeffRef(5) += -mv1.coeff(1)*mv2.coeff(2) - mv1.coeff(2)*mv2.coeff(3);
		mv3.coeffRef(6) +=  mv1.coeff(0)*mv2.coeff(4) - mv1.coeff(4)*mv2.coeff(0);
		mv3.coeffRef(7) +=  mv1.coeff(3)*mv2.coeff(4) - mv1.coeff(4)*mv2.coeff(1);
		mv3.coeffRef(8) += -mv1.coeff(2)*mv2.coeff(4) - mv1.coeff(4)*mv2.coeff(2);
		mv3.coeffRef(9) +=  mv1.coeff(1)*mv2.coeff(4) - mv1.coeff(4)*mv2.coeff(3);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 1) and mv2 (grade 5).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 1 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 5 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 4
	template<typename T>
	void inner_1_5(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(0)*mv2.coeff(0);
		mv3.coeffRef(1) += -mv1.coeff(3)*mv2.coeff(0);
		mv3.coeffRef(2) +=  mv1.coeff(2)*mv2.coeff(0);
		mv3.coeffRef(3) += -mv1.coeff(1)*mv2.coeff(0);
		mv3.coeffRef(4) += -mv1.coeff(4)*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 2) and mv2 (grade 0).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 2 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 0 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 2
	template<typename T>
	void inner_2_0(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 2) and mv2 (grade 1).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 2 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 1 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void inner_2_1(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(1) + mv1.coeff(1)*mv2.coeff(2) + mv1.coeff(2)*mv2.coeff(3) - mv1.coeff(3)*mv2.coeff(0);
		mv3.coeffRef(1) +=  mv1.coeff(0)*mv2.coeff(4) + mv1.coeff(4)*mv2.coeff(2) + mv1.coeff(5)*mv2.coeff(3) - mv1.coeff(6)*mv2.coeff(0);
		mv3.coeffRef(2) +=  mv1.coeff(1)*mv2.coeff(4) - mv1.coeff(4)*mv2.coeff(1) + mv1.coeff(7)*mv2.coeff(3) - mv1.coeff(8)*mv2.coeff(0);
		mv3.coeffRef(3) +=  mv1.coeff(2)*mv2.coeff(4) - mv1.coeff(5)*mv2.coeff(1) - mv1.coeff(7)*mv2.coeff(2) - mv1.coeff(9)*mv2.coeff(0);
		mv3.coeffRef(4) +=  mv1.coeff(3)*mv2.coeff(4) - mv1.coeff(6)*mv2.coeff(1) - mv1.coeff(8)*mv2.coeff(2) - mv1.coeff(9)*mv2.coeff(3);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 2) and mv2 (grade 2).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 2 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 2 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 0
	template<typename T>
	void inner_2_2(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(0)*mv2.coeff(6) - mv1.coeff(1)*mv2.coeff(8) - mv1.coeff(2)*mv2.coeff(9) + mv1.coeff(3)*mv2.coeff(3) - mv1.coeff(4)*mv2.coeff(4) - mv1.coeff(5)*mv2.coeff(5) - mv1.coeff(6)*mv2.coeff(0) - mv1.coeff(7)*mv2.coeff(7) - mv1.coeff(8)*mv2.coeff(1) - mv1.coeff(9)*mv2.coeff(2);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 2) and mv2 (grade 3).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 2 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 3 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void inner_2_3(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(0)*mv2.coeff(2) - mv1.coeff(1)*mv2.coeff(4) - mv1.coeff(2)*mv2.coeff(5) - mv1.coeff(4)*mv2.coeff(0) - mv1.coeff(5)*mv2.coeff(1) - mv1.coeff(7)*mv2.coeff(3);
		mv3.coeffRef(1) += -mv1.coeff(1)*mv2.coeff(7) - mv1.coeff(2)*mv2.coeff(8) - mv1.coeff(3)*mv2.coeff(2) - mv1.coeff(7)*mv2.coeff(6) + mv1.coeff(8)*mv2.coeff(0) + mv1.coeff(9)*mv2.coeff(1);
		mv3.coeffRef(2) +=  mv1.coeff(0)*mv2.coeff(7) - mv1.coeff(2)*mv2.coeff(9) - mv1.coeff(3)*mv2.coeff(4) + mv1.coeff(5)*mv2.coeff(6) - mv1.coeff(6)*mv2.coeff(0) + mv1.coeff(9)*mv2.coeff(3);
		mv3.coeffRef(3) +=  mv1.coeff(0)*mv2.coeff(8) + mv1.coeff(1)*mv2.coeff(9) - mv1.coeff(3)*mv2.coeff(5) - mv1.coeff(4)*mv2.coeff(6) - mv1.coeff(6)*mv2.coeff(1) - mv1.coeff(8)*mv2.coeff(3);
		mv3.coeffRef(4) += -mv1.coeff(4)*mv2.coeff(7) - mv1.coeff(5)*mv2.coeff(8) - mv1.coeff(6)*mv2.coeff(2) - mv1.coeff(7)*mv2.coeff(9) - mv1.coeff(8)*mv2.coeff(4) - mv1.coeff(9)*mv2.coeff(5);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 2) and mv2 (grade 4).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 2 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 4 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 2
	template<typename T>
	void inner_2_4(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(1)*mv2.coeff(1) - mv1.coeff(2)*mv2.coeff(2) - mv1.coeff(7)*mv2.coeff(0);
		mv3.coeffRef(1) +=  mv1.coeff(0)*mv2.coeff(1) - mv1.coeff(2)*mv2.coeff(3) + mv1.coeff(5)*mv2.coeff(0);
		mv3.coeffRef(2) +=  mv1.coeff(0)*mv2.coeff(2) + mv1.coeff(1)*mv2.coeff(3) - mv1.coeff(4)*mv2.coeff(0);
		mv3.coeffRef(3) += -mv1.coeff(4)*mv2.coeff(1) - mv1.coeff(5)*mv2.coeff(2) - mv1.coeff(7)*mv2.coeff(3);
		mv3.coeffRef(4) += -mv1.coeff(2)*mv2.coeff(4) + mv1.coeff(3)*mv2.coeff(1) - mv1.coeff(9)*mv2.coeff(0);
		mv3.coeffRef(5) +=  mv1.coeff(1)*mv2.coeff(4) + mv1.coeff(3)*mv2.coeff(2) + mv1.coeff(8)*mv2.coeff(0);
		mv3.coeffRef(6) += -mv1.coeff(7)*mv2.coeff(4) + mv1.coeff(8)*mv2.coeff(1) + mv1.coeff(9)*mv2.coeff(2);
		mv3.coeffRef(7) += -mv1.coeff(0)*mv2.coeff(4) + mv1.coeff(3)*mv2.coeff(3) - mv1.coeff(6)*mv2.coeff(0);
		mv3.coeffRef(8) +=  mv1.coeff(5)*mv2.coeff(4) - mv1.coeff(6)*mv2.coeff(1) + mv1.coeff(9)*mv2.coeff(3);
		mv3.coeffRef(9) += -mv1.coeff(4)*mv2.coeff(4) - mv1.coeff(6)*mv2.coeff(2) - mv1.coeff(8)*mv2.coeff(3);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 2) and mv2 (grade 5).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 2 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 5 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 3
	template<typename T>
	void inner_2_5(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(2)*mv2.coeff(0);
		mv3.coeffRef(1) +=  mv1.coeff(1)*mv2.coeff(0);
		mv3.coeffRef(2) += -mv1.coeff(7)*mv2.coeff(0);
		mv3.coeffRef(3) += -mv1.coeff(0)*mv2.coeff(0);
		mv3.coeffRef(4) +=  mv1.coeff(5)*mv2.coeff(0);
		mv3.coeffRef(5) += -mv1.coeff(4)*mv2.coeff(0);
		mv3.coeffRef(6) += -mv1.coeff(3)*mv2.coeff(0);
		mv3.coeffRef(7) += -mv1.coeff(9)*mv2.coeff(0);
		mv3.coeffRef(8) +=  mv1.coeff(8)*mv2.coeff(0);
		mv3.coeffRef(9) += -mv1.coeff(6)*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 3) and mv2 (grade 0).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 3 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 0 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 3
	template<typename T>
	void inner_3_0(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 3) and mv2 (grade 1).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 3 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 1 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 2
	template<typename T>
	void inner_3_1(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(2) + mv1.coeff(1)*mv2.coeff(3) - mv1.coeff(2)*mv2.coeff(0);
		mv3.coeffRef(1) += -mv1.coeff(0)*mv2.coeff(1) + mv1.coeff(3)*mv2.coeff(3) - mv1.coeff(4)*mv2.coeff(0);
		mv3.coeffRef(2) += -mv1.coeff(1)*mv2.coeff(1) - mv1.coeff(3)*mv2.coeff(2) - mv1.coeff(5)*mv2.coeff(0);
		mv3.coeffRef(3) += -mv1.coeff(2)*mv2.coeff(1) - mv1.coeff(4)*mv2.coeff(2) - mv1.coeff(5)*mv2.coeff(3);
		mv3.coeffRef(4) += -mv1.coeff(0)*mv2.coeff(4) + mv1.coeff(6)*mv2.coeff(3) - mv1.coeff(7)*mv2.coeff(0);
		mv3.coeffRef(5) += -mv1.coeff(1)*mv2.coeff(4) - mv1.coeff(6)*mv2.coeff(2) - mv1.coeff(8)*mv2.coeff(0);
		mv3.coeffRef(6) += -mv1.coeff(2)*mv2.coeff(4) - mv1.coeff(7)*mv2.coeff(2) - mv1.coeff(8)*mv2.coeff(3);
		mv3.coeffRef(7) += -mv1.coeff(3)*mv2.coeff(4) + mv1.coeff(6)*mv2.coeff(1) - mv1.coeff(9)*mv2.coeff(0);
		mv3.coeffRef(8) += -mv1.coeff(4)*mv2.coeff(4) + mv1.coeff(7)*mv2.coeff(1) - mv1.coeff(9)*mv2.coeff(3);
		mv3.coeffRef(9) += -mv1.coeff(5)*mv2.coeff(4) + mv1.coeff(8)*mv2.coeff(1) + mv1.coeff(9)*mv2.coeff(2);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 3) and mv2 (grade 2).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 3 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 2 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void inner_3_2(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(0)*mv2.coeff(4) - mv1.coeff(1)*mv2.coeff(5) - mv1.coeff(2)*mv2.coeff(0) - mv1.coeff(3)*mv2.coeff(7) - mv1.coeff(4)*mv2.coeff(1) - mv1.coeff(5)*mv2.coeff(2);
		mv3.coeffRef(1) +=  mv1.coeff(0)*mv2.coeff(8) + mv1.coeff(1)*mv2.coeff(9) - mv1.coeff(2)*mv2.coeff(3) - mv1.coeff(6)*mv2.coeff(7) - mv1.coeff(7)*mv2.coeff(1) - mv1.coeff(8)*mv2.coeff(2);
		mv3.coeffRef(2) += -mv1.coeff(0)*mv2.coeff(6) + mv1.coeff(3)*mv2.coeff(9) - mv1.coeff(4)*mv2.coeff(3) + mv1.coeff(6)*mv2.coeff(5) + mv1.coeff(7)*mv2.coeff(0) - mv1.coeff(9)*mv2.coeff(2);
		mv3.coeffRef(3) += -mv1.coeff(1)*mv2.coeff(6) - mv1.coeff(3)*mv2.coeff(8) - mv1.coeff(5)*mv2.coeff(3) - mv1.coeff(6)*mv2.coeff(4) + mv1.coeff(8)*mv2.coeff(0) + mv1.coeff(9)*mv2.coeff(1);
		mv3.coeffRef(4) += -mv1.coeff(2)*mv2.coeff(6) - mv1.coeff(4)*mv2.coeff(8) - mv1.coeff(5)*mv2.coeff(9) - mv1.coeff(7)*mv2.coeff(4) - mv1.coeff(8)*mv2.coeff(5) - mv1.coeff(9)*mv2.coeff(7);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 3) and mv2 (grade 3).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 3 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 3 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 0
	template<typename T>
	void inner_3_3(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(7) + mv1.coeff(1)*mv2.coeff(8) + mv1.coeff(2)*mv2.coeff(2) + mv1.coeff(3)*mv2.coeff(9) + mv1.coeff(4)*mv2.coeff(4) + mv1.coeff(5)*mv2.coeff(5) - mv1.coeff(6)*mv2.coeff(6) + mv1.coeff(7)*mv2.coeff(0) + mv1.coeff(8)*mv2.coeff(1) + mv1.coeff(9)*mv2.coeff(3);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 3) and mv2 (grade 4).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 3 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 4 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void inner_3_4(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(0)*mv2.coeff(1) - mv1.coeff(1)*mv2.coeff(2) - mv1.coeff(3)*mv2.coeff(3) + mv1.coeff(6)*mv2.coeff(0);
		mv3.coeffRef(1) += -mv1.coeff(3)*mv2.coeff(4) + mv1.coeff(4)*mv2.coeff(1) + mv1.coeff(5)*mv2.coeff(2) + mv1.coeff(9)*mv2.coeff(0);
		mv3.coeffRef(2) +=  mv1.coeff(1)*mv2.coeff(4) - mv1.coeff(2)*mv2.coeff(1) + mv1.coeff(5)*mv2.coeff(3) - mv1.coeff(8)*mv2.coeff(0);
		mv3.coeffRef(3) += -mv1.coeff(0)*mv2.coeff(4) - mv1.coeff(2)*mv2.coeff(2) - mv1.coeff(4)*mv2.coeff(3) + mv1.coeff(7)*mv2.coeff(0);
		mv3.coeffRef(4) += -mv1.coeff(6)*mv2.coeff(4) + mv1.coeff(7)*mv2.coeff(1) + mv1.coeff(8)*mv2.coeff(2) + mv1.coeff(9)*mv2.coeff(3);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 3) and mv2 (grade 5).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 3 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 5 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 2
	template<typename T>
	void inner_3_5(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(3)*mv2.coeff(0);
		mv3.coeffRef(1) += -mv1.coeff(1)*mv2.coeff(0);
		mv3.coeffRef(2) +=  mv1.coeff(0)*mv2.coeff(0);
		mv3.coeffRef(3) +=  mv1.coeff(6)*mv2.coeff(0);
		mv3.coeffRef(4) +=  mv1.coeff(5)*mv2.coeff(0);
		mv3.coeffRef(5) += -mv1.coeff(4)*mv2.coeff(0);
		mv3.coeffRef(6) +=  mv1.coeff(9)*mv2.coeff(0);
		mv3.coeffRef(7) +=  mv1.coeff(2)*mv2.coeff(0);
		mv3.coeffRef(8) += -mv1.coeff(8)*mv2.coeff(0);
		mv3.coeffRef(9) +=  mv1.coeff(7)*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 4) and mv2 (grade 0).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 4 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 0 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 4
	template<typename T>
	void inner_4_0(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 4) and mv2 (grade 1).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 4 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 1 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 3
	template<typename T>
	void inner_4_1(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(3) - mv1.coeff(1)*mv2.coeff(0);
		mv3.coeffRef(1) += -mv1.coeff(0)*mv2.coeff(2) - mv1.coeff(2)*mv2.coeff(0);
		mv3.coeffRef(2) += -mv1.coeff(1)*mv2.coeff(2) - mv1.coeff(2)*mv2.coeff(3);
		mv3.coeffRef(3) +=  mv1.coeff(0)*mv2.coeff(1) - mv1.coeff(3)*mv2.coeff(0);
		mv3.coeffRef(4) +=  mv1.coeff(1)*mv2.coeff(1) - mv1.coeff(3)*mv2.coeff(3);
		mv3.coeffRef(5) +=  mv1.coeff(2)*mv2.coeff(1) + mv1.coeff(3)*mv2.coeff(2);
		mv3.coeffRef(6) +=  mv1.coeff(0)*mv2.coeff(4) - mv1.coeff(4)*mv2.coeff(0);
		mv3.coeffRef(7) +=  mv1.coeff(1)*mv2.coeff(4) - mv1.coeff(4)*mv2.coeff(3);
		mv3.coeffRef(8) +=  mv1.coeff(2)*mv2.coeff(4) + mv1.coeff(4)*mv2.coeff(2);
		mv3.coeffRef(9) +=  mv1.coeff(3)*mv2.coeff(4) - mv1.coeff(4)*mv2.coeff(1);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 4) and mv2 (grade 2).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 4 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 2 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 2
	template<typename T>
	void inner_4_2(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(0)*mv2.coeff(7) - mv1.coeff(1)*mv2.coeff(1) - mv1.coeff(2)*mv2.coeff(2);
		mv3.coeffRef(1) +=  mv1.coeff(0)*mv2.coeff(5) + mv1.coeff(1)*mv2.coeff(0) - mv1.coeff(3)*mv2.coeff(2);
		mv3.coeffRef(2) += -mv1.coeff(0)*mv2.coeff(4) + mv1.coeff(2)*mv2.coeff(0) + mv1.coeff(3)*mv2.coeff(1);
		mv3.coeffRef(3) += -mv1.coeff(1)*mv2.coeff(4) - mv1.coeff(2)*mv2.coeff(5) - mv1.coeff(3)*mv2.coeff(7);
		mv3.coeffRef(4) += -mv1.coeff(0)*mv2.coeff(9) + mv1.coeff(1)*mv2.coeff(3) - mv1.coeff(4)*mv2.coeff(2);
		mv3.coeffRef(5) +=  mv1.coeff(0)*mv2.coeff(8) + mv1.coeff(2)*mv2.coeff(3) + mv1.coeff(4)*mv2.coeff(1);
		mv3.coeffRef(6) +=  mv1.coeff(1)*mv2.coeff(8) + mv1.coeff(2)*mv2.coeff(9) - mv1.coeff(4)*mv2.coeff(7);
		mv3.coeffRef(7) += -mv1.coeff(0)*mv2.coeff(6) + mv1.coeff(3)*mv2.coeff(3) - mv1.coeff(4)*mv2.coeff(0);
		mv3.coeffRef(8) += -mv1.coeff(1)*mv2.coeff(6) + mv1.coeff(3)*mv2.coeff(9) + mv1.coeff(4)*mv2.coeff(5);
		mv3.coeffRef(9) += -mv1.coeff(2)*mv2.coeff(6) - mv1.coeff(3)*mv2.coeff(8) - mv1.coeff(4)*mv2.coeff(4);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 4) and mv2 (grade 3).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 4 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 3 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void inner_4_3(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(0)*mv2.coeff(6) + mv1.coeff(1)*mv2.coeff(0) + mv1.coeff(2)*mv2.coeff(1) + mv1.coeff(3)*mv2.coeff(3);
		mv3.coeffRef(1) += -mv1.coeff(0)*mv2.coeff(9) - mv1.coeff(1)*mv2.coeff(4) - mv1.coeff(2)*mv2.coeff(5) + mv1.coeff(4)*mv2.coeff(3);
		mv3.coeffRef(2) +=  mv1.coeff(0)*mv2.coeff(8) + mv1.coeff(1)*mv2.coeff(2) - mv1.coeff(3)*mv2.coeff(5) - mv1.coeff(4)*mv2.coeff(1);
		mv3.coeffRef(3) += -mv1.coeff(0)*mv2.coeff(7) + mv1.coeff(2)*mv2.coeff(2) + mv1.coeff(3)*mv2.coeff(4) + mv1.coeff(4)*mv2.coeff(0);
		mv3.coeffRef(4) += -mv1.coeff(1)*mv2.coeff(7) - mv1.coeff(2)*mv2.coeff(8) - mv1.coeff(3)*mv2.coeff(9) + mv1.coeff(4)*mv2.coeff(6);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 4) and mv2 (grade 4).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 4 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 4 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 0
	template<typename T>
	void inner_4_4(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(4) - mv1.coeff(1)*mv2.coeff(1) - mv1.coeff(2)*mv2.coeff(2) - mv1.coeff(3)*mv2.coeff(3) + mv1.coeff(4)*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 4) and mv2 (grade 5).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 4 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 5 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void inner_4_5(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(0);
		mv3.coeffRef(1) +=  mv1.coeff(3)*mv2.coeff(0);
		mv3.coeffRef(2) += -mv1.coeff(2)*mv2.coeff(0);
		mv3.coeffRef(3) +=  mv1.coeff(1)*mv2.coeff(0);
		mv3.coeffRef(4) +=  mv1.coeff(4)*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 5) and mv2 (grade 0).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 5 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 0 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 5
	template<typename T>
	void inner_5_0(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 5) and mv2 (grade 1).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 5 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 1 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 4
	template<typename T>
	void inner_5_1(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(0)*mv2.coeff(0);
		mv3.coeffRef(1) += -mv1.coeff(0)*mv2.coeff(3);
		mv3.coeffRef(2) +=  mv1.coeff(0)*mv2.coeff(2);
		mv3.coeffRef(3) += -mv1.coeff(0)*mv2.coeff(1);
		mv3.coeffRef(4) += -mv1.coeff(0)*mv2.coeff(4);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 5) and mv2 (grade 2).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 5 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 2 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 3
	template<typename T>
	void inner_5_2(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(0)*mv2.coeff(2);
		mv3.coeffRef(1) +=  mv1.coeff(0)*mv2.coeff(1);
		mv3.coeffRef(2) += -mv1.coeff(0)*mv2.coeff(7);
		mv3.coeffRef(3) += -mv1.coeff(0)*mv2.coeff(0);
		mv3.coeffRef(4) +=  mv1.coeff(0)*mv2.coeff(5);
		mv3.coeffRef(5) += -mv1.coeff(0)*mv2.coeff(4);
		mv3.coeffRef(6) += -mv1.coeff(0)*mv2.coeff(3);
		mv3.coeffRef(7) += -mv1.coeff(0)*mv2.coeff(9);
		mv3.coeffRef(8) +=  mv1.coeff(0)*mv2.coeff(8);
		mv3.coeffRef(9) += -mv1.coeff(0)*mv2.coeff(6);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 5) and mv2 (grade 3).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 5 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 3 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 2
	template<typename T>
	void inner_5_3(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(3);
		mv3.coeffRef(1) += -mv1.coeff(0)*mv2.coeff(1);
		mv3.coeffRef(2) +=  mv1.coeff(0)*mv2.coeff(0);
		mv3.coeffRef(3) +=  mv1.coeff(0)*mv2.coeff(6);
		mv3.coeffRef(4) +=  mv1.coeff(0)*mv2.coeff(5);
		mv3.coeffRef(5) += -mv1.coeff(0)*mv2.coeff(4);
		mv3.coeffRef(6) +=  mv1.coeff(0)*mv2.coeff(9);
		mv3.coeffRef(7) +=  mv1.coeff(0)*mv2.coeff(2);
		mv3.coeffRef(8) += -mv1.coeff(0)*mv2.coeff(8);
		mv3.coeffRef(9) +=  mv1.coeff(0)*mv2.coeff(7);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 5) and mv2 (grade 4).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 5 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 4 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void inner_5_4(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(0);
		mv3.coeffRef(1) +=  mv1.coeff(0)*mv2.coeff(3);
		mv3.coeffRef(2) += -mv1.coeff(0)*mv2.coeff(2);
		mv3.coeffRef(3) +=  mv1.coeff(0)*mv2.coeff(1);
		mv3.coeffRef(4) +=  mv1.coeff(0)*mv2.coeff(4);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 5) and mv2 (grade 5).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 5 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 5 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 0
	template<typename T>
	void inner_5_5(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(0)*mv2.coeff(0);
	}

    template<typename T>
	std::array<std::array<std::function<void(const Eigen::Matrix<T, Eigen::Dynamic, 1> & , const Eigen::Matrix<T, Eigen::Dynamic, 1> & , Eigen::Matrix<T, Eigen::Dynamic, 1>&)>, 6>, 6> innerFunctionsContainer = {{
		{{inner_0_0<T>,inner_0_1<T>,inner_0_2<T>,inner_0_3<T>,inner_0_4<T>,inner_0_5<T>}},
		{{inner_1_0<T>,inner_1_1<T>,inner_1_2<T>,inner_1_3<T>,inner_1_4<T>,inner_1_5<T>}},
		{{inner_2_0<T>,inner_2_1<T>,inner_2_2<T>,inner_2_3<T>,inner_2_4<T>,inner_2_5<T>}},
		{{inner_3_0<T>,inner_3_1<T>,inner_3_2<T>,inner_3_3<T>,inner_3_4<T>,inner_3_5<T>}},
		{{inner_4_0<T>,inner_4_1<T>,inner_4_2<T>,inner_4_3<T>,inner_4_4<T>,inner_4_5<T>}},
		{{inner_5_0<T>,inner_5_1<T>,inner_5_2<T>,inner_5_3<T>,inner_5_4<T>,inner_5_5<T>}}
	}};

}/// End of Namespace

#endif // C3GA_INNER_PRODUCT_EXPLICIT_HPP__


// std includes
#include <iostream>
#include <vector>
#include <random>
// thirdparties includes
#include <Eigen/Dense>
// lib includes
#include "m0sh/uniform.h"

using TypeScalar = double;
// Space
const unsigned int DIM = 3;
using TypeVector = Eigen::Matrix<TypeScalar, DIM, 1>;
template<typename ...Args>
using TypeRef = Eigen::Ref<Args...>;
// Mesh
template<typename ...Args>
using TypeContainer = std::vector<Args...>;
using TypeMesh = m0sh::Uniform<TypeVector, TypeRef, TypeContainer>;
// Data
const int nc = 100;
const double length = 1.0;
const double origin = 0.5;

void print(const TypeMesh& mesh, std::uniform_int_distribution<int>& uniform, std::default_random_engine& e) {
    TypeContainer<int> ijk;
    TypeVector x;
    std::size_t index;
    // Print info
    ijk = {uniform(e), uniform(e), uniform(e)};
    x = mesh.positionPoint(ijk);
    index = mesh.indexPoint(ijk);
    std::cout << "i: " << ijk[0] << " j: " << ijk[1] << " k: " << ijk[2] << "\nindex: " << index << "\nx: " << x.transpose() << std::endl;
    ijk = mesh.ijkPoint(x);
    std::cout << "xReverse: " << " i: " << ijk[0] << " j: " << ijk[1] << " k: " << ijk[2] << std::endl;
    ijk = mesh.ijkPoint(index);
    std::cout << "indexReverse: " << " i: " << ijk[0] << " j: " << ijk[1] << " k: " << ijk[2] << std::endl;
    std::cout << std::endl;
}

int main () {
    TypeMesh mesh(TypeContainer<std::size_t>(DIM, nc), TypeContainer<double>(DIM, length), TypeVector::Constant(origin), TypeContainer<bool>(DIM, false));
    // Random setup
    std::random_device r;
    std::default_random_engine e(r());
    std::uniform_int_distribution<int> uniform(-nc, 2*nc);
    // Print
    print(mesh, uniform, e);
    print(mesh, uniform, e);
    print(mesh, uniform, e);
}


/*
 * Copyright Nick Thompson, 2018
 * Use, modification and distribution are subject to the
 * Boost Software License, Version 1.0. (See accompanying file
 * LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
 */
#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <complex>
#include <bitset>
#include <boost/assert.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/tools/polynomial.hpp>
#include <boost/math/tools/roots.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/multiprecision/cpp_complex.hpp>
#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>

typedef boost::multiprecision::float128 float128_t;
#else
typedef boost::multiprecision::cpp_bin_float_quad float128_t;
#endif
//#include <boost/multiprecision/complex128.hpp>
#include <boost/math/quadrature/gauss_kronrod.hpp>

using std::string;
using boost::math::tools::polynomial;
using boost::math::binomial_coefficient;
using boost::math::tools::schroder_iterate;
using boost::math::tools::halley_iterate;
using boost::math::tools::newton_raphson_iterate;
using boost::math::tools::complex_newton;
using boost::math::constants::half;
using boost::math::constants::root_two;
using boost::math::constants::pi;
using boost::math::quadrature::gauss_kronrod;
using boost::multiprecision::cpp_bin_float_100;
using boost::multiprecision::cpp_complex_100;

template<class Complex>
std::vector<std::pair<Complex, Complex>> find_roots(size_t p)
{
    // Initialize the polynomial; see Mallat, A Wavelet Tour of Signal Processing, equation 7.96
    BOOST_ASSERT(p>0);
    typedef typename Complex::value_type Real;
    std::vector<Complex> coeffs(p);
    for (size_t k = 0; k < coeffs.size(); ++k)
    {
        coeffs[k] = Complex(binomial_coefficient<Real>(p-1+k, k), 0);
    }

    polynomial<Complex> P(std::move(coeffs));
    polynomial<Complex> Pcopy = P;
    polynomial<Complex> Pcopy_prime = P.prime();
    auto orig = [&](Complex z) { return std::make_pair<Complex, Complex>(Pcopy(z), Pcopy_prime(z)); };

    polynomial<Complex> P_prime = P.prime();

    // Polynomial is of degree p-1.

    std::vector<Complex> roots(p-1, {std::numeric_limits<Real>::quiet_NaN(),std::numeric_limits<Real>::quiet_NaN()});
    size_t i = 0;
    while(P.size() > 1)
    {
        Complex guess = {0.0, 1.0};
        std::cout << std::setprecision(std::numeric_limits<Real>::digits10+3);

        auto f = [&](Complex x)->std::pair<Complex, Complex>
        {
            return std::make_pair<Complex, Complex>(P(x), P_prime(x));
        };

        Complex r = complex_newton(f, guess);
        using std::isnan;
        if(isnan(r.real()))
        {
            int i = 50;
            do {
                // Try a different guess
                guess *= Complex(1.0,-1.0);
                r = complex_newton(f, guess);
                std::cout << "New guess: " << guess << ", result? " << r << std::endl;

            } while (isnan(r.real()) && i-- > 0);

            if (isnan(r.real()))
            {
                std::cout << "Polynomial that killed the process: " << P << std::endl;
                throw std::logic_error("Newton iteration did not converge");
            }
        }
        // Refine r with the original function.
        // We only use the polynomial division to ensure we don't get the same root over and over.
        // However, the division induces error which can grow quickly-or slowly! See Numerical Recipes, section 9.5.1.
        r = complex_newton(orig, r);
        if (isnan(r.real()))
        {
            throw std::logic_error("Found a root for the deflated polynomial which is not a root for the original. Indicative of catastrophic numerical error.");
        }
        // Test the root:
        using std::sqrt;
        Real tol = sqrt(sqrt(std::numeric_limits<Real>::epsilon()));
        if (norm(Pcopy(r)) > tol)
        {
            std::cout << "This is a bad root: P" <<  r << " = " << Pcopy(r) << std::endl;
            std::cout << "Reduced polynomial leading to bad root: " << P << std::endl;
            throw std::logic_error("Donezo.");
        }

        BOOST_ASSERT(i < roots.size());
        roots[i] = r;
        ++i;
        polynomial<Complex> q{-r, {1,0}};
        // This optimization breaks at p = 11. I have no clue why.
        // Unfortunate, because I expect it to be considerably more stable than
        // repeatedly dividing by the complex root.
        /*polynomial<Complex> q;
        if (r.imag() > sqrt(std::numeric_limits<Real>::epsilon()))
        {
            // Then the complex conjugate is also a root:
            using std::conj;
            using std::norm;
            BOOST_ASSERT(i < roots.size());
            roots[i] = conj(r);
            ++i;
            q = polynomial<Complex>({{norm(r), 0}, {-2*r.real(),0}, {1,0}});
        }
        else
        {
            // The imaginary part is numerical noise:
            r.imag() = 0;
            q = polynomial<Complex>({-r, {1,0}});
        }*/

        auto PR = quotient_remainder(P, q);
        // I should validate that the remainder is small, but . . .
        //std::cout << "Remainder = " << PR.second<< std::endl;

        P = PR.first;
        P_prime = P.prime();
    }

    std::vector<std::pair<Complex, Complex>> Qroots(p-1);
    for (size_t i = 0; i < Qroots.size(); ++i)
    {
        Complex y = roots[i];
        Complex z1 = static_cast<Complex>(1) - static_cast<Complex>(2)*y + static_cast<Complex>(2)*sqrt(y*(y-static_cast<Complex>(1)));
        Complex z2 = static_cast<Complex>(1) - static_cast<Complex>(2)*y - static_cast<Complex>(2)*sqrt(y*(y-static_cast<Complex>(1)));
        Qroots[i] = {z1, z2};
    }

    return Qroots;
}

template<class Complex>
std::vector<typename Complex::value_type> daubechies_coefficients(std::vector<std::pair<Complex, Complex>> const & Qroots)
{
    typedef typename Complex::value_type Real;
    size_t p = Qroots.size() + 1;
    // Choose the minimum abs root; see Mallat, discussion just after equation 7.98
    std::vector<Complex> chosen_roots(p-1);
    for (size_t i = 0; i < p - 1; ++i)
    {
        if(norm(Qroots[i].first) <= 1)
        {
            chosen_roots[i] = Qroots[i].first;
        }
        else
        {
            BOOST_ASSERT(norm(Qroots[i].second) <= 1);
            chosen_roots[i] = Qroots[i].second;
        }
    }

    polynomial<Complex> R{1};
    for (size_t i = 0; i < p-1; ++i)
    {
        Complex ak = chosen_roots[i];
        R *= polynomial<Complex>({-ak/(static_cast<Complex>(1)-ak), static_cast<Complex>(1)/(static_cast<Complex>(1)-ak)});
    }
    polynomial<Complex> a{{half<Real>(), 0}, {half<Real>(),0}};
    polynomial<Complex> poly = root_two<Real>()*pow(a, p)*R;
    std::vector<Complex> result = poly.data();
    // If we reverse, we get the Numerical Recipes and Daubechies convention.
    // If we don't reverse, we get the Pywavelets and Mallat convention.
    // I believe this is because of the sign convention on the DFT, which differs between Daubechies and Mallat.
    // You implement a dot product in Daubechies/NR convention, and a convolution in PyWavelets/Mallat convention.
    std::reverse(result.begin(), result.end());
    std::vector<Real> h(result.size());
    for (size_t i = 0; i < result.size(); ++i)
    {
        Complex r = result[i];
        BOOST_ASSERT(r.imag() < sqrt(std::numeric_limits<Real>::epsilon()));
        h[i] = r.real();
    }

    // Quick sanity check: We could check all vanishing moments, but that sum is horribly ill-conditioned too!
    Real sum = 0;
    Real scale = 0;
    for (size_t i = 0; i < h.size(); ++i)
    {
        sum += h[i];
        scale += h[i]*h[i];
    }
    BOOST_ASSERT(abs(scale -1) < sqrt(std::numeric_limits<Real>::epsilon()));
    BOOST_ASSERT(abs(sum - root_two<Real>()) < sqrt(std::numeric_limits<Real>::epsilon()));
    return h;
}

int main()
{
    typedef boost::multiprecision::cpp_complex<500> Complex;
    size_t p_max = 20;
    std::ofstream fs{"daubechies_filters.hpp"};
    fs << "/*\n"
       << " * Copyright Nick Thompson, 2019\n"
       << " * Use, modification and distribution are subject to the\n"
       << " * Boost Software License, Version 1.0. (See accompanying file\n"
       << " * LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)\n"
       << " */\n"
       << "#ifndef BOOST_MATH_FILTERS_DAUBECHIES_HPP\n"
       << "#define BOOST_MATH_FILTERS_DAUBECHIES_HPP\n"
       << "#include <array>\n"
       << "#include <limits>\n"
       << "#include <boost/math/tools/big_constant.hpp>\n\n"
       << "namespace boost::math::filters {\n\n"
       << "template <typename Real, unsigned p>\n"
       << "constexpr std::array<Real, 2*p> daubechies_scaling_filter()\n"
       << "{\n"
       << "    static_assert(p < " << p_max << ", \"Filter coefficients only implemented up to " << p_max - 1 << ".\");\n";

    for(size_t p = 1; p < p_max; ++p)
    {
        fs << std::setprecision(std::numeric_limits<boost::multiprecision::cpp_bin_float_oct>::max_digits10);
        auto roots = find_roots<Complex>(p);
        auto h = daubechies_coefficients(roots);
        fs << "    if constexpr (p == " << p << ") {\n";
        fs << "       return {";
        for (size_t i = 0; i < h.size() - 1; ++i) {
            fs << "BOOST_MATH_BIG_CONSTANT(Real, std::numeric_limits<Real>::digits, " << h[i] << "), ";
        }
        fs << "BOOST_MATH_BIG_CONSTANT(Real, std::numeric_limits<Real>::digits, " << h[h.size()-1] << ") };\n";
        fs << "    }\n";
    }

    fs << "}\n\n";

    fs << "template<class Real, size_t p>\n";
    fs << "std::array<Real, 2*p> daubechies_wavelet_filter() {\n";
    fs << "    std::array<Real, 2*p> g;\n";
    fs << "    auto h = daubechies_scaling_filter<Real, p>();\n";
    fs << "    for (size_t i = 0; i < g.size(); i += 2)\n";
    fs << "    {\n";
    fs << "        g[i] = h[g.size() - i - 1];\n";
    fs << "        g[i+1] = -h[g.size() - i - 2];\n";
    fs << "    }\n";
    fs << "    return g;\n";
    fs << "}\n\n";
    fs << "} // namespaces\n";
    fs << "#endif\n";
    fs.close();
}