// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/base.hpp>
#include <laws/comparable.hpp>
#include <laws/euclidean_ring.hpp>
#include <laws/group.hpp>
#include <laws/logical.hpp>
#include <laws/monoid.hpp>
#include <laws/orderable.hpp>
#include <support/cnumeric.hpp>
#include <support/numeric.hpp>

#include <cstdlib>
#include <vector>
using namespace boost::hana;

struct invalid {
    template <typename T>
    operator T const() { std::abort(); }
};

int main() {
    //////////////////////////////////////////////////////////////////////////
    // Comparable
    //////////////////////////////////////////////////////////////////////////
    {
        test::_injection<0> f{};
        auto x = numeric(1);
        auto y = numeric(2);

        // equal
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(x, x));
            BOOST_HANA_CONSTEXPR_CHECK(not_(equal(x, y)));
        }

        // not_equal
        {
            BOOST_HANA_CONSTEXPR_CHECK(not_equal(x, y));
            BOOST_HANA_CONSTEXPR_CHECK(not_(not_equal(x, x)));
        }

        // comparing
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                comparing(f)(x, x),
                equal(f(x), f(x))
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                comparing(f)(x, y),
                equal(f(x), f(y))
            ));
        }
    }

    //////////////////////////////////////////////////////////////////////////
    // Orderable
    //////////////////////////////////////////////////////////////////////////
    {
        auto ord = numeric;

        // test::_injection is also monotonic
        test::_injection<0> f{};

        // less
        {
            BOOST_HANA_CONSTEXPR_CHECK(less(ord(0), ord(1)));
            BOOST_HANA_CONSTEXPR_CHECK(not_(less(ord(0), ord(0))));
            BOOST_HANA_CONSTEXPR_CHECK(not_(less(ord(1), ord(0))));
        }

        // less_equal
        {
            BOOST_HANA_CONSTEXPR_CHECK(less_equal(ord(0), ord(1)));
            BOOST_HANA_CONSTEXPR_CHECK(less_equal(ord(0), ord(0)));
            BOOST_HANA_CONSTEXPR_CHECK(not_(less_equal(ord(1), ord(0))));
        }

        // greater_equal
        {
            BOOST_HANA_CONSTEXPR_CHECK(greater_equal(ord(1), ord(0)));
            BOOST_HANA_CONSTEXPR_CHECK(greater_equal(ord(0), ord(0)));
            BOOST_HANA_CONSTEXPR_CHECK(not_(greater_equal(ord(0), ord(1))));
        }

        // greater
        {
            BOOST_HANA_CONSTEXPR_CHECK(greater(ord(1), ord(0)));
            BOOST_HANA_CONSTEXPR_CHECK(not_(greater(ord(0), ord(0))));
            BOOST_HANA_CONSTEXPR_CHECK(not_(greater(ord(0), ord(1))));
        }

        // max
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                max(ord(0), ord(0)), ord(0)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                max(ord(1), ord(0)), ord(1)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                max(ord(0), ord(1)), ord(1)
            ));
        }

        // min
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                min(ord(0), ord(0)),
                ord(0)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                min(ord(1), ord(0)),
                ord(0)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                min(ord(0), ord(1)),
                ord(0)
            ));
        }

        // ordering
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                ordering(f)(ord(1), ord(0)),
                less(f(ord(1)), f(ord(0)))
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                ordering(f)(ord(0), ord(1)),
                less(f(ord(0)), f(ord(1)))
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                ordering(f)(ord(0), ord(0)),
                less(f(ord(0)), f(ord(0)))
            ));
        }
    }

    //////////////////////////////////////////////////////////////////////////
    // Monoid
    //////////////////////////////////////////////////////////////////////////
    {
        constexpr int x = 2, y = 3;

        // zero
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                zero<Numeric>(), numeric(0)
            ));
        }

        // plus
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                plus(numeric(x), numeric(y)),
                numeric(x + y)
            ));
        }
    }

    //////////////////////////////////////////////////////////////////////////
    // Group
    //////////////////////////////////////////////////////////////////////////
    {
        constexpr int x = 2, y = 3;

        // minus
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                minus(numeric(x), numeric(y)),
                numeric(x - y)
            ));
        }

        // negate
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                negate(numeric(x)),
                numeric(-x)
            ));
        }
    }

    //////////////////////////////////////////////////////////////////////////
    // Ring
    //////////////////////////////////////////////////////////////////////////
    {
        constexpr int x = 2, y = 3;

        // one
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                one<Numeric>(),
                numeric(1)
            ));
        }

        // mult
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                mult(numeric(x), numeric(y)),
                numeric(x * y)
            ));
        }

        // power
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                power(numeric(x), zero<CNumeric<int>>()),
                one<Numeric>()
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                power(numeric(x), one<CNumeric<int>>()),
                numeric(x)
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                power(numeric(x), cnumeric<int, 2>),
                mult(numeric(x), numeric(x))
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                power(numeric(x), cnumeric<int, 3>),
                mult(mult(numeric(x), numeric(x)), numeric(x))
            ));
        }
    }

    //////////////////////////////////////////////////////////////////////////
    // EuclideanRing
    //////////////////////////////////////////////////////////////////////////
    {
        constexpr int x = 6, y = 3, z = 4;

        // div
        {
            using boost::hana::div; // hide ::div
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                div(numeric(x), numeric(y)),
                numeric(x / y)
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                div(numeric(x), numeric(z)),
                 numeric(x/ z)
            ));
        }

        // mod
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                mod(numeric(x), numeric(y)),
                numeric(x % y)
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                mod(numeric(x), numeric(z)),
                numeric(x % z)
            ));
        }
    }

    //////////////////////////////////////////////////////////////////////////
    // Logical
    //////////////////////////////////////////////////////////////////////////
    {
        auto logical = numeric;
        auto comparable = numeric;

        // not_
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                not_(logical(true)),
                logical(false)
            ));
        }

        // and_
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                and_(logical(true)),
                logical(true)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                and_(logical(false)),
                logical(false)
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                and_(logical(true), logical(true)),
                logical(true)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                and_(logical(true), logical(false)),
                logical(false)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                and_(logical(false), invalid{}),
                logical(false)
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                and_(logical(true), logical(true), logical(true)),
                logical(true)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                and_(logical(true), logical(true), logical(false)),
                logical(false)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                and_(logical(true), logical(false), invalid{}),
                logical(false)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                and_(logical(false), invalid{}, invalid{}),
                logical(false)
            ));
        }

        // or_
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                or_(logical(true)),
                logical(true)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                or_(logical(false)),
                logical(false)
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                or_(logical(false), logical(false)),
                logical(false)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                or_(logical(false), logical(true)),
                logical(true)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                or_(logical(true), invalid{}),
                logical(true)
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                or_(logical(false), logical(false), logical(false)),
                logical(false)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                or_(logical(false), logical(false), logical(true)),
                logical(true)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                or_(logical(false), logical(true), invalid{}),
                logical(true)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                or_(logical(true), invalid{}, invalid{}),
                logical(true)
            ));
        }

        // if_
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                if_(logical(true), comparable(0), comparable(1)),
                comparable(0)
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                if_(logical(false), comparable(0), comparable(1)),
                comparable(1)
            ));
        }

        // eval_if
        {
            auto t = [=](auto) { return comparable(0); };
            auto e = [=](auto) { return comparable(1); };

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                eval_if(logical(true), t, e),
                comparable(0)
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                eval_if(logical(false), t, e),
                comparable(1)
            ));
        }

        // while_
        {
            auto smaller_than = [](auto n) {
                return [n](auto v) { return v.size() < n; };
            };
            auto f = [](auto v) {
                v.push_back(v.size());
                return v;
            };

            BOOST_HANA_RUNTIME_CHECK(equal(
                while_(smaller_than(0u), std::vector<int>{}, f),
                std::vector<int>{}
            ));

            BOOST_HANA_RUNTIME_CHECK(equal(
                while_(smaller_than(1u), std::vector<int>{}, f),
                std::vector<int>{0}
            ));

            BOOST_HANA_RUNTIME_CHECK(equal(
                while_(smaller_than(2u), std::vector<int>{}, f),
                std::vector<int>{0, 1}
            ));

            BOOST_HANA_RUNTIME_CHECK(equal(
                while_(smaller_than(3u), std::vector<int>{}, f),
                std::vector<int>{0, 1, 2}
            ));

            BOOST_HANA_RUNTIME_CHECK(equal(
                while_(smaller_than(4u), std::vector<int>{}, f),
                std::vector<int>{0, 1, 2, 3}
            ));

            // Make sure it can be called with an lvalue state:
            std::vector<int> v{};
            BOOST_HANA_RUNTIME_CHECK(equal(
                while_(smaller_than(4u), v, f),
                std::vector<int>{0, 1, 2, 3}
            ));
        }

        // while_
        {
            auto less_than = [](auto n) {
                return [n](auto v) { return v.size() < n; };
            };
            auto f = [](auto v) {
                v.push_back(v.size());
                return v;
            };

            BOOST_HANA_RUNTIME_CHECK(equal(
                while_(less_than(0u), std::vector<int>{}, f),
                std::vector<int>{}
            ));

            BOOST_HANA_RUNTIME_CHECK(equal(
                while_(less_than(1u), std::vector<int>{}, f),
                std::vector<int>{0}
            ));

            BOOST_HANA_RUNTIME_CHECK(equal(
                while_(less_than(2u), std::vector<int>{}, f),
                std::vector<int>{0, 1}
            ));

            BOOST_HANA_RUNTIME_CHECK(equal(
                while_(less_than(3u), std::vector<int>{}, f),
                std::vector<int>{0, 1, 2}
            ));

            BOOST_HANA_RUNTIME_CHECK(equal(
                while_(less_than(4u), std::vector<int>{}, f),
                std::vector<int>{0, 1, 2, 3}
            ));

            // Make sure it can be called with an lvalue state:
            std::vector<int> v{};
            BOOST_HANA_RUNTIME_CHECK(equal(
                while_(less_than(4u), v, f),
                std::vector<int>{0, 1, 2, 3}
            ));
        }
    }
}


