// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// 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.

#ifndef SMOOTH__SE2_HPP_
#define SMOOTH__SE2_HPP_

#include <Eigen/Core>

#include <complex>

#include "internal/lie_group_base.hpp"
#include "internal/macro.hpp"
#include "internal/se2.hpp"
#include "so2.hpp"

namespace smooth {

// \cond
template<typename Scalar>
class SE3;
// \endcond

/**
 * @brief Base class for SE2 Lie group types.
 *
 * Internally represented as \f$\mathbb{U}(1) \times \mathbb{R}^2\f$.
 *
 * Memory layout
 * -------------
 *
 * - Group:    \f$ \mathbf{x} = [x, y, q_z, q_w] \f$
 * - Tangent:  \f$ \mathbf{a} = [v_x, v_y, \omega_z] \f$
 *
 * Constraints
 * -----------
 *
 * - Group:   \f$q_z^2 + q_w^2 = 1 \f$
 * - Tangent: \f$ -\pi < \omega_z \leq \pi \f$
 *
 * Lie group matrix form
 * ---------------------
 *
 * \f[
 * \mathbf{X} =
 * \begin{bmatrix}
 *  q_w & -q_z & x \\
 *  q_z &  q_w & y \\
 *  0   &    0 & 1
 * \end{bmatrix} \in \mathbb{R}^{3 \times 3}
 * \f]
 *
 *
 * Lie algebra matrix form
 * -----------------------
 *
 * \f[
 * \mathbf{a}^\wedge =
 * \begin{bmatrix}
 *   0 & -\omega_z & v_x \\
 *  \omega_z &   0 & v_y \\
 *  0 & 0 & 0
 * \end{bmatrix} \in \mathbb{R}^{3 \times 3}
 * \f]
 */
template<typename _Derived>
class SE2Base : public LieGroupBase<_Derived>
{
  using Base = LieGroupBase<_Derived>;

protected:
  SE2Base() = default;

public:
  SMOOTH_INHERIT_TYPEDEFS;

  /**
   * @brief Access SO(2) part.
   */
  Eigen::Map<SO2<Scalar>> so2() requires is_mutable
  {
    return Eigen::Map<SO2<Scalar>>(static_cast<_Derived &>(*this).data() + 2);
  }

  /**
   * @brief Const access SO(2) part.
   */
  Eigen::Map<const SO2<Scalar>> so2() const
  {
    return Eigen::Map<const SO2<Scalar>>(static_cast<const _Derived &>(*this).data() + 2);
  }

  /**
   * @brief Access R2 part.
   */
  Eigen::Map<Eigen::Matrix<Scalar, 2, 1>> r2() requires is_mutable
  {
    return Eigen::Map<Eigen::Matrix<Scalar, 2, 1>>(static_cast<_Derived &>(*this).data());
  }

  /**
   * @brief Const access R2 part.
   */
  Eigen::Map<const Eigen::Matrix<Scalar, 2, 1>> r2() const
  {
    return Eigen::Map<const Eigen::Matrix<Scalar, 2, 1>>(
      static_cast<const _Derived &>(*this).data());
  }

  /**
   * @brief Tranformation action on 2D vector.
   */
  template<typename EigenDerived>
  Eigen::Matrix<Scalar, 2, 1> operator*(const Eigen::MatrixBase<EigenDerived> & v) const
  {
    return so2() * v + r2();
  }

  /**
   * @brief Lift to SE3.
   *
   * @note SE3 header must be included.
   */
  SE3<Scalar> lift_se3() const
  {
    return SE3<Scalar>(
      so2().lift_so3(), Eigen::Matrix<Scalar, 3, 1>(r2().x(), r2().y(), Scalar(0)));
  }
};

// \cond
template<typename _Scalar>
class SE2;
// \endcond

// \cond
template<typename _Scalar>
struct lie_traits<SE2<_Scalar>>
{
  static constexpr bool is_mutable = true;

  using Impl   = SE2Impl<_Scalar>;
  using Scalar = _Scalar;

  template<typename NewScalar>
  using PlainObject = SE2<NewScalar>;
};
// \endcond

/**
 * @brief Storage implementation of SE2 Lie group.
 *
 * @see SE2Base for memory layout.
 */
template<typename _Scalar>
class SE2 : public SE2Base<SE2<_Scalar>>
{
  using Base = SE2Base<SE2<_Scalar>>;

  SMOOTH_GROUP_API(SE2);

public:
  /**
   * @brief Construct from SO2 and R2.
   *
   * @param so2 orientation component.
   * @param r2 translation component.
   */
  template<typename SO2Derived, typename T2Derived>
  SE2(const SO2Base<SO2Derived> & so2, const Eigen::MatrixBase<T2Derived> & r2)
  {
    Base::so2() = so2;
    Base::r2()  = r2;
  }
};

using SE2f = SE2<float>;   ///< SE2 with float
using SE2d = SE2<double>;  ///< SE2 with double

}  // namespace smooth

// MAP TYPE TRAITS

// \cond
template<typename _Scalar>
struct smooth::lie_traits<Eigen::Map<smooth::SE2<_Scalar>>>
    : public lie_traits<smooth::SE2<_Scalar>>
{};
// \endcond

/**
 * @brief Memory mapping of SE2 Lie group.
 *
 * @see SE2Base for memory layout.
 */
template<typename _Scalar>
class Eigen::Map<smooth::SE2<_Scalar>> : public smooth::SE2Base<Eigen::Map<smooth::SE2<_Scalar>>>
{
  using Base = smooth::SE2Base<Eigen::Map<smooth::SE2<_Scalar>>>;

  SMOOTH_MAP_API(Map);
};

// \cond
template<typename _Scalar>
struct smooth::lie_traits<Eigen::Map<const smooth::SE2<_Scalar>>>
    : public lie_traits<smooth::SE2<_Scalar>>
{
  static constexpr bool is_mutable = false;
};
// \endcond

/**
 * @brief Const memory mapping of SE2 Lie group.
 *
 * @see SE2Base for memory layout.
 */
template<typename _Scalar>
class Eigen::Map<const smooth::SE2<_Scalar>>
    : public smooth::SE2Base<Eigen::Map<const smooth::SE2<_Scalar>>>
{
  using Base = smooth::SE2Base<Eigen::Map<const smooth::SE2<_Scalar>>>;

  SMOOTH_CONST_MAP_API(Map);
};

#endif  // SMOOTH__SE2_HPP_


//
// Helper functions to construct MPC-style (continuous replanning) style policies using the iLQRTree
// class.
//

#pragma once

#include <ilqr/ilqr_taylor_expansions.hh>
#include <ilqr/ilqr_tree.hh>

#include <Eigen/Dense>

#include <vector>

namespace policy
{

// Does hindsight optimization over a probabilistic split in dynamics.
// Returns a control vector "u" by constructing a hindsight_split style tree.
// Linearly interpolates from xt to xT for the initialization for iLQR with nominal_control for all
// timesteps.
Eigen::VectorXd hindsight_tree_policy(const int t,
                                      const Eigen::VectorXd& xt,
                                      const int T,
                                      const Eigen::VectorXd& xT,
                                      const Eigen::VectorXd& nominal_control,
                                      const std::vector<double> &probabilities,
                                      const std::vector<ilqr::DynamicsFunc> &dynamics_funcs,
                                      const ilqr::CostFunc &cost,
                                      ilqr::iLQRTree& ilqr_tree);

// Does hindsight optimization over a probabilistic split in cost functions.
// Returns a control vector "u" by constructing a hindsight_split style tree.
// Linearly interpolates from xt to xT for the initialization for iLQR with nominal_control for all
// timesteps.
Eigen::VectorXd hindsight_tree_policy(const int t,
                                      const Eigen::VectorXd& xt,
                                      const int T,
                                      const Eigen::VectorXd& xT,
                                      const Eigen::VectorXd& nominal_control,
                                      const std::vector<double> &probabilities,
                                      const ilqr::DynamicsFunc &dynamics_func,
                                      const std::vector<ilqr::CostFunc> &cost_funcs,
                                      ilqr::iLQRTree& ilqr_tree);

// Returns a control vector "u" by constructing a chain tree structure.
Eigen::VectorXd chain_policy(const int t,
        const Eigen::VectorXd& xt,
        const int T,
        const Eigen::VectorXd& xT,
        const Eigen::VectorXd& nominal_control,
        const ilqr::DynamicsFunc &dynamics,
        const ilqr::CostFunc &cost,
        ilqr::iLQRTree& ilqr_tree);

// Probability weighted controller over a probabilistic split in dynamics functions.
Eigen::VectorXd probability_weighted_policy(const int t,
        const Eigen::VectorXd& xt,
        const int T,
        const Eigen::VectorXd& xT,
        const Eigen::VectorXd& nominal_control,
        const std::vector<double> &probabilities,
        const std::vector<ilqr::DynamicsFunc> &dynamics_funcs,
        const ilqr::CostFunc &cost);

// Probability weighted controller over a probabilistic split in cost functions.
Eigen::VectorXd probability_weighted_policy(const int t,
        const Eigen::VectorXd& xt,
        const int T,
        const Eigen::VectorXd& xT,
        const Eigen::VectorXd& nominal_control,
        const std::vector<double> &probabilities,
        const ilqr::DynamicsFunc &dynamics_func,
        const std::vector<ilqr::CostFunc> &cost_funcs);

} // namespace policy


#define EIGEN_DONT_PARALLELIZE
#include <Eigen/Core>

#include "update_ops.hpp"
#include "update_ops_cpp.hpp"
#include "utility.hpp"

void double_qubit_dense_matrix_gate(UINT target_qubit_index1,
    UINT target_qubit_index2, const CTYPE matrix[16], CTYPE* state, ITYPE dim) {
    double_qubit_dense_matrix_gate_c(
        target_qubit_index1, target_qubit_index2, matrix, state, dim);
}

void double_qubit_dense_matrix_gate(UINT target_qubit_index1,
    UINT target_qubit_index2, const Eigen::Matrix4cd& eigen_matrix,
    CTYPE* state, ITYPE dim) {
    double_qubit_dense_matrix_gate_eigen(
        target_qubit_index1, target_qubit_index2, eigen_matrix, state, dim);
}

void double_qubit_dense_matrix_gate_eigen(UINT target_qubit_index1,
    UINT target_qubit_index2, const Eigen::Matrix4cd& eigen_matrix,
    CTYPE* state, ITYPE dim) {
    // target mask

    const UINT min_qubit_index =
        get_min_ui(target_qubit_index1, target_qubit_index2);
    const UINT max_qubit_index =
        get_max_ui(target_qubit_index1, target_qubit_index2);
    const ITYPE min_qubit_mask = 1ULL << min_qubit_index;
    const ITYPE max_qubit_mask = 1ULL << (max_qubit_index - 1);
    const ITYPE low_mask = min_qubit_mask - 1;
    const ITYPE mid_mask = (max_qubit_mask - 1) ^ low_mask;
    const ITYPE high_mask = ~(max_qubit_mask - 1);

    const ITYPE target_mask1 = 1ULL << target_qubit_index1;
    const ITYPE target_mask2 = 1ULL << target_qubit_index2;
    std::complex<double>* eigen_state =
        reinterpret_cast<std::complex<double>*>(state);

    // loop variables
    const ITYPE loop_dim = dim / 4;
    ITYPE state_index;

    for (state_index = 0; state_index < loop_dim; ++state_index) {
        // create index
        ITYPE basis_0 = (state_index & low_mask) +
                        ((state_index & mid_mask) << 1) +
                        ((state_index & high_mask) << 2);

        // gather index
        ITYPE basis_1 = basis_0 + target_mask1;
        ITYPE basis_2 = basis_0 + target_mask2;
        ITYPE basis_3 = basis_1 + target_mask2;

        // fetch values
        Eigen::Vector4cd vec(
            state[basis_0], state[basis_1], state[basis_2], state[basis_3]);
        vec = eigen_matrix * vec;
        eigen_state[basis_0] = vec[0];
        eigen_state[basis_1] = vec[1];
        eigen_state[basis_2] = vec[2];
        eigen_state[basis_3] = vec[3];
    }
}


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

using namespace Eigen;

// [functor]
template<class ArgType, class RowIndexType, class ColIndexType>
class indexing_functor {
  const ArgType &m_arg;
  const RowIndexType &m_rowIndices;
  const ColIndexType &m_colIndices;
public:
  typedef Matrix<typename ArgType::Scalar,
                 RowIndexType::SizeAtCompileTime,
                 ColIndexType::SizeAtCompileTime,
                 ArgType::Flags&RowMajorBit?RowMajor:ColMajor,
                 RowIndexType::MaxSizeAtCompileTime,
                 ColIndexType::MaxSizeAtCompileTime> MatrixType;

  indexing_functor(const ArgType& arg, const RowIndexType& row_indices, const ColIndexType& col_indices)
    : m_arg(arg), m_rowIndices(row_indices), m_colIndices(col_indices)
  {}

  const typename ArgType::Scalar& operator() (Index row, Index col) const {
    return m_arg(m_rowIndices[row], m_colIndices[col]);
  }
};
// [functor]

// [function]
template <class ArgType, class RowIndexType, class ColIndexType>
CwiseNullaryOp<indexing_functor<ArgType,RowIndexType,ColIndexType>, typename indexing_functor<ArgType,RowIndexType,ColIndexType>::MatrixType>
mat_indexing(const Eigen::MatrixBase<ArgType>& arg, const RowIndexType& row_indices, const ColIndexType& col_indices)
{
  typedef indexing_functor<ArgType,RowIndexType,ColIndexType> Func;
  typedef typename Func::MatrixType MatrixType;
  return MatrixType::NullaryExpr(row_indices.size(), col_indices.size(), Func(arg.derived(), row_indices, col_indices));
}
// [function]

