// $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 <boost/compute/random/linear_congruential_engine.hpp>


#include <boost/python/def.hpp>
#include <boost/python/args.hpp>

#include <scitbx/math/interpolation.h>
#include <scitbx/math/linear_interpolation.h>
#include <scitbx/vec3.h>
#include <scitbx/vec2.h>

namespace scitbx { namespace math {

namespace {

  template <typename PointType>
  void wrap_splines()
  {
    using namespace boost::python;
    def("interpolate_catmull_rom_spline",
      (af::shared<PointType>(*)(
        PointType const&,
        PointType const&,
        PointType const&,
        PointType const&,
        unsigned)) interpolate_catmull_rom_spline, (
          arg("p0"),
          arg("p1"),
          arg("p2"),
          arg("p3"),
          arg("n_points")));
  }

  template <typename FloatType>
  void wrap_interp_2d()
  {
    using namespace boost::python;
    def("linear_interpolation_2d",
      (FloatType (*)(
          FloatType const&,
          FloatType const&,
          FloatType const&,
          FloatType const&,
          FloatType const&,
          FloatType const&,
          FloatType const&,
          FloatType const&,
          FloatType const&,
          FloatType const&)) linear_interpolation_2d, (
            arg("x1"),
            arg("y1"),
            arg("x2"),
            arg("y2"),
            arg("v1"),
            arg("v2"),
            arg("v3"),
            arg("v4"),
            arg("xx"),
            arg("yy")));
  }

} // namespace <anonymous>

namespace boost_python {

  void wrap_interpolation()
  {
    wrap_splines< scitbx::vec2<double> >();
    wrap_splines< scitbx::vec3<double> >();
    wrap_interp_2d <double> ();
  }

}}} // namespace scitbx::math::boost_python


#include <cmath>
#include <vector>

#include <Eigen/Dense>
#include "gtest/gtest.h"

#include "BingoCpp/backend.h"
#include "testing_utils.h"
#include "test_fixtures.h"

using namespace bingo;
namespace {
const int N_OPS = 13;

class AGraphBackend : public ::testing::TestWithParam<int> {
 public:

  const double AGRAPH_VAL_START =-1;
  const double AGRAPH_VAL_END = 0;
  const int N_AGRAPH_VAL = 11;

  testutils::AGraphValues sample_agraph_1_values;
  std::vector<Eigen::ArrayXXd> operator_evals_x0;
  std::vector<Eigen::ArrayXXd> operator_x_derivs;
  std::vector<Eigen::ArrayXXd> operator_c_derivs;

  Eigen::ArrayX3i simple_stack;
  Eigen::ArrayX3i simple_stack2;
  Eigen::ArrayXXd x;
  Eigen::ArrayXd constants;