/* boost random/sobol.hpp header file
 *
 * Copyright Justinas Vygintas Daugmaudis 2010-2018
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_RANDOM_SOBOL_HPP
#define BOOST_RANDOM_SOBOL_HPP

#include <boost/random/detail/sobol_table.hpp>
#include <boost/random/detail/gray_coded_qrng.hpp>
#include <boost/assert.hpp>

namespace boost {
namespace random {

/** @cond */
namespace qrng_detail {

// sobol_lattice sets up the random-number generator to produce a Sobol
// sequence of at most max dims-dimensional quasi-random vectors.
// Adapted from ACM TOMS algorithm 659, see

// http://doi.acm.org/10.1145/42288.214372

template<typename UIntType, unsigned w, typename SobolTables>
struct sobol_lattice
{
  typedef UIntType value_type;

  BOOST_STATIC_ASSERT(w > 0u);
  BOOST_STATIC_CONSTANT(unsigned, bit_count = w);

private:
  typedef std::vector<value_type> container_type;

public:
  explicit sobol_lattice(std::size_t dimension)
  {
    resize(dimension);
  }

  // default copy c-tor is fine

  void resize(std::size_t dimension)
  {
    dimension_assert("Sobol", dimension, SobolTables::max_dimension);

    // Initialize the bit array
    container_type cj(bit_count * dimension);

    // Initialize direction table in dimension 0
    for (unsigned k = 0; k != bit_count; ++k)
      cj[dimension*k] = static_cast<value_type>(1);

    // Initialize in remaining dimensions.
    for (std::size_t dim = 1; dim < dimension; ++dim)
    {
      const typename SobolTables::value_type poly = SobolTables::polynomial(dim-1);
      if (poly > (std::numeric_limits<value_type>::max)()) {
        boost::throw_exception( std::range_error("sobol: polynomial value outside the given value type range") );
      }
      const unsigned degree = qrng_detail::msb(poly); // integer log2(poly)

      // set initial values of m from table
      for (unsigned k = 0; k != degree; ++k)
        cj[dimension*k + dim] = SobolTables::minit(dim-1, k);

      // Calculate remaining elements for this dimension,
      // as explained in Bratley+Fox, section 2.
      for (unsigned j = degree; j < bit_count; ++j)
      {
        typename SobolTables::value_type p_i = poly;
        const std::size_t bit_offset = dimension*j + dim;

        cj[bit_offset] = cj[dimension*(j-degree) + dim];
        for (unsigned k = 0; k != degree; ++k, p_i >>= 1)
        {
          int rem = degree - k;
          cj[bit_offset] ^= ((p_i & 1) * cj[dimension*(j-rem) + dim]) << rem;
        }
      }
    }

    // Shift columns by appropriate power of 2.
    unsigned p = 1u;
    for (int j = bit_count-1-1; j >= 0; --j, ++p)
    {
      const std::size_t bit_offset = dimension * j;
      for (std::size_t dim = 0; dim != dimension; ++dim)
        cj[bit_offset + dim] <<= p;
    }

    bits.swap(cj);
  }