int main()
{
  std::cout << "[main1]\n";
  Eigen::MatrixXi A = Eigen::MatrixXi::Random(4,4);
  Array3i ri(1,2,1);
  ArrayXi ci(6); ci << 3,2,1,0,0,2;
  Eigen::MatrixXi B = mat_indexing(A, ri, ci);
  std::cout << "A =" << std::endl;
  std::cout << A << std::endl << std::endl;
  std::cout << "A([" << ri.transpose() << "], [" << ci.transpose() << "]) =" << std::endl;
  std::cout << B << std::endl;
  std::cout << "[main1]\n";

  std::cout << "[main2]\n";
  B =  mat_indexing(A, ri+1, ci);
  std::cout << "A(ri+1,ci) =" << std::endl;
  std::cout << B << std::endl << std::endl;
#if __cplusplus >= 201103L
  B =  mat_indexing(A, ArrayXi::LinSpaced(13,0,12).unaryExpr([](int x){return x%4;}), ArrayXi::LinSpaced(4,0,3));
  std::cout << "A(ArrayXi::LinSpaced(13,0,12).unaryExpr([](int x){return x%4;}), ArrayXi::LinSpaced(4,0,3)) =" << std::endl;
  std::cout << B << std::endl << std::endl;
#endif
  std::cout << "[main2]\n";
}


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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Matrix2f M = Matrix2f::Random();
Matrix2f m;
m = M;
cout << "Here is the matrix m:" << endl << m << endl;
cout << "Now we want to copy a column into a row." << endl;
cout << "If we do m.col(1) = m.row(0), then m becomes:" << endl;
m.col(1) = m.row(0);
cout << m << endl << "which is wrong!" << endl;
cout << "Now let us instead do m.col(1) = m.row(0).eval(). Then m becomes" << endl;
m = M;
m.col(1) = m.row(0).eval();
cout << m << endl << "which is right." << endl;

  return 0;
}


//
// Created by krab1k on 31/10/18.
//

#include <vector>
#include <cmath>
#include <functional>
#include <Eigen/LU>

#include "smpqeq.h"
#include "../parameters.h"
#include "../geometry.h"

CHARGEFW2_METHOD(SMP_QEq)

Eigen::VectorXd SMP_QEq::EE_system(const std::vector<const Atom *> &atoms, double total_charge) const {

    size_t n = atoms.size();

    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(n + 1, n + 1);
    Eigen::VectorXd b = Eigen::VectorXd::Zero(n + 1);

    for (int iter = 0; iter < 5; iter++)
    {
        for (size_t i = 0; i < n; i++) {
            const auto &atom_i = *atoms[i];
            A(i, i) = 2 * (parameters_->atom()->parameter(atom::second)(atom_i) +
                                parameters_->atom()->parameter(atom::third)(atom_i) * b(i) +
                                parameters_->atom()->parameter(atom::fourth)(atom_i) * b(i) * b(i));
            b(i) = -parameters_->atom()->parameter(atom::first)(atom_i);
            for (size_t j = i + 1; j < n; j++) {
                const auto &atom_j = *atoms[j];
                auto gamma = 2 * std::sqrt(parameters_->atom()->parameter(atom::second)(atom_i) *
                                           parameters_->atom()->parameter(atom::second)(atom_j));
                auto expr = 1 / std::cbrt(1 / std::pow(gamma, 3) + std::pow(distance(atom_i, atom_j), 3));
                A(i, j) = expr;
                A(j, i) = expr;
            }
        }

        A.row(n) = Eigen::VectorXd::Constant(n + 1, 1);
        A.col(n) = Eigen::VectorXd::Constant(n + 1, 1);
        A(n, n) = 0;
        b(n) = total_charge;

        b = A.partialPivLu().solve(b);
    }

    return b.head(n);
}

std::vector<double> SMP_QEq::calculate_charges(const Molecule &molecule) const {
    auto f = [this](const std::vector<const Atom *> &atoms, double total_charge) -> Eigen::VectorXd {
        return EE_system(atoms, total_charge);
    };

    Eigen::VectorXd q = solve_EE(molecule, f);
    return std::vector<double>(q.data(), q.data() + q.size());
}


#include <iostream>
#include <Eigen/Dense>
#include <math.h>
#include <unsupported/Eigen/MatrixFunctions>

//Global Variables
using Eigen::Vector3f;
Vector3f g(0,0,-9.81); //gravity

//Cross Product Equivalent
using Eigen::Matrix3f;
using Eigen::Vector3f;
Matrix3f crossProductEquivalent(Vector3f v)
{
  Matrix3f c;
  c << 0, -v(2), v(1),
       v(2), 0, -v(0),
       -v(1), v(0), 0;
  //std::cout << c << std::endl;
  return c;
}

//Quaternion Multiplication
using Eigen::Quaternionf;
using Eigen::Vector3f;
Quaternionf qMultiply(Quaternionf q1, Quaternionf q2)
{
  float w1 = q1.w();
  float w2 = q2.w();
  Vector3f v1 = q1.vec();
  Vector3f v2 = q2.vec();
  float wReturn = w1*w2 - v1(0)*v2(0) - v1(1)*v2(1) - v1(2)*v2(2);
  Vector3f vReturn;
  vReturn(0) = w1*v2(0) + v1(0)*w2 + v1(1)*v2(2) - v1(2)*v2(1);
  vReturn(1) = w1*v2(1) - v1(0)*v2(2) + v1(1)*w2 + v1(2)*v2(0);
  vReturn(2) = w1*v2(2) + v1(0)*v2(1) - v1(1)*v2(0) + v1(2)*w2;
  Quaternionf qReturn;
  qReturn.w() = wReturn;
  qReturn.vec() = vReturn;
  //std::cout << qReturn.w() << std::endl << qReturn.vec() << std::endl;
  return qReturn;
}

//Quaternion Exponential
//implements simple 0th-order integration
//ref: quaternion kinematics, section 4.6.1
using Eigen::Quaternionf;
using Eigen::Vector3f;
Quaternionf qExponential(float dt, Vector3f w)
{
  float wn = w.norm();
  Vector3f wN = w.normalized();
  Quaternionf qReturn;
  qReturn.w() = cos(wn * dt / 2);
  qReturn.vec() = wN * sin(wn * dt / 2);
  return qReturn;
}

//Quaternion to DCM
//ref: mathworks Quat2DCM documentation
using Eigen::Quaternionf;
using Eigen::Matrix3f;
Matrix3f q2dcm(Quaternionf q)
{
  float q0 = q.w();
  float q1 = q.x();
  float q2 = q.y();
  float q3 = q.z();
  Matrix3f DCM;
  DCM << (q0*q0 + q1*q1 - q2*q2 - q3*q3), 2*(q1*q2 + q0*q3), 2*(q1*q3 - q0*q2),
          2*(q1*q2 - q0*q3), (q0*q0 - q1*q1 + q2*q2 - q3*q3), 2*(q2*q3 + q0*q1),
          2*(q1*q3 + q0*q2), 2*(q2*q3 - q0*q1), (q0*q0 - q1*q1 - q2*q2 + q3*q3);
  return DCM;
}

//Euler angles to DCM
//e = [phi, theta, psi]
//3-1-2 rotation
//ref: Todd Humphreys euler2dcm MATLAB function, "Aerial Robotics," 2019
using Eigen::Matrix3f;
using Eigen::Vector3f;
Matrix3f euler2dcm(Vector3f e)
{
  std::cout << "Calculating trig values..." << std::endl;
  float cPhi = cos(e(0));
  float sPhi = sin(e(0));
  float cThe = cos(e(1));
  float sThe = sin(e(1));
  float cPsi = cos(e(2));
  float sPsi = sin(e(2));
  std::cout << "Building DCM..." << std::endl;
  Matrix3f DCM;
  DCM << (cPhi*cThe - sPhi*sPsi*sThe), (cThe*sPsi + cPsi * sPhi*sThe), (-cPhi*sThe),
         (-cPhi*sPsi),                                    (cPhi*cPsi),         sPhi,
         (cPsi*sThe + cThe*sPhi*sPsi), (sPsi*sThe - cPsi*cThe*sPhi),    (cPhi*cThe);
  return DCM;
}

//Estimate class
using Eigen::Quaternionf;
using Eigen::Vector3f;
class StateEstimate
{
  private:
    Quaternionf qHat;
    Vector3f pHat;
    Vector3f vHat;
    /*Vector3f dThetaHat;*/
    Vector3f bgHat;
    Vector3f baHat;
    float tk;

  public:
    StateEstimate(Quaternionf q, Vector3f p, Vector3f v, /*Vector3f dTheta,*/ Vector3f bg, Vector3f ba, float tk) {
      qHat = q;
      pHat = p;
      vHat = v;
      /*dThetaHat = dTheta;*/
      bgHat = bg;
      baHat = ba;
    }

    Quaternionf getqHat(){return qHat;}
    void setqHat(Quaternionf q){this->qHat = q;}
    /*Vector3f getdThetaHat(){return dThetaHat;}
    void setdThetaHat(Vector3f dTheta){this->dThetaHat = dThetaHat;}*/
    Vector3f getpHat(){return pHat;}
    void setpHat(Vector3f p){this->pHat = p;}
    Vector3f getvHat(){return vHat;}
    void setvHat(Vector3f v){this->vHat = v;}
    Vector3f getbgHat(){return bgHat;}
    void setbgHat(Vector3f bg){this->bgHat = bg;}
    Vector3f getbaHat(){return baHat;}
    void setbaHat(Vector3f ba){this->baHat = ba;}
    void gettk(){return tk;}
    void settk(float t){this->tk = t;}

};

//Estimator state class
using Eigen::Vector3f;
using Eigen::Matrix3f;
class EstimatorState
{
private:
	Quaternionf qBar;
	Vector3f pBar;
	Vector3f vBar;
	Vector3f e;
	Vector3f bgBar;
	Vector3f baBar;

	Matrix3f Pp;
	Matrix3f Pv;
	Matrix3f Pe;
	Matrix3f Pba;
	Matrix3f Pbg;

	float tk;

public:
	EstimatorState(Quaternionf q, Vector3f p, Vector3f v, Vector3f ee, Vector3f bg,
		Vector3f ba, Matrix3f pp, Matrix3f pv, Matrix3f pe, Matrix3f pba, Matrix3f pbg, float tk) {
        qBar = q;
        pBar = p;
        vBar = v;
        e = ee;
        bgBar = bg;
        baBar = ba;
        Pe = pe;
        Pp = pp;
        Pv = pv;
        Pba = pba;
        Pbg = pbg;
	}

	Quaternionf getqBar(){return qBar;}
    void setqBar(Quaternionf q){this->qBar = q;}
    /*Vector3f getdThetaHat(){return dThetaHat;}
    void setdThetaHat(Vector3f dTheta){this->dThetaHat = dThetaHat;}*/
    Vector3f getpBar(){return pBar;}
    void setpBar(Vector3f p){this->pBar = p;}
    Vector3f getvBar(){return vBar;}
    void sete(Vector3f ee){this->e = ee;}
    Vector3f gete(){return e;}
    void setvBar(Vector3f v){this->vBar = v;}
    Vector3f getbgBar(){return bgBar;}
    void setbgBar(Vector3f bg){this->bgBar = bg;}
    Vector3f getbaBar(){return baBar;}
    void setbaBar(Vector3f ba){this->baBar = ba;}
    void gettk(){return tk;}
    void settk(float t){this->tk = t;}
}

//Measurement class
using Eigen::Vector3f;
using Eigen::Matrix3f;
class Measurement
{
private:
	Vector3f wm;
	Vector3f am;

public:
	Measurement(Vector3f a, Vector3f w) {
        am = a;
        wm = w;
	}

	Vector3f getwm(){return wm;}
    Vector3f getam(){return am;}

}

//Error state class
/*using Eigen::Quaternionf;
using Eigen::Vector3f;
class ErrorState
{
  private:
    Quaternionf qTilde;
    Vector3f pTilde;
    Vector3f vTilde;
    Vector3f bgTilde;
    Vector3f baTilde;

  public:
    ErrorState(Quaternionf qT, Vector3f pT, Vector3f vT, Vector3f bgT, Vector3f baT) {
      qTilde = qT;
      pTilde = pT;

    }
}*/

//State Estimation (prop, update)
//EKF-based estimation
//ref: "Aerial Robotics," 'Laboratory Exercise 3: Meas. Simulation and State Estimation,' 2019
void prop(StateEstimate S, EstimatorState E)
{

}

void update(StateEstimate S, EstimatorState E, Measurements M)
{
    //Convert measurements to I-frame
    Vector3f wm = M.getwm();
    Vector3f am = M.getam();
    Matrix3f RBIBark = q2dcm(E.getqBar());
    Vector3f wmI = (RBIBark.transpose())*wm;
    Vector3f amI = (RBIBark.transpose())*am;

    //Push estimator state through measurement function
    //(relationship bewteen measurements and dynamics)
    //do stuff with image features

}

//Propagate State Estimate
/*StateEstimate propState(StateEstimate S, Vector3f am, Vector3f wm, float dt)
{
  // Constants
  using Eigen::Quaternionf;
  using Eigen::Vector3f;
  using Eigen::Matrix3f;
  Matrix3f I3 = Matrix3f::Identity(3,3);
  Matrix3f O3 = Matrix3f::Zero();
  Matrix3f wmx = crossProductEquivalent(wm);

  //State values
  Quaternionf qHat = S.getqHat();
  Vector3f dThetaHat = S.getdThetaHat();
  Vector3f pHat = S.getpHat();
  Vector3f vHat = S.getvHat();
  Vector3f bgHat = S.getbgHat();
  Vector3f baHat = S.getbaHat();

  //Propagate quaternion
  using Eigen::Quaternionf;
  using Eigen::Vector3f;
  using Eigen::Matrix3f;
  Quaternionf qExp = qExponential(dt, wm);
  Quaternionf qHatProp = qMultiply(qHat, qExp);

  //Calculate TIB, the DCM
  Matrix3f TIB = q2dcm(qHat);
  Matrix3f TBI = TIB.transpose();

  //Propagate p, the position
  Vector3f pHatProp = pHat
                      + vHat*dt
                      + 0.5*(TBI*crossProductEquivalent(am*(dt*dt)))*dThetaHat
                      - 0.5*TIB*baHat;

  //Propagate v, the velocity
  Vector3f vHatProp = vHat
  					  + (TBI*crossProductEquivalent((am-baHat)*dt))*dThetaHat
                      - TIB*dt*baHat;

  //Propagate dThetaHat, the 3x1 vector of angles of attitude error
  Vector3f dThetaHatProp = ((-crossProductEquivalent(wm)*dt).exp())*dThetaHat
                  - bgHat;

  //Propagate bg and ba, the gyroscope and accelerometer biases
  Vector3f bgHatProp = bgHat;
  Vector3f baHatProp = baHat;

  //Return the propagated state
  StateEstimate Sprop(qHatProp, pHatProp, vHatProp, dThetaHatProp, bgHatProp, baHatProp);
  return Sprop;
}*/