  virtual void SetUp() {
    sample_agraph_1_values = testutils::init_agraph_vals(AGRAPH_VAL_START,
                                                         AGRAPH_VAL_END,
                                                         N_AGRAPH_VAL);
    operator_evals_x0 = testutils::init_op_evals_x0(sample_agraph_1_values);
    operator_x_derivs = testutils::init_op_x_derivs(sample_agraph_1_values);
    operator_c_derivs = testutils::init_op_c_derivs(sample_agraph_1_values);

    simple_stack = testutils::stack_operators_0_to_5();
    simple_stack2 = testutils::stack_unary_operator(4);
    x = testutils::one_to_nine_3_by_3();
    constants = testutils::pi_ten_constants();
  }
  virtual void TearDown() {}
};

TEST_P(AGraphBackend, simplify_and_evaluate) {
  int operator_i = GetParam();
  Eigen::ArrayXXd expected_outcome = operator_evals_x0[operator_i];

  Eigen::ArrayX3i stack(3, 3);
  stack << 0, 0, 0,
           0, 1, 0,
           operator_i, 0, 0;
  Eigen::ArrayXXd f_of_x = simplify_and_evaluate(stack,
                                                 sample_agraph_1_values.x_vals,
                                                 sample_agraph_1_values.constants);
  ASSERT_TRUE(testutils::almost_equal(expected_outcome, f_of_x));
}

TEST_P(AGraphBackend, simplify_and_evaluate_x_deriv) {
  int operator_i = GetParam();
  Eigen::ArrayXXd expected_derivative =
    Eigen::ArrayXXd::Zero(sample_agraph_1_values.x_vals.rows(), 2);
  expected_derivative.col(0) = operator_x_derivs[operator_i];

  Eigen::ArrayX3i stack(4, 3);
  stack << 0, 0, 0,
           0, 0, 0,
           0, 1, 1,
           operator_i, 0, 1;

  Eigen::ArrayXXd x_0 = sample_agraph_1_values.x_vals;
  Eigen::ArrayXXd constants = sample_agraph_1_values.constants;
  std::pair<Eigen::ArrayXXd, Eigen::ArrayXXd> res_and_gradient =
    simplify_and_evaluate_with_derivative(stack,
                                          x_0,
                                          constants,
                                          true);
  Eigen::ArrayXXd df_dx = res_and_gradient.second;
  ASSERT_TRUE(testutils::almost_equal(expected_derivative, df_dx));
}

TEST_P(AGraphBackend, simplify_and_evaluate_c_deriv) {
  int operator_i = GetParam();
  int num_x_points = sample_agraph_1_values.x_vals.rows();
  int num_consts = sample_agraph_1_values.constants.size();
  int last_col = num_consts - 1;
  Eigen::ArrayXXd expected_derivative =
    Eigen::MatrixXd::Zero(num_x_points, num_consts).array();
  expected_derivative.col(last_col) = operator_c_derivs[operator_i];

  Eigen::ArrayX3i stack(4, 3);
  stack << 1, 1, 1,
           1, 1, 1,
           0, 1, 1,
           operator_i, 1, 0;

  Eigen::ArrayXXd x_0 = sample_agraph_1_values.x_vals;
  Eigen::ArrayXXd constants = sample_agraph_1_values.constants;
  std::pair<Eigen::ArrayXXd, Eigen::ArrayXXd> res_and_gradient =
    simplify_and_evaluate_with_derivative(stack,
                                          x_0,
                                          constants,
                                          false);
  Eigen::ArrayXXd df_dc = res_and_gradient.second;
  ASSERT_TRUE(testutils::almost_equal(expected_derivative, df_dc));
}
INSTANTIATE_TEST_CASE_P(,AGraphBackend, ::testing::Range(0, N_OPS, 1));

TEST_F(AGraphBackend, evaluate) {
  Eigen::ArrayXXd y = evaluate(simple_stack, x, constants);
  Eigen::ArrayXXd y_true = x.col(0) * (constants[0] + constants[1]
                          / x.col(1)) - x.col(0);
  ASSERT_TRUE(testutils::almost_equal(y, y_true));
}

TEST_F(AGraphBackend, evaluate_and_derivative) {
  std::pair<Eigen::ArrayXXd, Eigen::ArrayXXd> y_and_dy =
    evaluate_with_derivative(simple_stack, x, constants);
  Eigen::ArrayXXd y_true = x.col(0) * (constants[0] + constants[1]
                          / x.col(1)) - x.col(0);
  Eigen::ArrayXXd dy_true = Eigen::ArrayXXd::Zero(3, 3);
  dy_true.col(0) = constants[0] + constants[1] / x.col(1) - 1.;
  dy_true.col(1) = - x.col(0) * constants[1] / x.col(1) / x.col(1);

  ASSERT_TRUE(testutils::almost_equal(y_and_dy.first, y_true));
  ASSERT_TRUE(testutils::almost_equal(y_and_dy.second, dy_true));
}

TEST_F(AGraphBackend, mask_evaluate) {
  Eigen::ArrayXXd y = evaluate(simple_stack, x, constants);
  Eigen::ArrayXXd y_simple = simplify_and_evaluate(simple_stack, x, constants);
  ASSERT_TRUE(testutils::almost_equal(y, y_simple));
}

TEST_F(AGraphBackend, mask_evaluate_and_derivative) {
  std::pair<Eigen::ArrayXXd, Eigen::ArrayXXd> y_and_dy =
    evaluate_with_derivative(simple_stack, x, constants);
  std::pair<Eigen::ArrayXXd, Eigen::ArrayXXd> y_and_dy_simple =
    simplify_and_evaluate_with_derivative(simple_stack, x, constants);
  ASSERT_TRUE(testutils::almost_equal(y_and_dy.first, y_and_dy_simple.first));
  ASSERT_TRUE(testutils::almost_equal(y_and_dy.first, y_and_dy_simple.first));
}

// TEST_F(AcyclicGraphTest, simplify) {
//   // shorter stack
//   std::cout << "stack\n" << stack << std::endl;
//   Eigen::ArrayX3i short_stack = SimplifyStack(stack);
//   std::cout << "2\n";
//   ASSERT_LE(short_stack.rows(), stack.rows());
//   std::cout << "3\n";

//   // equivalent evatuation
//   Eigen::ArrayXXd y = Evaluate(stack, x, constants);
//   std::cout << "4\n";
//   Eigen::ArrayXXd simplified_y = Evaluate(short_stack, x, constants);
//   std::cout << "5\n";

//   for (size_t i = 0; i < x.rows(); ++i) {
//     ASSERT_EQ(y(i), simplified_y(i));
//   }
// }

TEST_F(AGraphBackend, get_utilized_commands) {
  std::vector<bool> used_commands = get_utilized_commands(simple_stack);
  int num_used_commands = 0;
  for (auto const& command_is_used : used_commands) {
    if (command_is_used) {
      ++num_used_commands;
    }
  }
  ASSERT_EQ(num_used_commands, 8);
}
} // namespace