  typename container_type::const_iterator iter_at(std::size_t n) const
  {
    BOOST_ASSERT(!(n > bits.size()));
    return bits.begin() + n;
  }

private:
  container_type bits;
};

} // namespace qrng_detail

typedef detail::qrng_tables::sobol default_sobol_table;

/** @endcond */

//!Instantiations of class template sobol_engine model a \quasi_random_number_generator.
//!The sobol_engine uses the algorithm described in
//! \blockquote
//![Bratley+Fox, TOMS 14, 88 (1988)]
//!and [Antonov+Saleev, USSR Comput. Maths. Math. Phys. 19, 252 (1980)]
//! \endblockquote
//!
//!\attention sobol_engine skips trivial zeroes at the start of the sequence. For example, the beginning
//!of the 2-dimensional Sobol sequence in @c uniform_01 distribution will look like this:
//!\code{.cpp}
//!0.5, 0.5,
//!0.75, 0.25,
//!0.25, 0.75,
//!0.375, 0.375,
//!0.875, 0.875,
//!...
//!\endcode
//!
//!In the following documentation @c X denotes the concrete class of the template
//!sobol_engine returning objects of type @c UIntType, u and v are the values of @c X.
//!
//!Some member functions may throw exceptions of type @c std::range_error. This
//!happens when the quasi-random domain is exhausted and the generator cannot produce
//!any more values. The length of the low discrepancy sequence is given by \f$L=Dimension \times (2^{w} - 1)\f$.
template<typename UIntType, unsigned w, typename SobolTables = default_sobol_table>
class sobol_engine
  : public qrng_detail::gray_coded_qrng<
      qrng_detail::sobol_lattice<UIntType, w, SobolTables>
    >
{
  typedef qrng_detail::sobol_lattice<UIntType, w, SobolTables> lattice_t;
  typedef qrng_detail::gray_coded_qrng<lattice_t> base_t;

public:
  //!Effects: Constructs the default `s`-dimensional Sobol quasi-random number generator.
  //!
  //!Throws: bad_alloc, invalid_argument, range_error.
  explicit sobol_engine(std::size_t s)
    : base_t(s)
  {}

  // default copy c-tor is fine

#ifdef BOOST_RANDOM_DOXYGEN
  //=========================Doxygen needs this!==============================
  typedef UIntType result_type;

  /** @copydoc boost::random::niederreiter_base2_engine::min() */
  static BOOST_CONSTEXPR result_type min BOOST_PREVENT_MACRO_SUBSTITUTION ()
  { return (base_t::min)(); }

  /** @copydoc boost::random::niederreiter_base2_engine::max() */
  static BOOST_CONSTEXPR result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ()
  { return (base_t::max)(); }

  /** @copydoc boost::random::niederreiter_base2_engine::dimension() */
  std::size_t dimension() const { return base_t::dimension(); }

  /** @copydoc boost::random::niederreiter_base2_engine::seed() */
  void seed()
  {
    base_t::seed();
  }

  /** @copydoc boost::random::niederreiter_base2_engine::seed(UIntType) */
  void seed(UIntType init)
  {
    base_t::seed(init);
  }

  /** @copydoc boost::random::niederreiter_base2_engine::operator()() */
  result_type operator()()
  {
    return base_t::operator()();
  }

  /** @copydoc boost::random::niederreiter_base2_engine::discard(boost::uintmax_t) */
  void discard(boost::uintmax_t z)
  {
    base_t::discard(z);
  }

  /** Returns true if the two generators will produce identical sequences of outputs. */
  BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(sobol_engine, x, y)
  { return static_cast<const base_t&>(x) == y; }

  /** Returns true if the two generators will produce different sequences of outputs. */
  BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(sobol_engine)

  /** Writes the textual representation of the generator to a @c std::ostream. */
  BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, sobol_engine, s)
  { return os << static_cast<const base_t&>(s); }

  /** Reads the textual representation of the generator from a @c std::istream. */
  BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, sobol_engine, s)
  { return is >> static_cast<base_t&>(s); }

#endif // BOOST_RANDOM_DOXYGEN
};

/**
 * @attention This specialization of \sobol_engine supports up to 3667 dimensions.
 *
 * Data on the primitive binary polynomials `a` and the corresponding starting values `m`
 * for Sobol sequences in up to 21201 dimensions was taken from
 *
 *  @blockquote
 *  S. Joe and F. Y. Kuo, Constructing Sobol sequences with better two-dimensional projections,
 *  SIAM J. Sci. Comput. 30, 2635-2654 (2008).
 *  @endblockquote
 *
 * See the original tables up to dimension 21201: https://web.archive.org/web/20170802022909/http://web.maths.unsw.edu.au/~fkuo/sobol/new-joe-kuo-6.21201
 *
 * For practical reasons the default table uses only the subset of binary polynomials `a` < 216.
 *
 * However, it is possible to provide your own table to \sobol_engine should the default one be insufficient.
 */
typedef sobol_engine<boost::uint_least64_t, 64u, default_sobol_table> sobol;

} // namespace random

} // namespace boost

#endif // BOOST_RANDOM_SOBOL_HPP


#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>

#include <Eigen/Dense>
using Eigen::Matrix;
using Eigen::Dynamic;

#include "ceres/autodiff_cost_function.h"
#include "ceres/cost_function.h"
#include "ceres/internal/autodiff.h"
#include "ceres/internal/eigen.h"

#include <glog/logging.h>

#include <adept.h>

#include <game/vsr/cga_op.h>

#include <hep/ga.hpp>

#include <iostream>
#include <sstream>
#include <string>

using namespace vsr::cga;
namespace py = pybind11;