//State Estimator
//EKF-based estimation
//ref: "Aerial Robotics," 'Laboratory Exercise 3: Meas. Simulation and State Estimation,' 2019
StateEstimate stateEstimatorEKF(float tk, Vector3f am, Vector3f wm, )

int main()
{
  /*using Eigen::Vector3f;
  using Eigen::Matrix3f;
  using Eigen::Quaternionf;
  Matrix3f A;
  A << 1, 2, 3, 4, 5, 6, 7, 8, 9;
  Vector3f b;
  b << 1, 2, 3;
  Vector3f c;
  c << 4, 5, 6;
  std::cout << A*b << std::endl;
  std::cout << 0.5*A*b <<std::endl;
  float dt = 0.1;
  std::cout << 0.5*b*dt*dt <<std::endl;*/

  /*using Eigen::Vector3f;
  using Eigen::Matrix3f;
  using Eigen::Quaternionf;
  Vector3f v(0.0, 3.1415, 6.2830);
  Matrix3f DCM = euler2dcm(v);
  std::cout << DCM << std::endl;*/

  Quaternionf q;
  Vector3f qv(0,0,0);
  float qw = 1;
  q.w() = qw;
  q.vec() = qv;
  Vector3f dTheta(0,0,0);
  Vector3f p(0,0,0);
  Vector3f v(0,0,0);
  Vector3f bg(0,0,0);
  Vector3f ba(0,0,0);
  StateEstimate S(q, p, v, dTheta, bg, ba);

  Vector3f am(0.2,0,-9.81);
  Vector3f wm(0.5*3.1415,0,0);
  float dt = 0.1;

  for ( int ii = 0; ii < 10; ii = ii + 1 ){
    StateEstimate Sprop = propState(S,am,wm,dt);
    std::cout << "qHatProp (w): " << Sprop.getqHat().w() << std::endl;
    std::cout << "qHatProp (v): " << std::endl << Sprop.getqHat().vec() << std::endl;
    std::cout << "dThetaHatProp: " << std::endl << Sprop.getdThetaHat() << std::endl;
    std::cout << "pHatProp: " << std::endl << Sprop.getpHat() << std::endl;
    std::cout << "vHatProp: " << std::endl << Sprop.getvHat() << std::endl;
    std::cout << "baHatProp: " << std::endl << Sprop.getbaHat() << std::endl;
    std::cout << "bgHatProp: " << std::endl << Sprop.getbgHat() << std::endl;
    std::cout << std::endl;
    S = Sprop;
  }

}


#include "BVH.h"
#include <iostream>
#include <Eigen/Geometry>
#include "dart/dart.hpp"
namespace MSS
{
Eigen::Matrix3d
R_x(double x)
{
	double cosa = cos(x*3.141592/180.0);
	double sina = sin(x*3.141592/180.0);
	Eigen::Matrix3d R;
	R<<	1,0		,0	  ,
		0,cosa	,-sina,
		0,sina	,cosa ;
	return R;
}
Eigen::Matrix3d R_y(double y)
{
	double cosa = cos(y*3.141592/180.0);
	double sina = sin(y*3.141592/180.0);
	Eigen::Matrix3d R;
	R <<cosa ,0,sina,
		0    ,1,   0,
		-sina,0,cosa;
	return R;
}
Eigen::Matrix3d R_z(double z)
{
	double cosa = cos(z*3.141592/180.0);
	double sina = sin(z*3.141592/180.0);
	Eigen::Matrix3d R;
	R<<	cosa,-sina,0,
		sina,cosa ,0,
		0   ,0    ,1;
	return R;
}
BVHNode::
BVHNode(const std::string& name,BVHNode* parent)
	:mParent(parent),mName(name),mChannelOffset(0),mNumChannels(0)
{

}
void
BVHNode::
SetChannel(int c_offset,std::vector<std::string>& c_name)
{
	mChannelOffset = c_offset;
	mNumChannels = c_name.size();
	for(const auto& cn : c_name)
		mChannel.push_back(CHANNEL_NAME[cn]);
}
void
BVHNode::
Set(const Eigen::VectorXd& m_t)
{
	mR.setIdentity();

	for(int i=0;i<mNumChannels;i++)
	{
		switch(mChannel[i])
		{
		case Xpos:break;
		case Ypos:break;
		case Zpos:break;
		case Xrot:mR = mR*R_x(m_t[mChannelOffset+i]);break;
		case Yrot:mR = mR*R_y(m_t[mChannelOffset+i]);break;
		case Zrot:mR = mR*R_z(m_t[mChannelOffset+i]);break;
		default:break;
		}
	}

}
void
BVHNode::
Set(const Eigen::Matrix3d& R_t)
{
	mR = R_t;
}
Eigen::Matrix3d
BVHNode::
Get()
{
	return mR;
}

void
BVHNode::
AddChild(BVHNode* child)
{
	mChildren.push_back(child);
}
BVHNode*
BVHNode::
GetNode(const std::string& name)
{
	if(!mName.compare(name))
		return this;

	for(auto& c : mChildren)
	{
		BVHNode* bn = c->GetNode(name);
		if(bn!=nullptr)
			return bn;
	}

	return nullptr;
}

Eigen::Vector3d
BVH::
GetP0()
{
	Eigen::VectorXd m_t = mMotions[0];

	Eigen::Vector3d p0 = m_t.segment<3>(0) - mRootCOMOffset;
	p0 *= 0.01;

	return p0;
}

BVH::
BVH()
{

}

void
BVH::
SetMotion(double t)
{

	int k = ((int)std::floor(t/mTimeStep));
	k = std::max(0,std::min(k,mNumTotalFrames-1));
	double dt = t/mTimeStep - std::floor(t/mTimeStep);
	Eigen::VectorXd m_t = mMotions[k];

	for(auto& bn: mMap)
		bn.second->Set(m_t);

	mRootCOM = m_t.segment<3>(0) - mRootCOMOffset;
	mRootCOM *= 0.01;

}
Eigen::Matrix3d
BVH::
Get(const std::string& bvh_node)
{
	return mMap[bvh_node]->Get();
}
void
BVH::
Parse(const std::string& file)
{
	std::ifstream is(file);

	char buffer[256];

	if(!is)
	{
		std::cout<<"Can't Open File"<<std::endl;
		return;
	}
	while(is>>buffer)
	{
		if(!strcmp(buffer,"HIERARCHY"))
		{
			is>>buffer;//Root
			is>>buffer;//Name
			int c_offset = 0;
			mRoot = ReadHierarchy(nullptr,buffer,c_offset,is);
			mNumTotalChannels = c_offset;
		}
		else if(!strcmp(buffer,"MOTION"))
		{
			is>>buffer; //Frames:
			is>>buffer; //num_frames
			mNumTotalFrames = atoi(buffer);
			is>>buffer; //Frame
			is>>buffer; //Time:
			is>>buffer; //time step
			mTimeStep = atof(buffer);
			mMotions.resize(mNumTotalFrames);
			for(auto& m_t : mMotions)
				m_t = Eigen::VectorXd::Zero(mNumTotalChannels);
			double val;
			for(int i=0;i<mNumTotalFrames;i++)
			{
				for(int j=0;j<mNumTotalChannels;j++)
				{
					is>>val;
					mMotions[i][j]=val;
				}
			}
		}
	}
	is.close();
}
BVHNode*
BVH::
ReadHierarchy(BVHNode* parent,const std::string& name,int& channel_offset,std::ifstream& is)
{
	char buffer[256];
	double offset[3];
	std::vector<std::string> c_name;

	BVHNode* new_node = new BVHNode(name,parent);
	mMap.insert(std::make_pair(name,new_node));

	is>>buffer; //{

	while(is>>buffer)
	{
		if(!strcmp(buffer,"}"))
			break;
		if(!strcmp(buffer,"OFFSET"))
		{
			//Ignore
			double x,y,z;

			is>>x;
			is>>y;
			is>>z;
			if(parent==nullptr)
			{
				mRootCOMOffset[0] = x;
				mRootCOMOffset[1] = y;
				mRootCOMOffset[2] = z;
			}
		}
		else if(!strcmp(buffer,"CHANNELS"))
		{

			is>>buffer;
			int n;
			n= atoi(buffer);

			for(int i=0;i<n;i++)
			{
				is>>buffer;
				c_name.push_back(std::string(buffer));
			}

			new_node->SetChannel(channel_offset,c_name);

			channel_offset+=n;
		}
		else if(!strcmp(buffer,"JOINT"))
		{
			is>>buffer;
			BVHNode* child = ReadHierarchy(new_node,std::string(buffer),channel_offset,is);
			new_node->AddChild(child);
		}
		else if(!strcmp(buffer,"End"))
		{
			is>>buffer;
			BVHNode* child = ReadHierarchy(new_node,std::string("EndEffector"),channel_offset,is);
			new_node->AddChild(child);
		}
	}

	return new_node;
}
std::map<std::string,MSS::BVHNode::CHANNEL> BVHNode::CHANNEL_NAME =
{
	{"Xposition",Xpos},
	{"XPOSITION",Xpos},
	{"Yposition",Ypos},
	{"YPOSITION",Ypos},
	{"Zposition",Zpos},
	{"ZPOSITION",Zpos},
	{"Xrotation",Xrot},
	{"XROTATION",Xrot},
	{"Yrotation",Yrot},
	{"YROTATION",Yrot},
	{"Zrotation",Zrot},
	{"ZROTATION",Zrot}
};
};


//	Prior for reconstructing the Equation of State of Dark Energy
//	ref:
//
//  @Jan-19-2017: This file is mainly copied from another projetc of mine, xcos.
//  Some improvemnet were made to make the code more readable.
//
//	@May-17-2019: Now the covariance matrix is loaded from a pre-computed data file

#include <armadillo>
#include <imcmc/imcmc.hpp>
#include <imcmc/parser++.hpp>
#include "Prior.hpp"

using namespace imcmc;
using namespace imcmc::parser;
using namespace std;

DDE_CPZ::DDE_CPZ(){
    floating = false;
    z = NULL;
    a = NULL;
    save_invcov = false; // this is for debug, set to false by default
}

DDE_CPZ::~DDE_CPZ() {

    if( dde_spacing == 0 )
        delete[] a;
    else if( dde_spacing == 1 )
        delete[] z;

}

void DDE_CPZ::Init( std::string& prior_settings ) {

    data_info.GetInfo(prior_settings);

    floating		= Read::Read_Bool_from_File(prior_settings, "dde_wfid_floating");
    nw 				= Read::Read_Int_from_File(prior_settings, "eos_bin_num");

    dde_z_max 		= Read::Read_Double_from_File(prior_settings, "tabulated_w_z_max");
    dde_a_min		= 1./(1.+dde_z_max);

    sigmafid 		= Read::Read_Double_from_File(prior_settings, "dde_cpz_prior_sigmafid");

    save_invcov     = Read::Read_Bool_from_File(prior_settings, "save_invcov");

//	====================================
//  load pre-computed covariance matrix
    string cov_file = Read::Read_String_from_File(prior_settings, "pre-computed_covmat");

//	====================================
//  load pre-determined bin centers
    string abin_file= Read::Read_String_from_File(prior_settings, "abin_centers");

    arma::vec abin_centers;
    abin_centers.load(abin_file,arma::raw_ascii);

//	============================================================
//	@ Nov-8, 2016, update 'sigma_mean_w' option names ...
//	sigma_mean_w is the same for either evenly spaced in z or a
//	============================================================
    sigma_mean_w	= Read::Read_Double_from_File(prior_settings, "dde_cpz_prior_sigma_mean_w");

    C	= arma::zeros(nw, nw);
    iC	= arma::zeros(nw, nw);

    C.load(cov_file, arma::raw_ascii);

    //  We choose sigma_mean_w=0.04 as a reference , so if one wants to change sigma_mean_w,
    //  he just needs to rescale the loaded covariance matrix without re-compute
    //  the covariance matrix
    C = C * pow(sigma_mean_w/0.04,2);

    iC	= C.i();	//	evaluate the inverse of the covariance matrix

//  make a copy of the original covariance matrix without local-average
    C_original  = C;
    iC_original = iC;

    dde_spacing = Read::Read_Int_from_File(prior_settings,"dde_spacing");
    if( dde_spacing == 0 )
        a = new double[nw];
    else if( dde_spacing == 1 )
        z = new double[nw];

//	====================================================================
//	NOTE: zbin[] and abin[] defined below is DIFFERENT from that used in
//	eos approximation !!!
//	====================================================================
    double zbin[nw];
    double abin[nw];

    double Delta;
    double xi0;
    double xij, x_plus, x_minus, xbar;

    if( dde_spacing == 0 ) {		//	evenly spaced in a

        ac	= Read::Read_Double_from_File(prior_settings, "dde_cpz_prior_ac");

        //  try to read smoothing scale $a_s$ from "prior_settings"
		if( Read::Has_Key_in_File(prior_settings,"dde_cpz_prior_as") ){
			as	= Read::Read_Double_from_File(prior_settings, "dde_cpz_prior_as");
		}
		else{
			as	= ac;
		}

    //  Correct bin center of the last abin to make sure that it is correlated with the one next to it.
    // 	Added by XYH @20190516
        abin_centers[nw-1] = abin_centers[nw-2] - (as-1e-10);

        double da = (1.0-dde_a_min)/nw;
        for(int i=0; i<nw; ++i) {
            abin[i] = 1.0 - (i+0.5)*da; // center of each a-bin

            if( floating ){
                a[i] = abin_centers[i];
            }
        }

        Delta 	= da;
        xi0		= sigma_mean_w*sigma_mean_w*(1-dde_a_min)/_PI_/ac;
    }
    else if( dde_spacing == 1 ) {	//	evenly spaced in z

        zc 	= Read::Read_Double_from_File(prior_settings, "dde_cpz_prior_zc");

		if( Read::Has_Key_in_File(prior_settings,"dde_cpz_prior_zs") ) {
			zs	= Read::Read_Double_from_File(prior_settings, "dde_cpz_prior_zs");
		}
		else{
			zs	= zc;
		}

        double dz = dde_z_max/nw;
        for(int i=0; i<nw; ++i) {
            zbin[i] = (i+0.5)*dz;	// center of each a-bin

            if( floating )
                z[i] = zbin[i];
        }

        Delta 	= dz;
        xi0		= sigma_mean_w*sigma_mean_w*dde_z_max/_PI_/zc;
    }
    else {
        string err = "\n*** DDE_CPZ::Init() ==> unsupported z-spacing !!!";
        throw runtime_error(err);
    }

    S	= arma::zeros(nw, nw);  // smoothing matrix
    I	= arma::eye(nw,nw);

    if( floating ) {

        for(int i=0; i<nw; ++i) {
            int count=0;

            if( dde_spacing == 0 ) {
                for( int j=0; j<nw; ++j ) {
                    if( fabs(a[j] - a[i]) <= as + 1E-10 ) {
                        S(i,j) = 1.0;
                        ++count;
                    }
                }
            }
            else if( dde_spacing == 1 ) {
                for( int j=0; j<nw; ++j ) {
                    if( fabs(z[j] - z[i]) <= zs + 1E-10 ) {
                        S(i,j) = 1.0;
                        ++count;
                    }
                }
            }

            for( int j=0; j<nw; ++j )
                S(i,j) = S(i,j)/count;
        }

        IS = I-S;
        iC = IS.t() * iC * IS;
    }

//    S.save("EoS_smooth_matrix.txt", arma::raw_ascii);
//    iC.save("EoS_inv_covmat_20190701.txt",arma::raw_ascii);

    if( save_invcov ){
        if( MPI::COMM_WORLD.Get_rank() == 0 ){
            cout << "Saving inverse of the CPZ covariance matrix \n";
            iC.save("DDE_CPZ_prior_invcov_uncorrected.txt",arma::raw_ascii);
        }
    }

//    exit(0);
}