/**
 * GlobalNearestNeighborAssociation.hpp
 * @author koide
 * 15/05/29
**/
#ifndef KKL_GLOBAL_NEAREST_NEIGHBOR_ASSOCIATION_HPP
#define KKL_GLOBAL_NEAREST_NEIGHBOR_ASSOCIATION_HPP

#include <memory>
#include <boost/range/algorithm.hpp>

#include <kkl/alg/munkres.hpp>
#include <kkl/alg/data_association.hpp>

namespace kkl {
	namespace alg {

/******************************************
 * GlobalNearestNeighborAssociation
 *
******************************************/
template<typename Tracker, typename Observation>
class GlobalNearestNeighborAssociation : public DataAssociation<Tracker, Observation> {
    typedef typename DataAssociation<Tracker, Observation>::Association Association;
public:
	// constructor, destructor
	GlobalNearestNeighborAssociation()
		: munkres( new Munkres<double>() ){}
	virtual ~GlobalNearestNeighborAssociation() {}

	// associate
    std::vector<Association> associate(const std::vector<Tracker>& trackers, const std::vector<Observation>& observations) override {
		const double HUGE_VALUE = 1000000.0;

		// create cost matrix between tracker and observation
		Eigen::MatrixXd cost_matrix;
		bool transposed = trackers.size() > observations.size();

		if (!transposed) {
			cost_matrix = Eigen::MatrixXd::Constant(trackers.size(), observations.size(), HUGE_VALUE + 1);
			for (int i = 0; i < trackers.size(); i++) {
				for (int j = 0; j < observations.size(); j++) {
					auto dist = distance<Tracker, Observation>(trackers[i], observations[j]);
					if (dist) {
						cost_matrix(i, j) = dist.get();
					}
				}
			}
		}
		else {
			cost_matrix = Eigen::MatrixXd::Constant(observations.size(), trackers.size(), HUGE_VALUE + 1);
			for (int i = 0; i < observations.size(); i++) {
				for (int j = 0; j < trackers.size(); j++) {
					auto dist = distance<Tracker, Observation>(trackers[j], observations[i]);
					if (dist) {
						cost_matrix(i, j) = dist.get();
					}
				}
			}
		}

		// solve combinatorial optimization using Munkres algorithm
		auto solution = munkres->solve(cost_matrix);

		std::vector<Association> associations;
		associations.reserve(trackers.size());

		for (int i = 0; i < solution.size(); i++) {
			int tracker;
			int observation;

			if (!transposed) {
				tracker = i;
				observation = solution[i];
			} else {
				tracker = solution[i];
				observation = i;
			}

			if (cost_matrix(i, solution[i]) < HUGE_VALUE) {
				associations.push_back(Association(tracker, observation, cost_matrix(i, solution[i])));
			}
		}

		return associations;
	}

private:
	std::unique_ptr<Munkres<double>> munkres;
};

	}
}