template <typename T>
void InnerProductVectorBivector(const T *vec, const T *biv, T *res) {
  Vector<T> vector(vec);
  Bivector<T> bivector(biv);
  Vector<T> result = vector <= bivector;
  for (int i = 0; i < 3; ++i)
    res[i] = result[i];
}

template <typename T>
void InnerProductVectorVector(const T *vec, const T *vec1, T *res) {
  Vector<T> vector(vec);
  Vector<T> vector1(vec1);
  Scalar<T> result = vector <= vector1;
  res[0] = result[0];
}

template <typename T>
void InnerProductVectorBivector2(const T *a, const T *b, T *res) {
  res[0] = -a[2] * b[1] - a[1] * b[0];
  res[1] = -a[2] * b[2] + a[0] * b[0];
  res[2] = a[1] * b[2] + a[0] * b[1];
}

struct InnerProductVectorBivectorFunctor2 {
  template <typename T>
  bool operator()(const T *vec, const T *biv, T *res) const {
    InnerProductVectorBivector2(vec, biv, res);
    return true;
  }
};

py::list AdeptDiffInnerProductVectorBivector(const Vec &vec, const Biv &biv) {
  auto result = py::array(py::buffer_info(
      nullptr,        /* Pointer to data (nullptr -> ask NumPy to allocate!) */
      sizeof(double), /* Size of one item */
      py::format_descriptor<double>::value(), /* Buffer format */
      2,                                      /* How many dimensions? */
      {3, 3},                 /* Number of elements for each dimension */
      {sizeof(double) * 3, 3} /* Strides for each dimension */
      ));

  auto buf = result.request();

  adept::Stack stack;

  double jac[9];
  std::array<adept::adouble, 3> vector{vec[0], vec[1], vec[2]};
  std::array<adept::adouble, 3> bivector{biv[0], biv[1], biv[2]};
  stack.new_recording();
  std::array<adept::adouble, 3> vec_ip_biv{0.0, 0.0, 0.0};

  InnerProductVectorBivector2(vector.begin(), bivector.begin(),
                              vec_ip_biv.begin());

  stack.independent(vector.begin(), 3);
  // stack.independent(bivector.begin(), 3);
  stack.dependent(vec_ip_biv.begin(), 3);

  // stack.jacobian(static_cast<double *>(buf.ptr), true);
  stack.jacobian_reverse(static_cast<double *>(buf.ptr), true);
  // stack.jacobian_forward(static_cast<double *>(buf.ptr), true);

  std::stringstream ss;
  stack.print_statements(ss);

  py::list list;
  list.append(result);
  list.append(py::str(ss.str().c_str()));

  return list;
}

struct InnerProductVectorBivectorFunctor {
  template <typename T>
  bool operator()(const T *vec, const T *biv, T *res) const {
    InnerProductVectorBivector(vec, biv, res);
    return true;
  }
};

struct InnerProductVectorVectorFunctor {
  template <typename T>
  bool operator()(const T *vec, const T *vec2, T *res) const {
    InnerProductVectorVector(vec, vec2, res);
    return true;
  }
};

py::list CeresDiffInnerProductVectorVector(const Vec &vec, const Vec &vec2) {
  auto py_array_jac = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {1, 3}, {sizeof(double) * 3, sizeof(double)}));

  auto py_array_result = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {1, 1}, {sizeof(double), sizeof(double)}));

  auto buf_jac = py_array_jac.request();
  auto buf_res = py_array_result.request();

  const double *parameters[2] = {vec.begin(), vec2.begin()};
  double *jacobians[2] = {static_cast<double *>(buf_jac.ptr), nullptr};

  ceres::AutoDiffCostFunction<InnerProductVectorVectorFunctor, 1, 3, 3>(
      new InnerProductVectorVectorFunctor())
      .Evaluate(parameters, static_cast<double *>(buf_res.ptr), jacobians);

  py::list list;
  list.append(py_array_result);
  list.append(py_array_jac);

  return list;
}
py::list CeresDiffInnerProductVectorBivector(const Vec &vec, const Biv &biv) {
  auto py_array_jac = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {3, 3}, {sizeof(double) * 3, sizeof(double)}));

  auto py_array_result = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {3, 1}, {sizeof(double) * 3, sizeof(double)}));

  auto buf_jac = py_array_jac.request();
  auto buf_res = py_array_result.request();

  const double *parameters[2] = {vec.begin(), biv.begin()};
  double *jacobians[2] = {static_cast<double *>(buf_jac.ptr), nullptr};

  ceres::AutoDiffCostFunction<InnerProductVectorBivectorFunctor, 3, 3, 3>(
      new InnerProductVectorBivectorFunctor())
      .Evaluate(parameters, static_cast<double *>(buf_res.ptr), jacobians);

  py::list list;
  list.append(py_array_result);
  list.append(py_array_jac);

  return list;
}

template <typename T> using Matrix4 = Eigen::Matrix<T, 4, 4>;

template <typename T> inline static Matrix4<T> s() {
  Matrix4<T> m;
  m << T(1), T(0), T(0), T(0), T(0), T(1), T(0), T(0), T(0), T(0), T(1), T(0),
      T(0), T(0), T(0), T(1);
  return m;
}

template <typename T> inline static Matrix4<T> e1() {
  Matrix4<T> m;
  m << T(0), T(0), T(0), T(1), T(0), T(0), T(1), T(0), T(0), T(1), T(0), T(0),
      T(1), T(0), T(0), T(0);
  return m;
}

template <typename T> inline static Matrix4<T> e2() {
  Matrix4<T> m;
  m << T(0), T(0), T(1), T(0), T(0), T(0), T(0), T(-1), T(1), T(0), T(0), T(0),
      T(0), T(-1), T(0), T(0);
  return m;
}

template <typename T> inline static Matrix4<T> e3() {
  Matrix4<T> m;
  m << T(1), T(0), T(0), T(0), T(0), T(1), T(0), T(0), T(0), T(0), T(-1), T(0),
      T(0), T(0), T(0), T(-1);
  return m;
}