double Prior_DDE_CPZ(  	imcmc_double&   param,
                        double&         lndet,
                        double&         chisq,
                        void*           model,
                        void*           data,
                        istate&         state ) {

    state.this_like_is_ok=true;

    lndet = chisq = 0;

    DDE_CPZ *cpz = static_cast<DDE_CPZ*>(data);

    arma::rowvec row_w = arma::zeros<arma::rowvec>(cpz->nw);
    arma::colvec col_w = arma::zeros<arma::colvec>(cpz->nw);

//  those extra high-z w_i will not be constrained by CPZ prior.

    for(int i=0; i<cpz->nw; ++i) {
        string wname= "DDE_w"+Read::IntToString(i);
        row_w[i] = param[wname];
        col_w[i] = row_w[i];
    }

    chisq = arma::as_scalar( row_w * cpz->iC * col_w );

    return -lndet - 0.5*chisq;
}


//  (C) Copyright John Maddock 2005.
//  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)

#ifndef BOOST_MATH_EXPM1_INCLUDED
#define BOOST_MATH_EXPM1_INCLUDED

#include <cmath>
#include <math.h> // platform's ::expm1
#include <boost/limits.hpp>
#include <boost/math/special_functions/detail/series.hpp>

#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
#  include <boost/static_assert.hpp>
#else
#  include <boost/assert.hpp>
#endif

#ifdef BOOST_NO_STDC_NAMESPACE
namespace std{ using ::exp; using ::fabs; }
#endif

namespace boost{ namespace math{

namespace detail{
//
// Functor expm1_series returns the next term in the Taylor series
// x^k / k!
// each time that operator() is invoked.
//
template <class T>
struct expm1_series
{
   typedef T result_type;

   expm1_series(T x)
      : k(0), m_x(x), m_term(1) {}

   T operator()()
   {
      ++k;
      m_term *= m_x;
      m_term /= k;
      return m_term;
   }

   int count()const
   {
      return k;
   }

private:
   int k;
   const T m_x;
   T m_term;
   expm1_series(const expm1_series&);
   expm1_series& operator=(const expm1_series&);
};

} // namespace

//
// Algorithm expm1 is part of C99, but is not yet provided by many compilers.
//
// This version uses a Taylor series expansion for 0.5 > |x| > epsilon.
//
template <class T>
T expm1(T x)
{
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
   BOOST_STATIC_ASSERT(::std::numeric_limits<T>::is_specialized);
#else
   BOOST_ASSERT(std::numeric_limits<T>::is_specialized);
#endif

   T a = std::fabs(x);
   if(a > T(0.5L))
      return std::exp(x) - T(1);
   if(a < std::numeric_limits<T>::epsilon())
      return x;
   detail::expm1_series<T> s(x);
   T result = detail::kahan_sum_series(s, std::numeric_limits<T>::digits + 2);
   return result;
}
#if BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x564))
inline float expm1(float z)
{
   return expm1<float>(z);
}
inline double expm1(double z)
{
   return expm1<double>(z);
}
inline long double expm1(long double z)
{
   return expm1<long double>(z);
}
#endif

#ifdef expm1
#  ifndef BOOST_HAS_expm1
#     define BOOST_HAS_expm1
#  endif
#  undef expm1
#endif

#ifdef BOOST_HAS_EXPM1
#  if defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901)
inline float expm1(float x){ return ::expm1f(x); }
inline long double expm1(long double x){ return ::expm1l(x); }
#else
inline float expm1(float x){ return ::expm1(x); }
#endif
inline double expm1(double x){ return ::expm1(x); }
#endif

} } // namespaces

#endif // BOOST_MATH_HYPOT_INCLUDED


/**
 * \file libsanm/tensor_linalg.cpp
 * This file is part of SANM, a symbolic asymptotic numerical solver.
 */

#include "libsanm/tensor_impl_helper.h"

#include <Eigen/Dense>

using namespace sanm;

namespace {
/*!
 * \brief compute cofactor of a single matrix
 * \param s_prod_rev a workspace of size dim+1, and the last value must be
 *      initialized to one
 */
template <int mat_dim>
void compute_cofactor(EigenMat<mat_dim, mat_dim>& mdst,
                      EigenMat<mat_dim, mat_dim>& msrc, fp_t* s_prod_rev) {
    Eigen::Index dim = msrc.rows();

    Eigen::JacobiSVD<Eigen::Matrix<fp_t, mat_dim, mat_dim>> svd{
            msrc, Eigen::ComputeFullU | Eigen::ComputeFullV};

    if (svd.rank() + 2 <= dim) {
        mdst.setZero();
        return;
    }

    // det(S) * S.inv()
    Eigen::Matrix<fp_t, mat_dim, 1> sinvd = svd.singularValues();
    if constexpr (mat_dim == 2) {
        fp_t a = sinvd(0), b = sinvd(1);
        sinvd(0) = b;
        sinvd(1) = a;
    } else if constexpr (mat_dim == 3) {
        fp_t a = sinvd(0), b = sinvd(1), c = sinvd(2);
        sinvd(0) = b * c;
        sinvd(1) = a * c;
        sinvd(2) = a * b;
    } else {
        for (int i = dim - 1; i >= 0; --i) {
            s_prod_rev[i] = s_prod_rev[i + 1] * sinvd[i];
        }
        fp_t prod = 1;
        for (Eigen::Index i = 0; i < dim; ++i) {
            fp_t orig = sinvd(i);
            sinvd(i) = prod * s_prod_rev[i + 1];
            prod *= orig;
        }
    }

    fp_t sign = (svd.matrixU() * svd.matrixV().transpose()).determinant();
    if (sign < 0) {
        sinvd = -sinvd;
    }
    mdst = svd.matrixU() * sinvd.asDiagonal() * svd.matrixV().transpose();
}

template <typename T>
constexpr T get_from_pair(int d, T x, T y) {
    return d == 0 ? x : y;
}

CBLAS_TRANSPOSE cblas_trans(bool t) {
    return t ? CblasTrans : CblasNoTrans;
}
}  // anonymous namespace

TensorND& TensorND::as_mm(const TensorND& lhs, const TensorND& rhs, bool accum,
                          bool trans_lhs, bool trans_rhs) {
    SANM_SCOPED_PROFILER("mm");
    sanm_assert(this != &lhs && this != &rhs);
    sanm_assert(lhs.rank() == 2 && rhs.rank() == 2);
    TensorShape dst_shape{
            get_from_pair(trans_lhs, lhs.shape(0), lhs.shape(1)),
            get_from_pair(trans_rhs ^ 1, rhs.shape(0), rhs.shape(1))};

    if (accum) {
        sanm_assert(shape() == dst_shape, "mm accum: expect shape %s, got %s",
                    dst_shape.str().c_str(), shape().str().c_str());

        if (lhs.is_zero() || rhs.is_zero()) {
            return *this;
        }
    } else {
        if (lhs.is_zero() || rhs.is_zero()) {
            // do not use set_shape to avoid unnecessary memory allocation
            m_shape = dst_shape;
            return fill_with_inplace(0);
        }
        set_shape(dst_shape);
    }

    static_assert(std::is_same_v<fp_t, double>, "unhandled fp_t");
    if constexpr (std::is_same_v<fp_t, double>) {
        cblas_dgemm(CblasRowMajor, cblas_trans(trans_lhs),
                    cblas_trans(trans_rhs), dst_shape[0], dst_shape[1],
                    get_from_pair(trans_lhs ^ 1, lhs.shape(0), lhs.shape(1)), 1,
                    lhs.ptr(), lhs.shape(1), rhs.ptr(), rhs.shape(1),
                    accum ? 1 : 0, rwptr(), dst_shape[1]);
    }
    return *this;
}

TensorND& TensorND::as_batched_mm(const TensorND& lhs, const TensorND& rhs,
                                  bool accum, bool trans_lhs, bool trans_rhs) {
    SANM_SCOPED_PROFILER("batched_mm");
    sanm_assert(this != &lhs && this != &rhs);
    sanm_assert(
            lhs.rank() == 3 && rhs.rank() == 3 && lhs.shape(0) == rhs.shape(0),
            "batched mm shape mismatch %s vs %s", lhs.shape().str().c_str(),
            rhs.shape().str().c_str());
    const size_t ls0 = lhs.shape(1), ls1 = lhs.shape(2), rs0 = rhs.shape(1),
                 rs1 = rhs.shape(2), batch = lhs.shape(0);
    sanm_assert(get_from_pair(trans_lhs ^ 1, ls0, ls1) ==
                        get_from_pair(trans_rhs, rs0, rs1),
                "matmul shape mismatch: %s vs %s, trans=%d,%d",
                lhs.shape().str().c_str(), rhs.shape().str().c_str(), trans_lhs,
                trans_rhs);

    TensorShape dst_shape{batch, get_from_pair(trans_lhs, ls0, ls1),
                          get_from_pair(trans_rhs ^ 1, rs0, rs1)};

    if (accum) {
        sanm_assert(shape() == dst_shape,
                    "batched_mm accum: expect shape %s, got %s",
                    dst_shape.str().c_str(), shape().str().c_str());

        if (lhs.is_zero() || rhs.is_zero()) {
            return *this;
        }
    } else {
        if (lhs.is_zero() || rhs.is_zero()) {
            // do not use set_shape to avoid unnecessary memory allocation
            m_shape = dst_shape;
            return fill_with_inplace(0);
        }
        set_shape(dst_shape);
    }

    // As of 2020, MKL batched dgemm is still slow for small matrices ...
    // So we roll out our simple accelerated impl using eigen

    auto pa = const_cast<fp_t*>(lhs.ptr()), pb = const_cast<fp_t*>(rhs.ptr()),
         pc = rwptr();
    auto run_static_shape = [pa, pb, pc, batch, accum, trans_lhs,
                             trans_rhs]<int ls0, int ls1, int rs0, int rs1>() {

    // eigen uses col-major; so we compute on the transformed matrices
#define FOREACH_8(cb)                                                       \
    cb(0, 0, 0) cb(0, 0, 1) cb(0, 1, 0) cb(0, 1, 1) cb(1, 0, 0) cb(1, 0, 1) \
            cb(1, 1, 0) cb(1, 1, 1)
#define ACC0 =
#define ACC1 +=
#define TR0(x) x
#define TR1(x) x.transpose()
#define CB(acc, tra, trb)                                               \
    if constexpr (get_from_pair(tra ^ 1, ls0, ls1) ==                   \
                  get_from_pair(trb, rs0, rs1)) {                       \
        constexpr int cs0 = get_from_pair(tra, ls0, ls1),               \
                      cs1 = get_from_pair(trb ^ 1, rs0, rs1);           \
        if (accum == acc && trans_lhs == tra && trans_rhs == trb) {     \
            for (size_t ib = 0; ib < batch; ++ib) {                     \
                EigenMat<ls1, ls0> ta{pa + ib * (ls0 * ls1), ls1, ls0}; \
                EigenMat<rs1, rs0> tb{pb + ib * (rs0 * rs1), rs1, rs0}; \
                EigenMat<cs1, cs0> tc{pc + ib * (cs0 * cs1), cs1, cs0}; \
                tc.noalias() ACC##acc TR##trb(tb) * TR##tra(ta);        \
            }                                                           \
            return;                                                     \
        }                                                               \
    }
        FOREACH_8(CB)
        sanm_assert(0, "impossible");
#undef CB
#undef TR1
#undef TR0
#undef ACC1
#undef ACC0
#undef FOREACH_8
    };

#define CASE(ls0_, ls1_, rs0_, rs1_)                                    \
    do {                                                                \
        if (ls0_ == ls0 && ls1_ == ls1 && rs0_ == rs0 && rs1_ == rs1) { \
            run_static_shape.operator()<ls0_, ls1_, rs0_, rs1_>();      \
            return *this;                                               \
        }                                                               \
    } while (0)

    CASE(1, 1, 1, 1);
    CASE(2, 2, 2, 2);
    CASE(3, 3, 3, 3);
    CASE(4, 4, 4, 4);

    // special handling of shapes can be added here
#undef CASE

    static_assert(std::is_same_v<fp_t, double>, "unhandled fp_t");
    if constexpr (std::is_same_v<fp_t, double>) {
        cblas_dgemm_batch_strided(
                CblasRowMajor, cblas_trans(trans_lhs), cblas_trans(trans_rhs),
                dst_shape[1], dst_shape[2],
                get_from_pair(trans_lhs ^ 1, ls0, ls1), 1, lhs.ptr(), ls1,
                ls0 * ls1, rhs.ptr(), rs1, rs0 * rs1, accum ? 1 : 0, rwptr(),
                dst_shape[2], dst_shape[1] * dst_shape[2], batch);
    }
    return *this;
}