#endif


#include <boost/cstdint.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/simd/sdk/memory/is_power_of_2.hpp>

using boost::simd::meta::is_power_of_2;

int main()
{
  BOOST_MPL_ASSERT(( is_power_of_2< boost::mpl::int_<2> >::type ));
  BOOST_MPL_ASSERT(( is_power_of_2< boost::mpl::int_<4> >::type ));
  BOOST_MPL_ASSERT(( is_power_of_2< boost::mpl::int_<8> >::type ));
  BOOST_MPL_ASSERT_NOT(( is_power_of_2< boost::mpl::int_<0> >::type ));
  BOOST_MPL_ASSERT_NOT(( is_power_of_2< boost::mpl::int_<10> >::type ));
}


#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;
}


/*!
 * @file
 * Forward declares the @ref Ring typeclass.
 *
 *
 * @copyright Louis Dionne 2014
 * Distributed under the Boost Software License, Version 1.0.
 *         (See accompanying file LICENSE.md or copy at
 *             http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_MPL11_FWD_RING_HPP
#define BOOST_MPL11_FWD_RING_HPP

#include <boost/mpl11/fwd/bool.hpp>

namespace boost { namespace mpl11 {
    /*!
     * @ingroup typeclasses
     * @defgroup Ring Ring
     *
     * The `Ring` typeclass is used for `Group`s that also form a `Monoid`
     * under a second binary operation that distributes over the first.
     *
     * Instances of `Ring` must satisfy the following laws:
     *
        @code
            mult a (mult b c) == mult (mult a b) c

            mult one a == a
            mult a one == a

            mult a (plus b c) == plus (mult a b) (mult a c)
        @endcode
     *
     *
     * ### Refines
     * `Group`
     *
     * ### Methods
     * `mult` and `one`
     *
     * ### Minimal complete definition
     * All the methods.
     *
     * @{
     */
    template <typename Left, typename Right = Left, typename = true_>
    struct Ring;

    /*!
     * `Ring` operation.
     *
     * `mult` can be invoked with more than two arguments. Specifically,
     * `mult<x1, x2, xn...>` is equivalent to `mult<mult<x1, x2>, xn...>`.
     */
    template <typename x1, typename x2, typename ...xn>
    struct mult;

    //! Multiplicative identity for the given `Datatype`.
    template <typename Datatype>
    struct one;
    //! @}
}} // end namespace boost::mpl11

#endif // !BOOST_MPL11_FWD_RING_HPP


#include <sstream>
#include <boost/program_options.hpp>

#include <frovedis/ml/fm/fm.hpp>
#include <frovedis/matrix/crs_matrix.hpp>