struct DiffRotorMatrixFunctor {
  template <typename T> bool operator()(const T *th, const T *a, T *b) const {
    Matrix4<T> rotor =
        cos(T(0.5) * th[0]) * s<T>() - sin(T(0.5) * th[0]) * e1<T>() * e2<T>();
    Matrix4<T> rotor_inv =
        cos(T(0.5) * th[0]) * s<T>() + sin(T(0.5) * th[0]) * e1<T>() * e2<T>();
    Matrix4<T> vec_a = a[0] * e1<T>() + a[1] * e2<T>() + a[2] * e3<T>();
    Matrix4<T> vec_b = rotor * vec_a * rotor_inv;
    b[0] = vec_b(0, 3); // e1
    b[1] = vec_b(0, 2); // e2
    b[2] = vec_b(0, 0); // e3
    return true;
  }
};

py::list CeresDiffRotorMatrix(const double theta, const Vec &a) {
  auto py_array_jac = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {3, 1}, {sizeof(double), sizeof(double)}));

  auto py_array_result = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {3, 1}, {sizeof(double), sizeof(double)}));

  auto buf_jac = py_array_jac.request();
  auto buf_res = py_array_result.request();

  const double *parameters[2] = {&theta, a.begin()};
  double *jacobians[2] = {static_cast<double *>(buf_jac.ptr), nullptr};

  ceres::AutoDiffCostFunction<DiffRotorMatrixFunctor, 3, 1, 3>(
      new DiffRotorMatrixFunctor())
      .Evaluate(parameters, static_cast<double *>(buf_res.ptr), jacobians);

  py::list list;
  list.append(py_array_result);
  list.append(py_array_jac);

  return list;
}

struct DiffRotorVersorFunctor {
  template <typename T> bool operator()(const T *th, const T *a, T *b) const {
    Rotor<T> rotor{cos(T(0.5) * th[0]), -sin(T(0.5) * th[0]), T(0.0), T(0.0)};
    Vector<T> vec_a{a[0], a[1], a[2]};
    Vector<T> vec_b = vec_a.spin(rotor);
    for (int i = 0; i < 3; ++i)
      b[i] = vec_b[i];
    return true;
  }
};
py::list CeresDiffRotorVersor(const double theta, const Vec &a) {
  auto py_array_jac = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {3, 1}, {sizeof(double), sizeof(double)}));

  auto py_array_result = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {3, 1}, {sizeof(double), sizeof(double)}));

  auto buf_jac = py_array_jac.request();
  auto buf_res = py_array_result.request();

  const double *parameters[2] = {&theta, a.begin()};
  double *jacobians[2] = {static_cast<double *>(buf_jac.ptr), nullptr};

  ceres::AutoDiffCostFunction<DiffRotorVersorFunctor, 3, 1, 3>(
      new DiffRotorVersorFunctor())
      .Evaluate(parameters, static_cast<double *>(buf_res.ptr), jacobians);

  py::list list;
  list.append(py_array_result);
  list.append(py_array_jac);

  return list;
}

py::list AdeptDiffRotorMatrixForward(const double theta, const Vec &vec) {
  auto py_array_jac = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {3, 1}, {sizeof(double), sizeof(double)}));

  auto py_array_result = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {3, 1}, {sizeof(double), sizeof(double)}));

  auto buf_jac = py_array_jac.request();
  auto buf_res = py_array_result.request();

  adept::Stack stack;
  adept::adouble th{theta};
  std::array<adept::adouble, 3> a{vec[0], vec[1], vec[2]};
  stack.new_recording();
  std::array<adept::adouble, 3> b{0.0, 0.0, 0.0};
  DiffRotorMatrixFunctor()(&th, a.begin(), b.begin());
  stack.independent(&th, 1);
  stack.dependent(b.begin(), 3);

  // stack.jacobian(static_cast<double *>(buf_jac.ptr), true);
  // stack.jacobian_reverse(static_cast<double *>(buf_jac.ptr), true);
  stack.jacobian_forward(static_cast<double *>(buf_jac.ptr), true);

  std::stringstream ss;
  stack.print_statements(ss);

  auto res_ptr = static_cast<double *>(buf_res.ptr);
  for (int i = 0; i < 3; ++i)
    res_ptr[i] = b[i].value();

  py::list list;
  list.append(py_array_result);
  list.append(py_array_jac);
  list.append(py::str(ss.str().c_str()));
  std::stringstream sstatus;
  stack.print_status(sstatus);
  list.append(py::str(sstatus.str().c_str()));

  return list;
}

py::list AdeptDiffRotorMatrixReverse(const double theta, const Vec &vec) {
  auto py_array_jac = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {3, 1}, {sizeof(double), sizeof(double)}));

  auto py_array_result = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {3, 1}, {sizeof(double), sizeof(double)}));

  auto buf_jac = py_array_jac.request();
  auto buf_res = py_array_result.request();

  adept::Stack stack;
  adept::adouble th{theta};
  std::array<adept::adouble, 3> a{vec[0], vec[1], vec[2]};
  stack.new_recording();
  std::array<adept::adouble, 3> b{0.0, 0.0, 0.0};
  DiffRotorMatrixFunctor()(&th, a.begin(), b.begin());
  stack.independent(&th, 1);
  stack.dependent(b.begin(), 3);

  // stack.jacobian(static_cast<double *>(buf_jac.ptr), true);
  stack.jacobian_reverse(static_cast<double *>(buf_jac.ptr), true);
  // stack.jacobian_forward(static_cast<double *>(buf_jac.ptr), true);

  std::stringstream ss;
  stack.print_statements(ss);

  auto res_ptr = static_cast<double *>(buf_res.ptr);
  for (int i = 0; i < 3; ++i)
    res_ptr[i] = b[i].value();

  py::list list;
  list.append(py_array_result);
  list.append(py_array_jac);
  list.append(py::str(ss.str().c_str()));
  std::stringstream sstatus;
  stack.print_status(sstatus);
  list.append(py::str(sstatus.str().c_str()));

  return list;
}