TensorND& TensorND::as_batched_transpose(const TensorND& src) {
    SANM_SCOPED_PROFILER("batched_transpose");
    sanm_assert(this != &src);
    sanm_assert(src.rank() == 3);
    {
        TensorShape oshp = src.shape();
        std::swap(oshp.dim[1], oshp.dim[2]);
        set_shape(oshp);
    }
    if (src.is_zero()) {
        return fill_with_inplace(0);
    }
    auto work = [this, &src]<int mat_dim0, int mat_dim1>() {
        auto sptr = const_cast<fp_t*>(src.ptr()), dptr = this->woptr();
        const size_t batch = src.shape(0);
        const Eigen::Index dim0 = src.shape(1), dim1 = src.shape(2);
        for (size_t i = 0; i < batch; ++i) {
            EigenMat<mat_dim0, mat_dim1> mdst{dptr + i * dim0 * dim1, dim0,
                                              dim1};
            EigenMat<mat_dim1, mat_dim0> msrc{sptr + i * dim0 * dim1, dim1,
                                              dim0};
            mdst = msrc.transpose();
        }
    };

    auto work_dispatch = [&work, &src]<int mat_dim0>() {
        switch (src.shape(2)) {
#define ON(x)                           \
    case x:                             \
        work.operator()<mat_dim0, x>(); \
        break
            ON(1);
            ON(2);
            ON(3);
            ON(4);
#undef ON
            default:
                work.operator()<mat_dim0, Eigen::Dynamic>();
                break;
        }
    };

    switch (src.shape(1)) {
#define ON(x)                          \
    case x:                            \
        work_dispatch.operator()<x>(); \
        break
        ON(1);
        ON(2);
        ON(3);
        ON(4);
#undef ON
        default:
            work_dispatch.operator()<Eigen::Dynamic>();
            break;
    }
    return *this;
}

TensorND& TensorND::as_transpose(const TensorND& src) {
    sanm_assert(src.rank() == 2);
    size_t m = src.shape(0), n = src.shape(1);
    set_shape({n, m});
    if (src.is_zero()) {
        return fill_with_inplace(0);
    }
    auto ei = [](size_t x) -> Eigen::Index { return x; };
    EigenMatDyn mdst{woptr(), ei(m), ei(n)},
            msrc{const_cast<fp_t*>(src.ptr()), ei(n), ei(m)};
    mdst = msrc.transpose();
    return *this;
}

TensorND& TensorND::as_batched_matinv(const TensorND& src) {
    SANM_SCOPED_PROFILER("batched_matinv");
    sanm_assert(this != &src);
    sanm_assert(src.rank() == 3 && src.shape(1) == src.shape(2));
    sanm_assert(!src.is_zero());
    set_shape(src.shape());
    auto work = [this, &src]<int mat_dim>() {
        ScopedAllowMalloc scoped_allow_malloc;
        auto sptr = const_cast<fp_t*>(src.ptr()), dptr = this->woptr();
        const size_t batch = src.shape(0);
        const Eigen::Index dim = src.shape(1);
        for (size_t i = 0; i < batch; ++i) {
            EigenMat<mat_dim, mat_dim> mdst{dptr + i * dim * dim, dim, dim},
                    msrc{sptr + i * dim * dim, dim, dim};
            mdst = msrc.inverse();
        }
    };
    switch (src.shape(1)) {
#define ON(x)                 \
    case x:                   \
        work.operator()<x>(); \
        break
        ON(1);
        ON(2);
        ON(3);
        ON(4);
#undef ON
        default:
            work.operator()<Eigen::Dynamic>();
            break;
    }
    return *this;
}

TensorND& TensorND::as_batched_determinant(const TensorND& src) {
    SANM_SCOPED_PROFILER("batched_det");
    ScopedAllowMalloc allow_mem_alloc;

    sanm_assert(this != &src);
    sanm_assert(src.rank() == 3 && src.shape(1) == src.shape(2));
    set_shape({src.shape(0), 1});
    if (src.is_zero()) {
        return fill_with_inplace(0);
    }
    auto work = [this, &src]<int mat_dim>() {
        auto sptr = const_cast<fp_t*>(src.ptr()), dptr = this->woptr();
        const size_t batch = src.shape(0);
        const Eigen::Index dim = src.shape(1);
        for (size_t i = 0; i < batch; ++i) {
            EigenMat<mat_dim, mat_dim> msrc{sptr + i * dim * dim, dim, dim};
            dptr[i] = msrc.determinant();
        }
    };
    switch (src.shape(1)) {
#define ON(x)                 \
    case x:                   \
        work.operator()<x>(); \
        break
        ON(1);
        ON(2);
        ON(3);
        ON(4);
#undef ON
        default:
            work.operator()<Eigen::Dynamic>();
            break;
    }
    return *this;
}

TensorND& TensorND::as_batched_cofactor(const TensorND& src) {
    SANM_SCOPED_PROFILER("batched_cofactor");
    ScopedAllowMalloc allow_mem_alloc;

    sanm_assert(this != &src);
    sanm_assert(src.rank() == 3 && src.shape(1) == src.shape(2) &&
                src.shape(1) >= 2);
    set_shape(src.shape());
    if (src.is_zero()) {
        return fill_with_inplace(0);
    }
    auto work = [this, &src]<int mat_dim>() {
        auto sptr = const_cast<fp_t*>(src.ptr()), dptr = this->woptr();
        const size_t batch = src.shape(0);
        const Eigen::Index dim = src.shape(1);
        std::unique_ptr<fp_t[]> s_prod_rev{new fp_t[dim + 1]};
        s_prod_rev[dim] = 1;
        for (size_t i = 0; i < batch; ++i) {
            EigenMat<mat_dim, mat_dim> mdst{dptr + i * dim * dim, dim, dim},
                    msrc{sptr + i * dim * dim, dim, dim};
            compute_cofactor(mdst, msrc, s_prod_rev.get());
        }
    };
    switch (src.shape(1)) {
#define ON(x)                 \
    case x:                   \
        work.operator()<x>(); \
        break
        ON(2);
        ON(3);
        ON(4);
#undef ON
        default:
            work.operator()<Eigen::Dynamic>();
            break;
    }
    return *this;
}

TensorND& TensorND::as_batched_mm_vecitem_left(const TensorND& lhs,
                                               const TensorND& rhs,
                                               bool accum) {
    SANM_SCOPED_PROFILER("batched_mm_vecitem");
    sanm_assert(this != &lhs && this != &rhs);
    sanm_assert(lhs.rank() == 3 && rhs.rank() == 3);
    size_t BATCH = lhs.shape(0), M = lhs.shape(1), P = lhs.shape(2),
           K = rhs.shape(1), N = rhs.shape(2);
    sanm_assert(M % K == 0 && BATCH == rhs.shape(0));
    M /= K;

    TensorShape expect_shape{BATCH, M * N, P};
    if (accum) {
        sanm_assert(m_shape == expect_shape);
    } else {
        set_shape(expect_shape);
    }

    if (lhs.is_zero() || rhs.is_zero()) {
        return fill_with_inplace(0);
    }

    auto ei = [](size_t x) -> Eigen::Index { return x; };

    auto lptr = const_cast<fp_t*>(lhs.ptr()),
         rptr = const_cast<fp_t*>(rhs.ptr()), optr = this->woptr();
    for (size_t b = 0; b < BATCH; ++b) {
        EigenMatDyn mat_rhs{rptr + b * K * N, ei(N), ei(K)};
        for (size_t m = 0; m < M; ++m) {
            EigenMatDyn mat_lhs{lptr + (b * M + m) * K * P, ei(P), ei(K)},
                    mat_dst{optr + (b * M + m) * N * P, ei(P), ei(N)};
            if (accum) {
                mat_dst.noalias() += mat_lhs * mat_rhs.transpose();
            } else {
                mat_dst.noalias() = mat_lhs * mat_rhs.transpose();
            }
        }
    }

    return *this;
}


#include <cstdlib>
#include <iostream>
#include <complex>
#include <type_traits>
#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/banded.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include "print.hpp"
#include "random.hpp"

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

int main(int argc, char *argv[]) {
  {
    typedef std::complex<double> complex;
    typedef ublas::vector<complex> vector;
    typedef ublas::banded_matrix<complex, ublas::column_major> matrix;
    typedef typename std::make_signed<vector::size_type>::type size_type;
    rand_normal<complex>::reset();
    size_type m=6, n=8;
    matrix A(m, n, 1, 2);
    for (size_type j=0; j<n; ++j)
      for (size_type i=std::max(j-2, size_type(0)); i<std::min(j+1+1, m); ++i)
 	A(i, j)=rand_normal<complex>::get();
    // A banded ublas matrux and its transpose have very different
    // memory layouts, thus the following line does not work.
    // matrix A_t(ublas::trans(A));
    matrix A_t(n, m, 2, 1);
    for (size_type j=0; j<m; ++j)
      for (size_type i=std::max(j-1, size_type(0)); i<std::min(j+2+1, n); ++i)
    	A_t(i, j)=A(j, i);
    vector x(n);
    for (size_type i=0; i<n; ++i)
      x(i)=rand_normal<complex>::get();
    vector y(m);
    for (size_type i=0; i<m; ++i)
      y(i)=rand_normal<complex>::get();
    complex alpha(rand_normal<complex>::get());
    complex beta(rand_normal<complex>::get());
    vector y1(alpha*ublas::prod(A, x)+beta*y);
    vector y2(y);
    blas::gbmv(alpha, A, x, beta, y2);
    vector y3(y);
    blas::gbmv(alpha, blas::trans(A_t), x, beta, y3);
    std::cout << "testing boost::ublas containers\n"
	      << "using ublas           : " << print_vec(y1) << '\n'
	      << "using blas            : " << print_vec(y2) << '\n'
	      << "using blas (tranposed): " << print_vec(y3) << '\n'
	      << '\n';
  }
  return EXIT_SUCCESS;
}


/*
 * phase_oscillator_ensemble.cpp
 *
 * Demonstrates the phase transition from an unsynchronized to an synchronized state.
 *
 * Copyright 2011-2012 Karsten Ahnert
 * Copyright 2011-2012 Mario Mulansky
 * 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)
 *
 */

#include <iostream>
#include <utility>

#include <boost/numeric/odeint.hpp>

#ifndef M_PI //not there on windows
#define M_PI 3.141592653589793 //...
#endif

#include <boost/random.hpp>

using namespace std;
using namespace boost::numeric::odeint;

//[ phase_oscillator_ensemble_system_function
typedef vector< double > container_type;

pair< double , double > calc_mean_field( const container_type &x )
{
    size_t n = x.size();
    double cos_sum = 0.0 , sin_sum = 0.0;
    for( size_t i=0 ; i<n ; ++i )
    {
        cos_sum += cos( x[i] );
        sin_sum += sin( x[i] );
    }
    cos_sum /= double( n );
    sin_sum /= double( n );

    double K = sqrt( cos_sum * cos_sum + sin_sum * sin_sum );
    double Theta = atan2( sin_sum , cos_sum );

    return make_pair( K , Theta );
}

struct phase_ensemble
{
    container_type m_omega;
    double m_epsilon;

    phase_ensemble( const size_t n , double g = 1.0 , double epsilon = 1.0 )
    : m_omega( n , 0.0 ) , m_epsilon( epsilon )
    {
        create_frequencies( g );
    }

    void create_frequencies( double g )
    {
        boost::mt19937 rng;
        boost::cauchy_distribution<> cauchy( 0.0 , g );
        boost::variate_generator< boost::mt19937&, boost::cauchy_distribution<> > gen( rng , cauchy );
        generate( m_omega.begin() , m_omega.end() , gen );
    }

    void set_epsilon( double epsilon ) { m_epsilon = epsilon; }

    double get_epsilon( void ) const { return m_epsilon; }

    void operator()( const container_type &x , container_type &dxdt , double /* t */ ) const
    {
        pair< double , double > mean = calc_mean_field( x );
        for( size_t i=0 ; i<x.size() ; ++i )
            dxdt[i] = m_omega[i] + m_epsilon * mean.first * sin( mean.second - x[i] );
    }
};
//]

//[ phase_oscillator_ensemble_observer
struct statistics_observer
{
    double m_K_mean;
    size_t m_count;

    statistics_observer( void )
    : m_K_mean( 0.0 ) , m_count( 0 ) { }

    template< class State >
    void operator()( const State &x , double t )
    {
        pair< double , double > mean = calc_mean_field( x );
        m_K_mean += mean.first;
        ++m_count;
    }

    double get_K_mean( void ) const { return ( m_count != 0 ) ? m_K_mean / double( m_count ) : 0.0 ; }

    void reset( void ) { m_K_mean = 0.0; m_count = 0; }
};
//]