using namespace boost;
using namespace frovedis;
using namespace std;

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

  // debug option
  set_loglevel(TRACE);

  using namespace boost::program_options;

  vector<size_t> dims;
  double init_stdev;
  size_t iteration;
  double init_learn_rate;
  string optimizer_name;
  string input_path, label_path, output_path;
  vector<double> regulars;
  string task;
  size_t batch_size_pernode;
  bool binary_input;

  options_description opt("option");
  opt.add_options()
    ("dim", value<vector<size_t>>(&dims)->multitoken()->zero_tokens()->default_value({1,1,8}, "'1 1 8'"),
     "k0 k1 k2: k0=use bias, k1=use 1-way interactions, k2=dim of 2-way interactions")
    ("help,h", "print help")
    ("init-stdev", value<double>(&init_stdev)->default_value(0.1, "0.1"),
     "stdev of initialization of 2-way factors")
    ("iter", value<size_t>(&iteration)->default_value(100, "100"), "number of iteration")
    ("learn-rate", value<double>(&init_learn_rate)->default_value(0.1, "0.1"),
     "learning rate for SGD; dafault=0.1")
    ("method", value<string>(&optimizer_name)->default_value("SGD"),
     "learning method in SGD, SGDA, ALS, MCMC")
    ("input,i", value<string>(&input_path)->required(), "filename for training data, crs matrix format")
    ("label,l", value<string>(&label_path)->required(), "filename for training label, dvector format")
    ("output,o", value<string>(&output_path)->required(), "filename for output model")
    ("regular", value<vector<double>>(&regulars)->multitoken()->default_value(vector<double>{0.,0.,0.}, "'0 0 0'"),
     "r0,r1,r2 for SGD or ALS: r0=bias reg., r1=1-way reg., r2=2-way reg.")
    ("task,t", value<string>(&task)->default_value("r"), "r=regression, c=classification")
    ("batchsize,s", value<size_t>(&batch_size_pernode)->default_value(5000, "5000"),
     "minibatch size per node")
    ("binary", bool_switch(&binary_input)->default_value(false, "false"),
     "use binary input file")
    ;

  variables_map argmap;
  store(command_line_parser(argc,argv).options(opt).allow_unregistered().run(), argmap);
  if(argmap.count("help")){
    cerr << opt << endl;
    exit(1);
  }

  notify(argmap);

  if (dims.size() != 3)  throw runtime_error("--dim is required to have 3 values.");
  bool dim_0 = dims[0];
  bool dim_1 = dims[1];
  size_t dim_2 = dims[2];

  fm::FmOptimizer optimizer;
  if (optimizer_name == "SGD") {
    optimizer = FmOptimizer::SGD;
  } else if (optimizer_name == "SGDA") {
    optimizer = FmOptimizer::SGDA;
  } else if (optimizer_name == "ALS") {
    optimizer = FmOptimizer::ALS;
  } else if (optimizer_name == "MCMC") {
    optimizer = FmOptimizer::MCMC;
  } else {
    throw runtime_error("optimizer specified by -method is not supported.");
  }

  if (regulars.size() != 3)  throw runtime_error("--regular is required to have 3 values.");
  double regular_0 = regulars[0];
  double regular_1 = regulars[1];
  double regular_2 = regulars[2];

  bool is_regression;
  if (task == "r") {
    is_regression = true;
  } else if (task == "c") {
    is_regression = false;
  } else {
    throw runtime_error("-task is required to be 'r' or 'c'.");
  }

  crs_matrix<double> nl_data;
  if (binary_input) {
    nl_data = make_crs_matrix_loadbinary<double>(input_path);
  } else {
    nl_data = make_crs_matrix_load<double>(input_path);
  }

  dvector<double> dv_label;
  if (binary_input) {
    dv_label = make_dvector_loadbinary<double>(label_path);
  } else {
    dv_label = make_dvector_loadline<double>(label_path);
  }

  auto model = fm_train(dim_0, dim_1, dim_2,
        init_stdev, iteration, init_learn_rate, optimizer,
        regular_0, regular_1, regular_2, is_regression,
        nl_data, dv_label, batch_size_pernode);

  model.save(output_path);
  return 0;
}


#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 Matt Borland 2021.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Basic sanity check that header
// #includes all the files that it needs to.
//
#include <boost/math/tools/engel_expansion.hpp>
//
// Note this header includes no other headers, this is
// important if this test is to be meaningful:
//
#include "test_compile_result.hpp"

void compile_and_link_test()
{
    boost::math::tools::engel_expansion<float> f_test(1.0f);
    check_result<int64_t>(f_test.digits().front());

    boost::math::tools::engel_expansion<double> d_test(1.0);
    check_result<int64_t>(d_test.digits().front());

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    boost::math::tools::engel_expansion<long double> ld_test(1.0l);
    check_result<int64_t>(ld_test.digits().front());
#endif
}