py::list AdeptDiffRotorVersorForward(const double theta, const Vec &vec) {
  auto py_array_jac = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {3, 1}, {sizeof(double), sizeof(double)}));

  auto py_array_result = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {3, 1}, {sizeof(double), sizeof(double)}));

  auto buf_jac = py_array_jac.request();
  auto buf_res = py_array_result.request();

  adept::Stack stack;
  adept::adouble th{theta};
  std::array<adept::adouble, 3> a{vec[0], vec[1], vec[2]};
  stack.new_recording();
  std::array<adept::adouble, 3> b{0.0, 0.0, 0.0};
  DiffRotorVersorFunctor()(&th, a.begin(), b.begin());
  stack.independent(&th, 1);
  stack.dependent(b.begin(), 3);

  // stack.jacobian(static_cast<double *>(buf_jac.ptr), true);
  // stack.jacobian_reverse(static_cast<double *>(buf_jac.ptr), true);
  stack.jacobian_forward(static_cast<double *>(buf_jac.ptr), true);

  std::stringstream ss;
  stack.print_statements(ss);

  auto res_ptr = static_cast<double *>(buf_res.ptr);
  for (int i = 0; i < 3; ++i)
    res_ptr[i] = b[i].value();

  py::list list;
  list.append(py_array_result);
  list.append(py_array_jac);
  list.append(py::str(ss.str().c_str()));
  std::stringstream sstatus;
  stack.print_status(sstatus);
  list.append(py::str(sstatus.str().c_str()));

  return list;
}

py::list AdeptDiffRotorVersorReverse(const double theta, const Vec &vec) {
  auto py_array_jac = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {3, 1}, {sizeof(double), sizeof(double)}));

  auto py_array_result = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {3, 1}, {sizeof(double), sizeof(double)}));

  auto buf_jac = py_array_jac.request();
  auto buf_res = py_array_result.request();

  adept::Stack stack;
  adept::adouble th{theta};
  std::array<adept::adouble, 3> a{vec[0], vec[1], vec[2]};
  stack.new_recording();
  std::array<adept::adouble, 3> b{0.0, 0.0, 0.0};
  DiffRotorVersorFunctor()(&th, a.begin(), b.begin());
  stack.independent(&th, 1);
  stack.dependent(b.begin(), 3);

  // stack.jacobian(static_cast<double *>(buf_jac.ptr), true);
  stack.jacobian_reverse(static_cast<double *>(buf_jac.ptr), true);
  // stack.jacobian_forward(static_cast<double *>(buf_jac.ptr), true);

  std::stringstream ss;
  stack.print_statements(ss);

  auto res_ptr = static_cast<double *>(buf_res.ptr);
  for (int i = 0; i < 3; ++i)
    res_ptr[i] = b[i].value();

  py::list list;
  list.append(py_array_result);
  list.append(py_array_jac);
  list.append(py::str(ss.str().c_str()));
  std::stringstream sstatus;
  stack.print_status(sstatus);
  list.append(py::str(sstatus.str().c_str()));

  return list;
}

struct DiffRotorHepGAFunctor {
  template <typename T> bool operator()(const T *th, const T *a, T *b) const {
    using Algebra = hep::algebra<T, 3, 0>;
    using Rotor = hep::multi_vector<Algebra, hep::list<0, 3, 5, 6>>;
    using Vector = hep::multi_vector<Algebra, hep::list<1, 2, 4>>;
    Rotor rotor{cos(T(0.5) * th[0]), -sin(T(0.5) * th[0]), T(0.0), T(0.0)};
    Vector pnt_a{a[0], a[1], a[2]};
    Vector pnt_b = hep::grade<1>(rotor * pnt_a * ~rotor);
    for (int i = 0; i < 3; ++i)
      b[i] = pnt_b[i];
    return true;
  }
};

py::list AdeptDiffRotorHepGAForward(const double theta, const Vec &vec) {
  auto py_array_jac = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {3, 1}, {sizeof(double), sizeof(double)}));

  auto py_array_result = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {3, 1}, {sizeof(double), sizeof(double)}));

  auto buf_jac = py_array_jac.request();
  auto buf_res = py_array_result.request();

  adept::Stack stack;
  adept::adouble th{theta};
  std::array<adept::adouble, 3> a{vec[0], vec[1], vec[2]};
  stack.new_recording();
  std::array<adept::adouble, 3> b{0.0, 0.0, 0.0};
  DiffRotorHepGAFunctor()(&th, a.begin(), b.begin());
  stack.independent(&th, 1);
  stack.dependent(b.begin(), 3);

  // stack.jacobian(static_cast<double *>(buf_jac.ptr), true);
  // stack.jacobian_reverse(static_cast<double *>(buf_jac.ptr), true);
  stack.jacobian_forward(static_cast<double *>(buf_jac.ptr), true);

  std::stringstream ss;
  stack.print_statements(ss);

  auto res_ptr = static_cast<double *>(buf_res.ptr);
  for (int i = 0; i < 3; ++i)
    res_ptr[i] = b[i].value();

  py::list list;
  list.append(py_array_result);
  list.append(py_array_jac);
  list.append(py::str(ss.str().c_str()));
  std::stringstream sstatus;
  stack.print_status(sstatus);
  list.append(py::str(sstatus.str().c_str()));

  return list;
}

py::list AdeptDiffRotorHepGAReverse(const double theta, const Vec &vec) {
  auto py_array_jac = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {3, 1}, {sizeof(double), sizeof(double)}));

  auto py_array_result = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {3, 1}, {sizeof(double), sizeof(double)}));

  auto buf_jac = py_array_jac.request();
  auto buf_res = py_array_result.request();

  adept::Stack stack;
  adept::adouble th{theta};
  std::array<adept::adouble, 3> a{vec[0], vec[1], vec[2]};
  stack.new_recording();
  std::array<adept::adouble, 3> b{0.0, 0.0, 0.0};
  DiffRotorHepGAFunctor()(&th, a.begin(), b.begin());
  stack.independent(&th, 1);
  stack.dependent(b.begin(), 3);

  // stack.jacobian(static_cast<double *>(buf_jac.ptr), true);
  stack.jacobian_reverse(static_cast<double *>(buf_jac.ptr), true);
  // stack.jacobian_forward(static_cast<double *>(buf_jac.ptr), true);

  std::stringstream ss;
  stack.print_statements(ss);

  auto res_ptr = static_cast<double *>(buf_res.ptr);
  for (int i = 0; i < 3; ++i)
    res_ptr[i] = b[i].value();

  py::list list;
  list.append(py_array_result);
  list.append(py_array_jac);
  list.append(py::str(ss.str().c_str()));
  std::stringstream sstatus;
  stack.print_status(sstatus);
  list.append(py::str(sstatus.str().c_str()));

  return list;
}