int main( int argc , char **argv )
{
    //[ phase_oscillator_ensemble_integration
    const size_t n = 16384;
    const double dt = 0.1;

    container_type x( n );

    boost::mt19937 rng;
    boost::uniform_real<> unif( 0.0 , 2.0 * M_PI );
    boost::variate_generator< boost::mt19937&, boost::uniform_real<> > gen( rng , unif );

    // gamma = 1, the phase transition occurs at epsilon = 2
    phase_ensemble ensemble( n , 1.0 );
    statistics_observer obs;

    for( double epsilon = 0.0 ; epsilon < 5.0 ; epsilon += 0.1 )
    {
        ensemble.set_epsilon( epsilon );
        obs.reset();

        // start with random initial conditions
        generate( x.begin() , x.end() , gen );

        // calculate some transients steps
        integrate_const( runge_kutta4< container_type >() , boost::ref( ensemble ) , x , 0.0 , 10.0 , dt );

        // integrate and compute the statistics
        integrate_const( runge_kutta4< container_type >() , boost::ref( ensemble ) , x , 0.0 , 100.0 , dt , boost::ref( obs ) );
        cout << epsilon << "\t" << obs.get_K_mean() << endl;
    }

    //]

    return 0;
}


// Copyright (C) 2016 Daniele Panozzo <daniele.panozzo@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "tutorial_nrosy.h"
#include <Eigen/Geometry>
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
#include <Eigen/Eigenvalues>
#include <iostream>

#include "mesh_param.h"

using namespace std;
using namespace Eigen;

MatrixXd tutorial_nrosy
        (
        const MatrixXd& V,          // Vertices of the mesh
        const MatrixXi& F,          // Faces
        const MatrixXi& TT,         // Adjacency triangle-triangle
        const VectorXi& soft_id,    // Soft constraints face ids
        const MatrixXd& soft_value, // Soft constraints 3d vectors
        const int n                 // Degree of the n-rosy field
        )
{
  assert(soft_id.size() > 0); // One constraint is necessary to make the solution unique

  // This code works only for n==1, see tutorial_nrosy_complete for a generic implementation that works for n >= 1
  assert(n==1);

  Matrix<double,Eigen::Dynamic,3> T1(F.rows(),3), T2(F.rows(),3);

  // Compute the local reference systems for each face
  for (unsigned i=0;i<F.rows();++i)
  {
    Vector3d e1 =  V.row(F(i, 1)) - V.row(F(i, 0));
    Vector3d e2 =  V.row(F(i, 2)) - V.row(F(i, 0));
    T1.row(i) = e1.normalized();
    T2.row(i) = T1.row(i).cross(T1.row(i).cross(e2)).normalized();
  }

  // Build the sparse matrix, with an energy term for each edge
  std::vector< Triplet<std::complex<double> > > t;
  std::vector< Triplet<std::complex<double> > > tb;

  unsigned count = 0;
  for (unsigned f=0;f<F.rows();++f)
  {
    for (unsigned ei=0;ei<F.cols();++ei)
    {
      // Look up the opposite face
      int g = TT(f,ei);
      // If it is a boundary edge, it does not contribute to the energy
      if (g == -1) continue;
      // Avoid to count every edge twice
      if (f > g) continue;
      // Compute the complex representation of the common edge
      Vector3d e  = (V.row(F(f,(ei+1)%3)) - V.row(F(f,ei)));
      Vector2d vef = Vector2d(e.dot(T1.row(f)),e.dot(T2.row(f))).normalized();
      std::complex<double> ef(vef(0),vef(1));
      Vector2d veg = Vector2d(e.dot(T1.row(g)),e.dot(T2.row(g))).normalized();
      std::complex<double> eg(veg(0),veg(1));
      // Add the term conj(f)^n*ui - conj(g)^n*uj to the energy matrix
      t.push_back(Triplet<std::complex<double> >(count,f,    std::conj(ef)));
      t.push_back(Triplet<std::complex<double> >(count,g,-1.*std::conj(eg)));
      ++count;
    }
  }

  // Convert the constraints into the complex polynomial coefficients and add them as soft constraints
  double lambda = 10e6;
  for (unsigned r=0; r<soft_id.size(); ++r)
  {
    int f = soft_id(r);
    Vector3d v = soft_value.row(r);
    std::complex<double> c(v.dot(T1.row(f)),v.dot(T2.row(f)));
    t.push_back(Triplet<std::complex<double> >(count,f, sqrt(lambda)));
    tb.push_back(Triplet<std::complex<double> >(count,0, c * std::complex<double>(sqrt(lambda),0)));
    ++count;
  }

  // Solve the linear system
  typedef SparseMatrix<std::complex<double>> SparseMatrixXcd;
  SparseMatrixXcd A(count,F.rows());
  A.setFromTriplets(t.begin(), t.end());
  SparseMatrixXcd b(count,1);
  b.setFromTriplets(tb.begin(), tb.end());
  SimplicialLDLT< SparseMatrixXcd > solver;
  solver.compute(A.adjoint()*A);
  assert(solver.info()==Success);
  MatrixXcd u = solver.solve(A.adjoint()*MatrixXcd(b));
  assert(solver.info()==Success);

  // Debugging informations
  #ifdef DEBUG_1
  cout << "V: " << V.rows() << " * " << V.cols() << endl;
  cout << "F: " << F.rows() << " * " << F.cols() << endl;
  cout << "TT: " << TT.rows() << " * " << TT.cols() << endl;
  cout << "soft_id: " << soft_id.rows() << " * " << soft_id.cols() << endl;
  cout << "soft_value: " << soft_value.rows() << " * " << soft_value.cols() << endl;
  cout << "T1: " << T1.rows() << " * " << T1.cols() << endl;
  cout << "T2: " << T2.rows() << " * " << T2.cols() << endl;
  cout << "t: " << t.size() << endl;
  cout << "tb: " << tb.size() << endl;
  cout << "count = " << count << endl;
  cout << "A: " << A.rows() << " * " << A.cols() << endl;
  cout << "A*: " << A.adjoint().rows() << " * " << A.adjoint().cols() << endl;
  cout << "b: " << b.rows() << " * " << b.cols() << endl;
  cout << "u: " << u.rows() << " * " << u.cols() << endl;
  #endif

  // Convert the interpolated polyvector into Euclidean vectors
  MatrixXd R(F.rows(),3);
  for (int f=0; f<F.rows(); ++f)
    R.row(f) = T1.row(f) * u(f).real() + T2.row(f) * u(f).imag();

  #ifdef DEBUG_4
  cout << "final locations and values: ";
  for (int i = 0; i < soft_id.size(); i++) {
      cout << soft_id(i) << "," << u(soft_id(i)) << " ";
  }
  cout << endl;
  cout << "final locations and coordinates: ";
  for (int i = 0; i < soft_id.size(); i++) {
      cout << soft_id(i) << "," << R.row(soft_id(i)) << " ";
  }
  cout << endl;
  cout << "original locations and coordinates: ";
  for (int i = 0; i < soft_id.size(); i++) {
      cout << soft_id(i) << "," << soft_value.row(i) << " ";
  }
  cout << endl;
  #endif

  return R;
}


#include <multi-camera-motion/approx_relpose_generalized.h>
#include <multi-camera-motion/translation.h>

#include <Eigen/Geometry>

#include "problem.h"
#include "so3.h"

#include <iostream>

#include <cstdlib>
#include <ctime>

static Eigen::Vector2d project( const Eigen::Vector3d &x )
{
    return x.head(2)/x[2];
}

static Eigen::Matrix<double,6,1> pluecker( const Ray & ray )
{
    // make ray in Pluecker coordinates
    Eigen::Vector3d d = ray.x/ray.x.norm();
    Eigen::Vector3d m = ray.c.cross(d);
    Eigen::Matrix<double,6,1> p;
    p.head(3) = d;
    p.tail(3) = m;
    return p;
}

static Eigen::Matrix<double,6,6> make_w( const RayPair &ray_pair )
{
    Eigen::Matrix<double,6,1> u = pluecker(ray_pair.first);
    Eigen::Matrix<double,6,1> v = pluecker(ray_pair.second);
    return u*v.transpose();
}

int main( int argc, char **argv )
{
    srand(time(NULL));

    const double trans_mag = 1.;
    const double angle = 5.*M_PI/180.;
    const double noise = 0;

    Problem prob;
    generateProblem( trans_mag, angle, noise, prob );

    Eigen::MatrixXd x(2,6);
    Eigen::MatrixXd y(2,6);
    Eigen::MatrixXd cu(3,6);
    Eigen::MatrixXd cv(3,6);
    for ( int i = 0; i < 6; i++ )
    {
        x.col(i) = project(prob.ray_pairs[i].first.x);
        y.col(i) = project(prob.ray_pairs[i].second.x);
        cu.col(i) = prob.ray_pairs[i].first.c;
        cv.col(i) = prob.ray_pairs[i].second.c;
    }

    Eigen::Matrix<double,6,6> w1;
    Eigen::Matrix<double,6,6> w2;
    Eigen::Matrix<double,6,6> w3;
    Eigen::Matrix<double,6,6> w4;
    Eigen::Matrix<double,6,6> w5;
    Eigen::Matrix<double,6,6> w6;
    std::vector<Eigen::Vector3d> rsolns;

    w1 = make_w(prob.ray_pairs[0]);
    w2 = make_w(prob.ray_pairs[1]);
    w3 = make_w(prob.ray_pairs[2]);
    w4 = make_w(prob.ray_pairs[3]);
    w5 = make_w(prob.ray_pairs[4]);
    w6 = make_w(prob.ray_pairs[5]);
    approx_relpose_generalized(w1,w2,w3,w4,w5,w6,rsolns);

    for ( int i = 0; i < rsolns.size(); i++ )
    {
        Eigen::Matrix3d Rsoln = so3exp(rsolns[i]);
        Eigen::Vector3d tsoln = solve_translation(x,y,cu,cv,Rsoln);
        double rot_angle_err, trans_angle_err, trans_scale_err;
        compute_error( prob.R, prob.t,
                       Rsoln, tsoln,
                       rot_angle_err, trans_angle_err, trans_scale_err );
        std::cout << "solution " << i+1 << " error: ";
        std::cout << "\trotation error: " << rot_angle_err*180./M_PI << " deg\n";
        std::cout << "\ttranslation error: " << trans_angle_err*180./M_PI << " deg\n";
        std::cout << "\ttranslation scale error: " << trans_scale_err << "\n";
    }

}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

// Peter Sonneveld and Martin B. van Gijzen, IDR(s): a family of simple and fast algorithms for solving large nonsymmetric linear systems.
// SIAM J. Sci. Comput. Vol. 31, No. 2, pp. 1035-1062 (2008). (copyright SIAM)

#ifndef ITL_IDR_S_INCLUDE
#define ITL_IDR_S_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/operation/random.hpp>
#include <boost/numeric/mtl/operation/orth.hpp>
#include <boost/numeric/mtl/operation/resource.hpp>
#include <boost/numeric/mtl/matrix/strict_upper.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/irange.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

#include <boost/numeric/itl/krylov/base_solver.hpp>

namespace itl {

/// Induced Dimension Reduction on s dimensions (IDR(s))
template < typename LinearOperator, typename Vector,
	   typename LeftPreconditioner, typename RightPreconditioner,
	   typename Iteration >
int idr_s(const LinearOperator &A, Vector &x, const Vector &b,
	  const LeftPreconditioner &, const RightPreconditioner &,
	  Iteration& iter, size_t s)
{
    mtl::vampir_trace<7010> tracer;
    using mtl::size; using mtl::iall; using mtl::mat::strict_upper;
    typedef typename mtl::Collection<Vector>::value_type Scalar;
    typedef typename mtl::Collection<Vector>::size_type  Size;

    if (size(b) == 0) throw mtl::logic_error("empty rhs vector");
    if (s < 1) s= 1;

    const Scalar                zero= math::zero(Scalar());
    Scalar                      omega(zero);
    Vector                      x0(x), y(resource(x)), v(resource(x)), t(resource(x)), q(resource(x)), r(b - A * x);
    mtl::mat::multi_vector<Vector>   dR(Vector(resource(x), zero), s), dX(Vector(resource(x), zero), s), P(Vector(resource(x), zero), s);
    mtl::dense_vector<Scalar>   m(s), c(s), dm(s);   // replicated in distributed solvers
    mtl::mat::dense2D<Scalar>        M(s, s);             // dito

    random(P);
    P.vector(0)= r;
    orth(P);

    for (size_t k= 0; k < s; k++) {
	v= A * r;
	omega= dot(v, r) / dot(v, v);
	dX.vector(k)= omega * r;
	dR.vector(k)= -omega * v;
	x+= dX.vector(k);
	r+= dR.vector(k);
	if ((++iter).finished(r)) return iter;
	M[iall][k]= trans(P) * dR.vector(k);
    }

    Size oldest= 0;
    m= trans(P) * r;

    while (! iter.finished(r)) {

	for (size_t k= 0; k < s; k++) {
	    c= lu_solve(M, m);
	    q= dR * -c;
	    v= r + q;
	    if (k == 0) {
		t= A * v;
		omega= dot(t, v) / dot(t, t);
		dR.vector(oldest)= q - omega * t;
		dX.vector(oldest)= omega * v - dX * c;
	    } else {
		dX.vector(oldest)= omega * v - dX * c;
		dR.vector(oldest)= A * -dX.vector(oldest);
	    }
	    r+= dR.vector(oldest);
	    x+= dX.vector(oldest);

	    if ((++iter).finished(r))
		return iter;

	    dm= trans(P) * dR.vector(oldest);
	    M[iall][oldest]= dm;
	    m+= dm;
	    oldest= (oldest + 1) % s;
	}
    }
    return iter;
}

/// Solver class for IDR(s) method; right preconditioner ignored (prints warning if not identity)
/** Methods inherited from \ref base_solver. **/
template < typename LinearOperator, typename Preconditioner= pc::identity<LinearOperator>,
	   typename RightPreconditioner= pc::identity<LinearOperator> >
class idr_s_solver
  : public base_solver< idr_s_solver<LinearOperator, Preconditioner, RightPreconditioner>, LinearOperator >
{
    typedef base_solver< idr_s_solver<LinearOperator, Preconditioner, RightPreconditioner>, LinearOperator > base;
  public:
  public:
    /// Construct solver from a linear operator; generate (left) preconditioner from it
    explicit idr_s_solver(const LinearOperator& A, size_t s= 8) : base(A), s(s), L(A), R(A) {}

    /// Construct solver from a linear operator and left preconditioner
    idr_s_solver(const LinearOperator& A, size_t s, const Preconditioner& L) : base(A), s(s), L(L), R(A) {}

    /// Construct solver from a linear operator and left preconditioner
    idr_s_solver(const LinearOperator& A, size_t s, const Preconditioner& L, const RightPreconditioner& R)
      : base(A), s(s), L(L), R(R) {}

    /// Solve linear system approximately as specified by \p iter
    template < typename HilbertSpaceX, typename HilbertSpaceB, typename Iteration >
    int solve(HilbertSpaceX& x, const HilbertSpaceB& b, Iteration& iter) const
    {
	return idr_s(this->A, x, b, L, R, iter, s);
    }

  private:
    size_t                s;
    Preconditioner        L;
    RightPreconditioner   R;
};

} // namespace itl