/* boost random/detail/const_mod.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_CONST_MOD_HPP
#define BOOST_RANDOM_CONST_MOD_HPP

#include <boost/assert.hpp>
#include <boost/static_assert.hpp>
#include <boost/integer_traits.hpp>
#include <boost/type_traits/make_unsigned.hpp>
#include <boost/random/detail/large_arithmetic.hpp>

#include <boost/random/detail/disable_warnings.hpp>

namespace lslboost {
namespace random {

template<class IntType, IntType m>
class const_mod
{
public:
  static IntType apply(IntType x)
  {
    if(((unsigned_m() - 1) & unsigned_m()) == 0)
      return (unsigned_type(x)) & (unsigned_m() - 1);
    else {
      IntType suppress_warnings = (m == 0);
      BOOST_ASSERT(suppress_warnings == 0);
      return x % (m + suppress_warnings);
    }
  }

  static IntType add(IntType x, IntType c)
  {
    if(((unsigned_m() - 1) & unsigned_m()) == 0)
      return (unsigned_type(x) + unsigned_type(c)) & (unsigned_m() - 1);
    else if(c == 0)
      return x;
    else if(x < m - c)
      return x + c;
    else
      return x - (m - c);
  }

  static IntType mult(IntType a, IntType x)
  {
    if(((unsigned_m() - 1) & unsigned_m()) == 0)
      return unsigned_type(a) * unsigned_type(x) & (unsigned_m() - 1);
    else if(a == 0)
      return 0;
    else if(a == 1)
      return x;
    else if(m <= traits::const_max/a)      // i.e. a*m <= max
      return mult_small(a, x);
    else if(traits::is_signed && (m%a < m/a))
      return mult_schrage(a, x);
    else
      return mult_general(a, x);
  }

  static IntType mult_add(IntType a, IntType x, IntType c)
  {
    if(((unsigned_m() - 1) & unsigned_m()) == 0)
      return (unsigned_type(a) * unsigned_type(x) + unsigned_type(c)) & (unsigned_m() - 1);
    else if(a == 0)
      return c;
    else if(m <= (traits::const_max-c)/a) {  // i.e. a*m+c <= max
      IntType suppress_warnings = (m == 0);
      BOOST_ASSERT(suppress_warnings == 0);
      return (a*x+c) % (m + suppress_warnings);
    } else
      return add(mult(a, x), c);
  }

  static IntType pow(IntType a, lslboost::uintmax_t exponent)
  {
      IntType result = 1;
      while(exponent != 0) {
          if(exponent % 2 == 1) {
              result = mult(result, a);
          }
          a = mult(a, a);
          exponent /= 2;
      }
      return result;
  }

  static IntType invert(IntType x)
  { return x == 0 ? 0 : (m == 0? invert_euclidian0(x) : invert_euclidian(x)); }

private:
  typedef integer_traits<IntType> traits;
  typedef typename make_unsigned<IntType>::type unsigned_type;

  const_mod();      // don't instantiate

  static IntType mult_small(IntType a, IntType x)
  {
    IntType suppress_warnings = (m == 0);
    BOOST_ASSERT(suppress_warnings == 0);
    return a*x % (m + suppress_warnings);
  }

  static IntType mult_schrage(IntType a, IntType value)
  {
    const IntType q = m / a;
    const IntType r = m % a;

    BOOST_ASSERT(r < q);        // check that overflow cannot happen

    return sub(a*(value%q), r*(value/q));
  }

  static IntType mult_general(IntType a, IntType b)
  {
    IntType suppress_warnings = (m == 0);
    BOOST_ASSERT(suppress_warnings == 0);
    IntType modulus = m + suppress_warnings;
    BOOST_ASSERT(modulus == m);
    if(::lslboost::uintmax_t(modulus) <=
        (::std::numeric_limits< ::lslboost::uintmax_t>::max)() / modulus)
    {
      return static_cast<IntType>(lslboost::uintmax_t(a) * b % modulus);
    } else {
      return static_cast<IntType>(detail::mulmod(a, b, modulus));
    }
  }

  static IntType sub(IntType a, IntType b)
  {
    if(a < b)
      return m - (b - a);
    else
      return a - b;
  }

  static unsigned_type unsigned_m()
  {
      if(m == 0) {
          return unsigned_type((std::numeric_limits<IntType>::max)()) + 1;
      } else {
          return unsigned_type(m);
      }
  }

  // invert c in the finite field (mod m) (m must be prime)
  static IntType invert_euclidian(IntType c)
  {
    // we are interested in the gcd factor for c, because this is our inverse
    BOOST_ASSERT(c > 0);
    IntType l1 = 0;
    IntType l2 = 1;
    IntType n = c;
    IntType p = m;
    for(;;) {
      IntType q = p / n;
      l1 += q * l2;
      p -= q * n;
      if(p == 0)
        return l2;
      IntType q2 = n / p;
      l2 += q2 * l1;
      n -= q2 * p;
      if(n == 0)
        return m - l1;
    }
  }

  // invert c in the finite field (mod m) (c must be relatively prime to m)
  static IntType invert_euclidian0(IntType c)
  {
    // we are interested in the gcd factor for c, because this is our inverse
    BOOST_ASSERT(c > 0);
    if(c == 1) return 1;
    IntType l1 = 0;
    IntType l2 = 1;
    IntType n = c;
    IntType p = m;
    IntType max = (std::numeric_limits<IntType>::max)();
    IntType q = max / n;
    BOOST_ASSERT(max % n != n - 1 && "c must be relatively prime to m.");
    l1 += q * l2;
    p = max - q * n + 1;
    for(;;) {
      if(p == 0)
        return l2;
      IntType q2 = n / p;
      l2 += q2 * l1;
      n -= q2 * p;
      if(n == 0)
        return m - l1;
      q = p / n;
      l1 += q * l2;
      p -= q * n;
    }
  }
};

} // namespace random
} // namespace lslboost

#include <boost/random/detail/enable_warnings.hpp>

#endif // BOOST_RANDOM_CONST_MOD_HPP


// 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__


/// ---------------------------------------------------------------------------
/// @section LICENSE
///
/// Copyright (c) 2016 Georgia Tech Research Institute (GTRI)
///               All Rights Reserved
///
/// The above copyright notice and this permission notice shall be included in
/// all copies or substantial portions of the Software.
///
/// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
/// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
/// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
/// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
/// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
/// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
/// DEALINGS IN THE SOFTWARE.
/// ---------------------------------------------------------------------------
/// @file filename.ext
/// @author Kevin DeMarco <kevin.demarco@gtri.gatech.edu>
/// @author Eric Squires <eric.squires@gtri.gatech.edu>
/// @version 1.0
/// ---------------------------------------------------------------------------
/// @brief A brief description.
///
/// @section DESCRIPTION
/// A long description.
/// ---------------------------------------------------------------------------
#include <scrimmage/common/Utilities.h>
#include <scrimmage/parse/ParseUtils.h>
#include <scrimmage/plugin_manager/RegisterPlugin.h>
#include <scrimmage/entity/Entity.h>
#include <scrimmage/math/State.h>
#include <scrimmage/entity/Entity.h>
#include <boost/algorithm/clamp.hpp>

#include "Unicycle.h"

REGISTER_PLUGIN(scrimmage::MotionModel, Unicycle,
                Unicycle_plugin)

namespace sc = scrimmage;

namespace pl = std::placeholders;

enum ModelParams
{
    X = 0,
    Y,
    THETA,
    MODEL_NUM_ITEMS
};

Unicycle::Unicycle()
{
    x_.resize(MODEL_NUM_ITEMS);
}

bool Unicycle::init(std::map<std::string, std::string> &info,
                            std::map<std::string, std::string> &params)
{
    x_[X] = state_->pos()(0);
    x_[Y] = state_->pos()(1);
    x_[THETA] = state_->quat().yaw();

    turn_rate_max_ = std::stod(params.at("turn_rate_max"));
    vel_max_ = std::stod(params.at("vel_max"));
    return true;
}

bool Unicycle::step(double t, double dt)
{
    u_ = std::static_pointer_cast<Controller>(parent_.lock()->controllers().back())->u();

    double prev_x = x_[X];
    double prev_y = x_[Y];

    ode_step(dt);

    double dx = (x_[X] - prev_x) / dt;
    double dy = (x_[Y] - prev_y) / dt;

    state_->vel()(0) = dx;
    state_->vel()(1) = dy;

    state_->pos()(0) = x_[X];
    state_->pos()(1) = x_[Y];

    state_->quat().set(0, 0, x_[THETA]);

    return true;
}

void Unicycle::model(const vector_t &x , vector_t &dxdt , double t)
{
    double vel = boost::algorithm::clamp(u_(0), -vel_max_, vel_max_);
    double yaw_rate = boost::algorithm::clamp(u_(1), -turn_rate_max_, turn_rate_max_);
    dxdt[X] = vel * cos(x[THETA]);
    dxdt[Y] = vel * sin(x[THETA]);
    dxdt[THETA] = yaw_rate;
}


//  (C) Copyright Matt Borland 2021.
//  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 <cmath>
#include <cfloat>
#include <cstdint>
#include <limits>
#include <utility>
#include <type_traits>
#include <boost/math/ccmath/modf.hpp>
#include <boost/math/ccmath/isnan.hpp>
#include <boost/math/ccmath/isinf.hpp>

#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
#endif

template <typename T>
inline constexpr T floating_point_value(const T val)
{
    T i = 0;
    const T ans = boost::math::ccmath::modf(val, &i);

    return ans;
}

template <typename T>
inline constexpr T integral_value(const T val)
{
    T i = 0;
    boost::math::ccmath::modf(val, &i);

    return i;
}

template <typename T>
inline constexpr std::pair<T, T> pair_value(const T val)
{
    T i = 0;
    T ans = boost::math::ccmath::modf(val, &i);

    return std::make_pair(i, ans);
}

template <typename T>
constexpr void test()
{
    if constexpr (std::numeric_limits<T>::has_quiet_NaN)
    {
        constexpr std::pair<T, T> NaN_val = pair_value(std::numeric_limits<T>::quiet_NaN());
        static_assert(boost::math::ccmath::isnan(NaN_val.first));
        static_assert(boost::math::ccmath::isnan(NaN_val.second));
    }

    // if x is +-0, +-0 is returned and +-0 is stored in *iptr
    static_assert(floating_point_value(T(0)) == 0);
    static_assert(floating_point_value(-T(0)) == -0);
    static_assert(integral_value(T(0)) == 0);
    static_assert(integral_value(-T(0)) == -0);

    // if x is +- inf, +-0 is returned and +-inf is stored in *iptr
    static_assert(floating_point_value(std::numeric_limits<T>::infinity()) == 0);
    static_assert(floating_point_value(-std::numeric_limits<T>::infinity()) == -0);
    static_assert(integral_value(std::numeric_limits<T>::infinity()) == std::numeric_limits<T>::infinity());
    static_assert(integral_value(-std::numeric_limits<T>::infinity()) == -std::numeric_limits<T>::infinity());

    // The returned value is exact, the current rounding mode is ignored
    // The return value and *iptr each have the same type and sign as x
    static_assert(integral_value(T(123.45)) == 123);
    static_assert(integral_value(T(-234.56)) == -234);
    static_assert(floating_point_value(T(1.0/2)) == T(1.0/2));
    static_assert(floating_point_value(T(-1.0/3)) == T(-1.0/3));
}

#if !defined(BOOST_MATH_NO_CONSTEXPR_DETECTION) && !defined(BOOST_MATH_USING_BUILTIN_CONSTANT_P)
int main()
{
    test<float>();
    test<double>();

    #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test<long double>();
    #endif

    #ifdef BOOST_HAS_FLOAT128
    test<boost::multiprecision::float128>();
    #endif

    return 0;
}
#else
int main()
{
    return 0;
}
#endif