py::list CeresDiffRotorHepGA(const double theta, const Vec &a) {
  auto py_array_jac = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {3, 1}, {sizeof(double), sizeof(double)}));

  auto py_array_result = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {3, 1}, {sizeof(double), sizeof(double)}));

  auto buf_jac = py_array_jac.request();
  auto buf_res = py_array_result.request();

  const double *parameters[2] = {&theta, a.begin()};
  double *jacobians[2] = {static_cast<double *>(buf_jac.ptr), nullptr};

  ceres::AutoDiffCostFunction<DiffRotorHepGAFunctor, 3, 1, 3>(
      new DiffRotorHepGAFunctor())
      .Evaluate(parameters, static_cast<double *>(buf_res.ptr), jacobians);

  py::list list;
  list.append(py_array_result);
  list.append(py_array_jac);

  return list;
}
struct DiffRotorGaalopFunctor {
  template <typename T>
  bool operator()(const T *th, const T *a, T *b) const {
    b[0] = (-(a[0] * sin(th[0] / 2.0) * sin(th[0] / 2.0))) -
           2.0 * a[1] * cos(th[0] / 2.0) * sin(th[0] / 2.0) +
           a[0] * cos(th[0] / 2.0) * cos(th[0] / 2.0); // e1
    b[1] = (-(a[1] * sin(th[0] / 2.0) * sin(th[0] / 2.0))) +
           2.0 * a[0] * cos(th[0] / 2.0) * sin(th[0] / 2.0) +
           a[1] * cos(th[0] / 2.0) * cos(th[0] / 2.0); // e2
    b[2] = a[2] * sin(th[0] / 2.0) * sin(th[0] / 2.0) +
           a[2] * cos(th[0] / 2.0) * cos(th[0] / 2.0); // e3
    return true;
  }
};

py::list CeresDiffRotorGaalop(const double theta, const Vec &a) {
  auto py_array_jac = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {3, 1}, {sizeof(double), sizeof(double)}));

  auto py_array_result = py::array(py::buffer_info(
      nullptr, sizeof(double), py::format_descriptor<double>::value(), 2,
      {3, 1}, {sizeof(double), sizeof(double)}));

  auto buf_jac = py_array_jac.request();
  auto buf_res = py_array_result.request();

  const double *parameters[2] = {&theta, a.begin()};
  double *jacobians[2] = {static_cast<double *>(buf_jac.ptr), nullptr};

  ceres::AutoDiffCostFunction<DiffRotorGaalopFunctor, 3, 1, 3>(
      new DiffRotorGaalopFunctor())
      .Evaluate(parameters, static_cast<double *>(buf_res.ptr), jacobians);

  py::list list;
  list.append(py_array_result);
  list.append(py_array_jac);

  return list;
}

PYBIND11_PLUGIN(autodiff_multivector) {
  py::module m("autodiff_multivector", "autodiff_multivector");
  m.def("diff_adept", &AdeptDiffInnerProductVectorBivector);
  m.def("diff_ceres", &CeresDiffInnerProductVectorBivector);
  m.def("diff_ceres2", &CeresDiffInnerProductVectorVector);
  m.def("diff_ceres_rotor_versor", &CeresDiffRotorVersor);
  m.def("diff_adept_rotor_versor_forward", &AdeptDiffRotorVersorForward);
  m.def("diff_adept_rotor_versor_reverse", &AdeptDiffRotorVersorReverse);
  m.def("diff_ceres_rotor_matrix", &CeresDiffRotorMatrix);
  m.def("diff_adept_rotor_matrix_forward", &AdeptDiffRotorMatrixForward);
  m.def("diff_adept_rotor_matrix_reverse", &AdeptDiffRotorMatrixReverse);
  m.def("diff_ceres_rotor_hepga", &CeresDiffRotorHepGA);
  m.def("diff_ceres_rotor_gaalop", &CeresDiffRotorGaalop);
  m.def("diff_adept_rotor_hepga_forward", &AdeptDiffRotorHepGAForward);
  m.def("diff_adept_rotor_hepga_reverse", &AdeptDiffRotorHepGAReverse);
  return m.ptr();
}


#include <GaussIncludes.h>
#include <FEMIncludes.h>
#include <Eigen/SparseCholesky>

//Any extra things I need such as constraints
#include <LoubignacIterations.h>
#include <ConstraintFixedPoint.h>

//IGL Viewer
#include <igl/opengl/glfw/Viewer.h>
#include <igl/opengl/glfw/imgui/ImGuiMenu.h>
#include <igl/opengl/glfw/imgui/ImGuiHelpers.h>
#include <imgui/imgui.h>
#include <igl/readMESH.h>
#include <igl/jet.h>

//Global variables for UI
igl::opengl::glfw::Viewer viewer;

using namespace Gauss;
using namespace FEM;
using namespace ParticleSystem; //For Force Spring

/* Tetrahedral finite elements */
//typedef scene
typedef PhysicalSystemFEM<double, LinearTet> FEMLinearTets;
typedef World<double, std::tuple<FEMLinearTets *>, std::tuple<ForceSpringFEMParticle<double> *>, std::tuple<ConstraintFixedPoint<double> *> > MyWorld;

//This code from libigl boundary_facets.h
void tetsToTriangles(Eigen::MatrixXi &Fout, Eigen::MatrixXi &T) {

    unsigned int simplex_size = 4;
    std::vector<std::vector<int> > allF(
                                        T.rows()*simplex_size,
                                        std::vector<int>(simplex_size-1));

    // Gather faces, loop over tets
    for(int i = 0; i< (int)T.rows();i++)
    {
        // get face in correct order
        allF[i*simplex_size+0][0] = T(i,2);
        allF[i*simplex_size+0][1] = T(i,3);
        allF[i*simplex_size+0][2] = T(i,1);
        // get face in correct order
        allF[i*simplex_size+1][0] = T(i,3);
        allF[i*simplex_size+1][1] = T(i,2);
        allF[i*simplex_size+1][2] = T(i,0);
        // get face in correct order
        allF[i*simplex_size+2][0] = T(i,1);
        allF[i*simplex_size+2][1] = T(i,3);
        allF[i*simplex_size+2][2] = T(i,0);
        // get face in correct order
        allF[i*simplex_size+3][0] = T(i,2);
        allF[i*simplex_size+3][1] = T(i,1);
        allF[i*simplex_size+3][2] = T(i,0);
    }

    Fout.resize(allF.size(), simplex_size-1);
    for(unsigned int ii=0; ii<allF.size(); ++ii) {
        Fout(ii,0) = allF[ii][0];
        Fout(ii,1) = allF[ii][1];
        Fout(ii,2) = allF[ii][2];
    }
}