#endif // ITL_IDR_S_INCLUDE


#pragma once

#include <vector>

#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Eigen;

/**
 * @brief Calcola l'insieme di facce adiancenti a ogni vertice.
 *
 * Una faccia e' adiacente a un vertice se la faccia punta ad esso,
 * ovvero se esso e' uno dei suoi corner.
 *
 * @param V I vertici della mesh. Per ogni riga della matrice V, la posizione
 *          del vertice e' costituita dalle coordinate x,y,z memorizzate nelle
 *          3 colonne della riga.
 * @param F I triangoli della mesh. Per ogni riga della matrice F, il triangolo
 *          e' descritto dagli indici i,j,k memorizzati nelle 3 colonne della
 *          riga. Gli indici i,j,k si riferiscono ai 3 vertici A, B, C del
 *          triangolo, memorizzati in V.row(i), V.row(j) e V.row(k),
 *          rispettivamente. NOTA: per ogni triangolo, i 3 vertici sono da
 *          considerarsi indicati in senso antiorario.
 * @param VF Array di array di indici. Ogni elemento dell'array esterno
 *           corrisponde all'elenco di indici delle facce adiacenti al
 *           al vertice corrispondente. L'array esterno ha quindi V.rows() elementi.
 * @param VFi Array di array di indici. L'array esterno ha V.rows() elementi. VFi[i]
 *            ha lo stesso numero di elementi del corrispondente elemento in VF[i].
 *            VFi[i][j] corrisponde al corner di Vf[i][j] che incide sul vertice i.
 */
void vertex_face_adjacency(
    MatrixXd const &V,
    MatrixXi const &F,
    std::vector<std::vector<int>> &VF,
    std::vector<std::vector<int>> &VFi)
{
    VF.clear();
    VFi.clear();
    VF.resize(V.rows());
    VFi.resize(V.rows());

    for (int f = 0; f < F.rows(); ++f) {
        for (int p = 0; p < F.cols(); ++p) {
            int v = F(f, p);
            VF[v].push_back(f);
            VFi[v].push_back(p);
        }
    }
}

/**
 * @brief Calcola la lista di facce adiacenti ad ogni faccia.
 *
 * Una faccia e' adiacente a un'altra se queste condividono un lato, ovvero se
 * puntano agli stessi due vertici, ma in ordine inverso:
 * F.row(i) || F.row(j) <=> F(i,p) == F(j,q) &&
 *                          F(i,(p+1)%F.cols()) == F(j, (F.cols()+q-1)%F.cols())
 *
 * Questa funzione necessita dell'adiacenza vertice->facce, risultato della funzione
 * vertex_face_adjacency().
 *
 * @param V I vertici della mesh. Per ogni riga della matrice V, la posizione
 *          del vertice e' costituita dalle coordinate x,y,z memorizzate nelle
 *          3 colonne della riga.
 * @param F I triangoli della mesh. Per ogni riga della matrice F, il triangolo
 *          e' descritto dagli indici i,j,k memorizzati nelle 3 colonne della
 *          riga. Gli indici i,j,k si riferiscono ai 3 vertici A, B, C del
 *          triangolo, memorizzati in V.row(i), V.row(j) e V.row(k),
 *          rispettivamente. NOTA: per ogni triangolo, i 3 vertici sono da
 *          considerarsi indicati in senso antiorario.
 * @param VF Array di array di indici. Ogni elemento dell'array esterno
 *           corrisponde all'elenco di indici delle facce adiacenti al
 *           al vertice corrispondente. L'array esterno ha quindi V.rows() elementi.
 * @param VFi Array di array di indici. L'array esterno ha V.rows() elementi. VFi[i]
 *            ha lo stesso numero di elementi del corrispondente elemento in VF[i].
 *            VFi[i][j] corrisponde al corner di Vf[i][j] che incide sul vertice i.
 * @param FF Lista di indici di facce adiacenti per ogni faccia. Ogni riga corrisponde
 *           alla lista di adiacenza di una faccia. Ogni faccia ha F.cols() lati,
 *           per cui avra' F.cols() facce adiacenti (alcune potrebbero essere
 *           nulle in presenza di buchi - indice -1).
 * @param FFi Lista di indici per ogni faccia. Stessa dimensionalita' di FF. Ogni
 *            riga corrisponde a indici per una faccia, un indice per ogni faccia
 *            adiacente. Alla riga i, colonna j, l'indice corrisponde al lato della
 *            j-esima faccia adiacente alla i-esima, condiviso con il lato j-esimo
 *            della faccia i-esima.
 */
void face_face_adjacency(
    MatrixXd const &V,
    MatrixXi const &F,
    std::vector<std::vector<int>> const& VF,
    std::vector<std::vector<int>> const& VFi,
    MatrixXi &FF,
    MatrixXi &FFi)
{
    FF.resize(F.rows(), F.cols());
    FFi.resize(F.rows(), F.cols());
    FF.setConstant(-1);
    FFi.setConstant(-1);

    for (int f = 0; f < F.rows(); ++f) {
        for (int p = 0; p < F.cols(); ++p) {
            int v = F(f, p);
            std::vector<int> const& VF_adj = VF[v];
            std::vector<int> const& VFi_adj = VFi[v];

            int v_next = F(f, (p + 1) % F.cols());

            for (std::size_t i = 0; i < VF_adj.size(); ++i) {
                int f_adj = VF_adj[i];

                if (f_adj == f) {
                    continue;
                }

                int fi_adj = VFi_adj[i];
                int fi_prev = (F.cols() + fi_adj - 1) % F.cols();

                if (F(f_adj, fi_prev) == v_next) {
                    // trovato!
                    FF(f, p) = f_adj;
                    FFi(f, p) = fi_prev;
                }
            }
        }
    }
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2010-2011 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "common.h"
#include <Eigen/LU>

// computes an LU factorization of a general M-by-N matrix A using partial pivoting with row interchanges
EIGEN_LAPACK_FUNC(getrf,(int *m, int *n, RealScalar *pa, int *lda, int *ipiv, int *info))
{
  *info = 0;
        if(*m<0)                  *info = -1;
  else  if(*n<0)                  *info = -2;
  else  if(*lda<std::max(1,*m))   *info = -4;
  if(*info!=0)
  {
    int e = -*info;
    return xerbla_(SCALAR_SUFFIX_UP"GETRF", &e, 6);
  }

  if(*m==0 || *n==0)
    return 0;

  Scalar* a = reinterpret_cast<Scalar*>(pa);
  int nb_transpositions;
  int ret = int(Eigen::internal::partial_lu_impl<Scalar,ColMajor,int>
                     ::blocked_lu(*m, *n, a, *lda, ipiv, nb_transpositions));

  for(int i=0; i<std::min(*m,*n); ++i)
    ipiv[i]++;

  if(ret>=0)
    *info = ret+1;

  return 0;
}

//GETRS solves a system of linear equations
//    A * X = B  or  A' * X = B
//  with a general N-by-N matrix A using the LU factorization computed  by GETRF
EIGEN_LAPACK_FUNC(getrs,(char *trans, int *n, int *nrhs, RealScalar *pa, int *lda, int *ipiv, RealScalar *pb, int *ldb, int *info))
{
  *info = 0;
        if(OP(*trans)==INVALID)  *info = -1;
  else  if(*n<0)                 *info = -2;
  else  if(*nrhs<0)              *info = -3;
  else  if(*lda<std::max(1,*n))  *info = -5;
  else  if(*ldb<std::max(1,*n))  *info = -8;
  if(*info!=0)
  {
    int e = -*info;
    return xerbla_(SCALAR_SUFFIX_UP"GETRS", &e, 6);
  }

  Scalar* a = reinterpret_cast<Scalar*>(pa);
  Scalar* b = reinterpret_cast<Scalar*>(pb);
  MatrixType lu(a,*n,*n,*lda);
  MatrixType B(b,*n,*nrhs,*ldb);

  for(int i=0; i<*n; ++i)
    ipiv[i]--;
  if(OP(*trans)==NOTR)
  {
    B = PivotsType(ipiv,*n) * B;
    lu.triangularView<UnitLower>().solveInPlace(B);
    lu.triangularView<Upper>().solveInPlace(B);
  }
  else if(OP(*trans)==TR)
  {
    lu.triangularView<Upper>().transpose().solveInPlace(B);
    lu.triangularView<UnitLower>().transpose().solveInPlace(B);
    B = PivotsType(ipiv,*n).transpose() * B;
  }
  else if(OP(*trans)==ADJ)
  {
    lu.triangularView<Upper>().adjoint().solveInPlace(B);
    lu.triangularView<UnitLower>().adjoint().solveInPlace(B);
    B = PivotsType(ipiv,*n).transpose() * B;
  }
  for(int i=0; i<*n; ++i)
    ipiv[i]++;

  return 0;
}


#pragma once
#include <cmath>
#include <limits>
#include <algorithm>
#include <boost/unordered_map.hpp>
#include "types.hh"

using Bounds = std::pair<double, bool>;
static Bounds operator+ (Bounds a,Bounds b) {
  return Bounds(a.first + b.first, a.second && b.second);
}
static inline std::ostream& operator << (std::ostream& os, const Bounds& b) {
  os << "(" << b.first << ", " << b.second << ")";
  return os;
}

#include <eigen3/Eigen/Core>
//! @TODO configure include directory for eigen

struct Zone {
  using Variables = char;
  using Tuple = std::tuple<std::vector<Bounds>,Bounds>;
  Eigen::Matrix<Bounds, Eigen::Dynamic, Eigen::Dynamic> value;
  Bounds M;

  inline std::size_t getNumOfVar() const {
    return value.cols() - 1;
  }

  inline void cutVars (std::shared_ptr<Zone> &out,std::size_t from,std::size_t to) {
    out = std::make_shared<Zone>();
    out->value.resize(to - from + 2, to - from + 2);
    out->value.block(0,0,1,1) << Bounds(0,true);
    out->value.block(1, 1, to - from + 1, to - from + 1) = value.block(from + 1, from + 1, to - from + 1,to - from + 1);
    out->value.block(1, 0, to - from + 1, 1) = value.block(from + 1, 0, to - from + 1, 1);
    out->value.block(0, 1, 1, to - from + 1) = value.block(0, from + 1, 1, to - from + 1);
    out->M = M;
  }

  static Zone zero(int size) {
    static Zone zeroZone;
    zeroZone.value.resize(size, size);
    zeroZone.value.fill(Bounds(0, true));
    return zeroZone;
  }

  std::tuple<std::vector<Bounds>,Bounds> toTuple() const {
    // omit (0,0)
    return std::tuple<std::vector<Bounds>,Bounds>(std::vector<Bounds>(value.data() + 1, value.data() + value.size()),M);
  }

  //! @brief add the constraint x - y \le (c,s)
  void tighten(Variables x, Variables y, Bounds c) {
    x++;
    y++;
    value(x,y) = std::min(value(x, y), c);
    close1(x);
    close1(y);
  }

  void close1(Variables x) {
    for (int i = 0; i < value.rows(); i++) {
      for (int j = 0; j < value.cols(); j++) {
        value(i, j) = std::min(value(i, j), value(i, x) + value(x, j));
      }
    }
  }

  // The reset value is always (0, \le)
  void reset(Variables x) {
    // 0 is the special varibale here
    x++;
    value(0,x) = Bounds(0, true);
    value(x,0) = Bounds(0, true);
    value.col(x).tail(value.rows() - 1) = value.col(0).tail(value.rows() - 1);
    value.row(x).tail(value.cols() - 1) = value.row(0).tail(value.cols() - 1);
  }

  void elapse() {
    static const Bounds infinity = Bounds(std::numeric_limits<double>::infinity(), false);
    value.col(0).fill(infinity);
    for (int i = 0; i < value.row(0).size(); ++i) {
      value.row(0)[i].second = false;
    }
  }

  void canonize() {
    for (int k = 0; k < value.cols(); k++) {
      close1(k);
    }
  }

  bool isSatisfiable() {
    canonize();
    return (value + value.transpose()).minCoeff() >= Bounds(0.0,true);
  }

  void abstractize() {
    static const Bounds infinity = Bounds(std::numeric_limits<double>::infinity(), false);
    for (auto it = value.data(); it < value.data() + value.size(); it++) {
      if (*it >= M) {
        *it = infinity;
      }
    }
  }

  bool operator== (Zone z) const {
    z.value(0,0) = value(0,0);
    return value == z.value;
  }
};

struct ZoneAutomaton : public AbstractionAutomaton<Zone> {
  struct TAEdge {
    State source;
    State target;
    Alphabet c;
    std::vector<Alphabet> resetVars;
    std::vector<Constraint> guard;
  };

  boost::unordered_map<std::tuple<State, State, Alphabet>, TAEdge> edgeMap;
  boost::unordered_map<std::pair<TAState, typename Zone::Tuple>, RAState> zones_in_za;
  int numOfVariables;
};

static inline std::ostream& operator << (std::ostream& os, const Zone& z) {
  for (int i = 0; i < z.value.rows();i++) {
    for (int j = 0; j < z.value.cols();j++) {
      os << z.value(i,j);
    }
    os << "\n";
  }
  os << std::endl;
  return os;
}


#include "Vector.hpp"
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include "fhe/EncryptedArray.h"
#include <cmath>
#include <cassert>
#include <algorithm>
namespace MDL {
template<typename T>
double Vector<T>::L2() const {
    double norm = 0.0;

    for (auto e : *this) {
        auto de = static_cast<double>(e);
        norm += de * de;
    }
    return std::sqrt(norm);
}

template<typename T>
Vector<double> Vector<T>::reduce(double factor) const {
    Vector<double> vec(dimension());
    std::transform(this->begin(), this->end(), vec.begin(),
                   [&factor](const T &v) { return v / factor; });
    return vec;
}

template<>
Vector<double> Vector<NTL::ZZX>::reduce(double factor) const {
    Vector<double> vec(dimension());
    std::transform(this->begin(), this->end(), vec.begin(),
                   [&factor](const NTL::ZZX &v) {
                   return NTL::to_long(v[0]) / factor;
                   });
    return vec;
}

template<>
Vector<double> Vector<NTL::ZZ>::reduce(double factor) const {
    Vector<double> vec(dimension());
    std::transform(this->begin(), this->end(), vec.begin(),
                   [&factor](const NTL::ZZ &v) {
                   double d;
                   NTL::conv(d, v);
                   return d / factor;
                   });
    return vec;
}

template<>
double Vector<NTL::ZZX>::L2() const {
    NTL::ZZ summation(0);

    for (auto& e : *this) {
		if (e.rep.length() > 0) summation += e[0] * e[0];
    }
    return std::sqrt(std::exp(log(summation)));
}

template<>
double Vector<NTL::ZZ>::L2() const {
    NTL::ZZ summation(0);

    for (auto& e : *this) {
		summation += e * e;
    }
    return std::sqrt(std::exp(log(summation)));
}

template<typename T>
T Vector<T>::dot(const Vector<T>& oth) const {
    assert(dimension() == oth.dimension());
    T sum(0);

    for (size_t i = 0; i < dimension(); i++) {
        sum += this->at(i) * oth[i];
    }
    return sum;
}

template<typename T>
Vector<T> Vector<T>::subvector(long startIndex, long endIndex) const
{
    while (startIndex < 0) { startIndex += dimension(); }
    while (endIndex < 0) { endIndex += dimension(); }
    if (endIndex < startIndex || endIndex >= this->size()) {
        std::cerr << "Invalid subvector arguments" << std::endl;
        return *this;
    }
    Vector<T> sub(endIndex - startIndex + 1);
    auto start = this->begin();
    auto end = start;
    std::advance(start, startIndex);
    std::advance(end, endIndex + 1);
    std::copy(start, end, sub.begin());
    return sub;
}

template<typename T>
Vector<long> Vector<T>::div(long factor) const
{
    Vector<long> vec(dimension());
    std::transform(this->begin(), this->end(), vec.begin(), [&factor](T e) { return std::lround(e / factor); });
    return vec;
}

template<>
Vector<long> Vector<NTL::ZZX>::div(long factor) const = delete;

template<>
NTL::ZZX Vector<long>::encode(const EncryptedArray &ea) const
{
    assert(this->size() <= ea.size());
    NTL::ZZX encoded;
    if (this->size() < ea.size()) {
        auto tmp(*this);
        tmp.resize(ea.size());
        ea.encode(encoded, tmp);
    } else {
        ea.encode(encoded, *this);
    }
    return encoded;
}

template<typename T>
Vector<T>& Vector<T>::operator*=(const T& val)
{
    for (auto &ele : *this) {
        ele *= val;
    }
    return *this;
}

template<typename T>
Vector<T>& Vector<T>::operator-=(const Vector<T> &oth)
{
    auto dd = std::min(dimension(), oth.dimension());
    for (size_t d = 0; d < dd; d++) {
        this->at(d) -= oth[d];
    }
    return *this;
}

template<typename T>
Vector<T>& Vector<T>::operator+=(const Vector<T> &oth)
{
    auto dd = std::min(dimension(), oth.dimension());
    for (size_t d = 0; d < dd; d++) {
        this->at(d) += oth[d];
    }
    return *this;
}

template<>
void Vector<long>::random(const long &domain)
{
    for (size_t i = 0; i < size(); i++) {
        this->at(i) = NTL::RandomBnd(domain);
    }
}

template class Vector<long>;
template class Vector<double>;
template class Vector<NTL::ZZX>;
} // namespace MDL


#include "RBGL.hpp"
#include "Basic2DMatrix.hpp"
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <boost/graph/dag_shortest_paths.hpp>
#include <boost/graph/bellman_ford_shortest_paths.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>

extern "C"
{
    SEXP BGL_dijkstra_shortest_paths_D (SEXP num_verts_in,
                                        SEXP num_edges_in, SEXP R_edges_in,
                                        SEXP R_weights_in, SEXP init_ind)
    {
        using namespace boost;

        typedef graph_traits < Graph_dd >::edge_descriptor Edge;
        typedef graph_traits < Graph_dd >::vertex_descriptor Vertex;
        Graph_dd g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);

        int N = num_vertices(g);
        std::vector<Vertex> p(N);
        std::vector<double> d(N);

        dijkstra_shortest_paths(g, vertex((int)INTEGER(init_ind)[0], g),
                                predecessor_map(&p[0]).distance_map(&d[0]));

        SEXP dists, pens, ansList;
        PROTECT(dists = allocVector(REALSXP,N));
        PROTECT(pens = allocVector(INTSXP,N));
        graph_traits < Graph_dd >::vertex_iterator vi, vend;
        for (tie(vi, vend) = vertices(g); vi != vend; ++vi) {
            REAL(dists)[*vi] = d[*vi];
            INTEGER(pens)[*vi] = p[*vi];
        }
        PROTECT(ansList = allocVector(VECSXP,2));
        SET_VECTOR_ELT(ansList,0,dists);
        SET_VECTOR_ELT(ansList,1,pens);

        UNPROTECT(3);
        return(ansList);
    }

    SEXP BGL_johnson_all_pairs_shortest_paths_D(SEXP num_verts_in,
            SEXP num_edges_in, SEXP R_edges_in,
            SEXP R_weights_in)
    {
        using namespace boost;
        int nv = INTEGER(num_verts_in)[0];
        SEXP out;

        Graph_dd g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);

        Basic2DMatrix<double> D(nv, nv);

        johnson_all_pairs_shortest_paths(g, D);

        PROTECT(out = NEW_NUMERIC(nv*nv));
        int k = 0;
        for (int i = 0 ; i < nv ; i++)
            for (int j = 0; j < nv; j++ )
            {
                REAL(out)[k] = D[i][j];
                k++;
            }
        UNPROTECT(1);
        return out;
    }

    SEXP BGL_bellman_ford_shortest_paths(SEXP num_verts_in,
                                         SEXP num_edges_in, SEXP R_edges_in,
                                         SEXP R_weights_in, SEXP init_ind)
    {
        using namespace boost;

        typedef adjacency_list < vecS, vecS, directedS,
        no_property, property < edge_weight_t, double> > EdgeGraph;

        int i;
        int NE = (int)INTEGER(num_edges_in)[0];
        int N = (int)INTEGER(num_verts_in)[0];
        int s = (int)INTEGER(init_ind)[0];

        EdgeGraph g(N);

        int* edges_in = INTEGER(R_edges_in);
        for ( i = 0; i < NE; i++, edges_in += 2 )
            add_edge(*edges_in, *(edges_in+1), g);

        std::vector<std::size_t> p(N);
        for ( i = 0; i < N; i++ ) p[i] = i;

        std::vector<double> d(N, std::numeric_limits<double>::max());
        d[s] = 0;

        property_map<EdgeGraph, edge_weight_t>::type w = get(edge_weight, g);

        int* weight_i = (isReal(R_weights_in)) ? 0 : INTEGER(R_weights_in);
        double* weight_d = (isReal(R_weights_in)) ? REAL(R_weights_in) : 0;

        graph_traits< EdgeGraph >::edge_iterator ei, ei_end;
        for ( tie(ei, ei_end) = edges(g); ei != ei_end; ++ei )
            w[*ei] = weight_i ? (*weight_i++) : (*weight_d++);

        bool r = bellman_ford_shortest_paths(g, N,
                 weight_map(w).predecessor_map(&p[0]).distance_map(&d[0]));

        SEXP conn, dList, pList, ansList;
        PROTECT(ansList = allocVector(VECSXP,3));
        PROTECT(conn = NEW_LOGICAL(1));
        PROTECT(dList = allocVector(REALSXP,N));
        PROTECT(pList = allocVector(INTSXP,N));

        LOGICAL(conn)[0] = r;

        for (i = 0; i < N; i++)
        {
            INTEGER(pList)[i] = p[i];
            REAL(dList)[i] = d[i];
        }

        SET_VECTOR_ELT(ansList,0,conn);
        SET_VECTOR_ELT(ansList,1,dList);
        SET_VECTOR_ELT(ansList,2,pList);

        UNPROTECT(4);
        return(ansList);
    }

    SEXP BGL_dag_shortest_paths(SEXP num_verts_in,
                                SEXP num_edges_in, SEXP R_edges_in,
                                SEXP R_weights_in, SEXP init_ind)
    {
        using namespace boost;

        typedef graph_traits < Graph_dd >::edge_descriptor Edge;
        typedef graph_traits < Graph_dd >::vertex_descriptor Vertex;
        Graph_dd g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);

        int N = num_vertices(g);
        std::vector<Vertex> p(N);
        std::vector<double> d(N);

        dag_shortest_paths(g, vertex((int)INTEGER(init_ind)[0], g),
                           predecessor_map(&p[0]).distance_map(&d[0]));

        SEXP dists, pens, ansList;
        PROTECT(dists = allocVector(REALSXP,N));
        PROTECT(pens = allocVector(INTSXP,N));
        graph_traits < Graph_dd >::vertex_iterator vi, vend;
        for (tie(vi, vend) = vertices(g); vi != vend; ++vi) {
            if ( int(d[*vi]) == std::numeric_limits<int>::max() )
            {
                REAL(dists)[*vi] = R_NaN;
                INTEGER(pens)[*vi] = *vi;
            }
            else
            {
                REAL(dists)[*vi] = d[*vi];
                INTEGER(pens)[*vi] = p[*vi];
            }
        }
        PROTECT(ansList = allocVector(VECSXP,2));
        SET_VECTOR_ELT(ansList,0,dists);
        SET_VECTOR_ELT(ansList,1,pens);

        UNPROTECT(3);
        return(ansList);
    }

    SEXP BGL_floyd_warshall_all_pairs_shortest_paths_D(SEXP num_verts_in,
            SEXP num_edges_in, SEXP R_edges_in,
            SEXP R_weights_in)
    {
        using namespace boost;
        typedef adjacency_list<vecS, vecS, directedS, no_property,
        property< edge_weight_t, double, property< edge_weight2_t, double > > > Graph;
        int nv = INTEGER(num_verts_in)[0];
        SEXP out;
        typedef std::pair < int, int >Edge;

        Graph_dd g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);

        Basic2DMatrix<double> D(nv, nv);

        floyd_warshall_all_pairs_shortest_paths(g, D);

        PROTECT(out = NEW_NUMERIC(nv*nv));
        int k = 0;
        for (int i = 0 ; i < nv ; i++)
            for (int j = 0; j < nv; j++ )
            {
                REAL(out)[k] = D[i][j];
                k++;
            }
        UNPROTECT(1);
        return out;
    }
}


// std includes
#include <iostream> // cout, endl
#include <memory> // shared_ptr
#include <tuple>
#include <vector>
// thirdparties includes
#include <Eigen/Dense>
// lib includes
// // s0s
#include "s0s/runge_kutta_fehlberg.h"
#include "sl0/point.h"
// // sa0
#include "sa0/active.h"
#include "sa0/actuator/point.h"
// simple includes
#include "flow.h"

using TypeScalar = double;
template<typename ...Args>
using TypeContainer = std::vector<Args...>;
// Space
template<int StateSize>
using TypeState = Eigen::Matrix<TypeScalar, StateSize, 1>;
constexpr unsigned int DIM = 3;
using TypeVector = Eigen::Matrix<TypeScalar, DIM, 1>;
// Ref and View
template<typename ...Args>
using TypeRef = Eigen::Ref<Args...>;
template<typename ...Args>
using TypeView = Eigen::Map<Args...>;
// Point
using TypeStepPoint = sl0::StepPoint<TypeState, DIM, TypeRef, TypeView, Flow>;
// Choose passive
using TypeStepPassive = TypeStepPoint;
// Active
using TypeStepActuator = sl0::sa0::StepActuator<TypeStepPassive::TypeStateStatic, TypeRef, TypeStepPassive>;
using TypeStepPointSwim = sl0::sa0::StepPointSwim<TypeStepPassive::TypeStateStatic, TypeRef, TypeStepPassive, TypeVector>;
// Solver
using TypeSolver = s0s::SolverRungeKuttaFehlberg;

int main () {
    TypeVector us = TypeVector::Constant(0.0);
    us[0] = 1.0;
    TypeVector x0 = TypeVector::Constant(1.0);
    double t0 = 0.0;
    double dt = 1e-0;
    double tEnd = 1.0;
    unsigned int nt = std::round((tEnd - t0) / dt);
    // Create activePoint
    sl0::sa0::ObjectActive<TypeState, TypeRef, TypeView, TypeStepPassive, TypeStepActuator, TypeSolver> activePoint(TypeStepPassive(std::make_shared<Flow>()));
    std::shared_ptr<TypeStepPointSwim> sStepPointSwim = std::make_shared<TypeStepPointSwim>(us);
    activePoint.sStep->register_actuator(sStepPointSwim);
    // Set initial state
    activePoint.sStep->x(activePoint.state) = x0;
    activePoint.t = t0;
    // Computation
    for(std::size_t i = 0; i < nt; i++) {
        activePoint.update(dt);
    }
    // out
    std::cout << "\n";
    std::cout << "activePoint advected and swimming in an exponential flow, t = " << activePoint.t << "\n";
    std::cout << "\n";
    std::cout << "activePoint position : " << "\n" << activePoint.sStep->x(activePoint.state) << "\n";
    std::cout << std::endl;
}