int main(int argc, char **argv) {
    std::cout<<"Test Linear FEM \n";

    //Setup Physics
    MyWorld world;

    //new code -- load tetgen files

    Eigen::MatrixXd V;
    Eigen::MatrixXi T;

    readTetgen(V, T, dataDir()+"/meshesTetgen/Beam/Beam.node", dataDir()+"/meshesTetgen/Beam/Beam.ele");

    Eigen::MatrixXd C;
    Eigen::MatrixXi F;
    Eigen::VectorXd s;

    //Eigen::MatrixXd V(4,3);
    //Eigen::MatrixXi T(1,4);
     /*V << 0,0,0,
     1,0,0,
     0,1,0,
     0,0,1;

    T << 0, 1, 2, 3;*/
    FEMLinearTets *test = new FEMLinearTets(V,T);
    world.addSystem(test);
    world.finalize();

    auto q = mapStateEigen(world);
    q.setZero();

    Eigen::VectorXi indices = minVertices(test, 0);
    Eigen::SparseMatrix<double> P = fixedPointProjectionMatrix(indices, *test,world);

    AssemblerEigenSparseMatrix<double> K;
    AssemblerEigenVector<double> f;

    getStiffnessMatrix(K, world);
    getForceVector(f, world);

    Eigen::SparseMatrix<double> Kp = P*(*K)*P.transpose();
    Eigen::VectorXd fp = -P*(*f);

    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > solver;
    solver.compute(Kp);

    if(solver.info()!=Eigen::Success) {
        // decomposition failed
        assert(1 == 0);
        std::cout<<"Decomposition Failed \n";
        exit(1);
    }

    //std::cout<<"ANSWER: "<<(P.transpose()*solver.solve(fp)).transpose()<<"\n";
    mapStateEigen<0>(world) = P.transpose()*solver.solve(fp);

    Eigen::MatrixXd stress;
    loubignacIterations(stress, (*K), P, world.getState(), (*f), *test, 1e-7);

    s = stress.rowwise().squaredNorm();

    tetsToTriangles(F, T);

    std::cout<<"Loubignac Iterations: \n"<<s.minCoeff()<<" "<<s.maxCoeff()<<"\n";
    igl::jet(s, s.minCoeff(), 100000000000, C);
    viewer.data().set_mesh(V, F);
    viewer.data().set_colors(C);
    viewer.launch();

}


//
// Created by Sergej Krivonos on 25.02.18.
//
#define BOOST_TEST_MODULE Stasis test
#include <boost/test/unit_test.hpp>

#include "Variable.h"

using namespace omnn::math;
using namespace boost::unit_test;
using namespace std;

BOOST_AUTO_TEST_CASE(Stasis
                     ,*disabled()
                     ) // Solve magic square
{
    constexpr auto Sz = 5;
    constexpr auto m = (Sz*(Sz*Sz+1))/2;
    auto& x = "x"_va;
    auto& y = "y"_va;
    auto& v = "v"_va;

    auto at = [&](const Valuable& xx, const Valuable& yy, const Valuable& vv) -> Valuable {
        return x.Equals(xx).LogicAnd(y.Equals(yy)).LogicAnd(v.Equals(vv));
    };

    auto world = at(0,0,1);

    auto getat = [&](const Valuable& xx, const Valuable& yy) {
        /*constexpr*/static Variable t = "t"_va;
        return at(xx,yy,t)(t);
    };

    auto sumd0 = 0_v, sumd1 = 0_v;
    Valuable::optimizations = {};
    for (auto xx=Sz; xx--;) {
        auto sumx=0_v, sumy=0_v;
        for (auto yy=Sz; yy--; ) {
            sumx += getat(xx,yy);
            sumy += getat(xx,yy);
        }
        world += sumx.Equals(m).sq();
        world += sumy.Equals(m).sq();
        sumd0 += getat(xx,xx);
        sumd1 += getat(Sz-xx, Sz-xx);
    }
    world += sumd0.Equals(m).sq();
    world += sumd1.Equals(m).sq();

    for (auto xx=Sz; xx--;) {
        for (auto yy=Sz; yy--; ) {
            auto co = world;
            co.eval({{x,xx},{y,yy}});
            Valuable::optimizations = true;
            co.optimize();
            co.SetView(Valuable::View::Solving);
            co.optimize();
            Valuable::optimizations = {};
            auto s = co(v);
            if (!s.IsInt()
//                s.size()!=1
                ) {
                IMPLEMENT
            }else{
                std::cout
                    << s
//                    << *s.begin()
                    << ' ';
            }
        }
        std::cout << std::endl;
    }
}

BOOST_AUTO_TEST_CASE(Stasis_empty_test){} // to success end if all tests disabled


/* test_binomial.cpp
 *
 * Copyright Steven Watanabe 2010
 * 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)
 *
 * $Id: test_binomial.cpp 71018 2011-04-05 21:27:52Z steven_watanabe $
 *
 */

#include <boost/random/binomial_distribution.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/math/distributions/binomial.hpp>

#define BOOST_RANDOM_DISTRIBUTION boost::random::binomial_distribution<>
#define BOOST_RANDOM_DISTRIBUTION_NAME binomial
#define BOOST_MATH_DISTRIBUTION boost::math::binomial
#define BOOST_RANDOM_ARG1_TYPE int
#define BOOST_RANDOM_ARG1_NAME n
#define BOOST_RANDOM_ARG1_DEFAULT 100000
#define BOOST_RANDOM_ARG1_DISTRIBUTION(n) boost::uniform_int<>(0, n)
#define BOOST_RANDOM_ARG2_TYPE double
#define BOOST_RANDOM_ARG2_NAME p
#define BOOST_RANDOM_ARG2_DEFAULT 1000.0
#define BOOST_RANDOM_ARG2_DISTRIBUTION(n) boost::uniform_01<>()
#define BOOST_RANDOM_DISTRIBUTION_MAX n

#include "test_real_distribution.ipp"