//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#ifndef __spatial_explog_hpp__
#define __spatial_explog_hpp__

#include <Eigen/Geometry>

#include "pinocchio/fwd.hpp"
#include "pinocchio/utils/static-if.hpp"
#include "pinocchio/math/fwd.hpp"
#include "pinocchio/math/sincos.hpp"
#include "pinocchio/math/taylor-expansion.hpp"
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/skew.hpp"
#include "pinocchio/spatial/se3.hpp"

namespace pinocchio
{
  /// \brief Exp: so3 -> SO3.
  ///
  /// Return the integral of the input angular velocity during time 1.
  ///
  /// \param[in] v The angular velocity vector.
  ///
  /// \return The rotational matrix associated to the integration of the angular velocity during time 1.
  ///
  template<typename Vector3Like>
  typename Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
  exp3(const Eigen::MatrixBase<Vector3Like> & v)
  {
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, v, 3, 1);

    typedef typename Vector3Like::Scalar Scalar;
    typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like) Vector3LikePlain;
    typedef Eigen::Matrix<Scalar,3,3,Vector3LikePlain::Options> Matrix3;

    const Scalar t2 = v.squaredNorm();

    const Scalar t = math::sqrt(t2);
    if(t > TaylorSeriesExpansion<Scalar>::template precision<3>())
    {
      Scalar ct,st; SINCOS(t,&st,&ct);
      const Scalar alpha_vxvx = (1 - ct)/t2;
      const Scalar alpha_vx = (st)/t;
      Matrix3 res(alpha_vxvx * v * v.transpose());
      res.coeffRef(0,1) -= alpha_vx * v[2]; res.coeffRef(1,0) += alpha_vx * v[2];
      res.coeffRef(0,2) += alpha_vx * v[1]; res.coeffRef(2,0) -= alpha_vx * v[1];
      res.coeffRef(1,2) -= alpha_vx * v[0]; res.coeffRef(2,1) += alpha_vx * v[0];
      res.diagonal().array() += ct;

      return res;
    }
    else
    {
      const Scalar alpha_vxvx = Scalar(1)/Scalar(2) - t2/24;
      const Scalar alpha_vx = Scalar(1) - t2/6;
      Matrix3 res(alpha_vxvx * v * v.transpose());
      res.coeffRef(0,1) -= alpha_vx * v[2]; res.coeffRef(1,0) += alpha_vx * v[2];
      res.coeffRef(0,2) += alpha_vx * v[1]; res.coeffRef(2,0) -= alpha_vx * v[1];
      res.coeffRef(1,2) -= alpha_vx * v[0]; res.coeffRef(2,1) += alpha_vx * v[0];
      res.diagonal().array() += Scalar(1) - t2/2;

      return res;
    }
  }

  /// \brief Same as \ref log3
  ///
  /// \param[in] R the rotation matrix.
  /// \param[out] theta the angle value.
  ///
  /// \return The angular velocity vector associated to the rotation matrix.
  ///
  template<typename Matrix3Like>
  Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
  log3(const Eigen::MatrixBase<Matrix3Like> & R,
       typename Matrix3Like::Scalar & theta)
  {
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, R, 3, 3);

    typedef typename Matrix3Like::Scalar Scalar;
    typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3;

    static const Scalar PI_value = PI<Scalar>();

    Vector3 res;
    const Scalar tr = R.trace();
    if(tr > Scalar(3))       theta = 0; // acos((3-1)/2)
    else if(tr < Scalar(-1)) theta = PI_value; // acos((-1-1)/2)
    else                     theta = math::acos((tr - Scalar(1))/Scalar(2));
    assert(theta == theta && "theta contains some NaN"); // theta != NaN

    // From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small.
    if (theta < PI_value - 1e-2)
    {
      const Scalar t = ((theta > TaylorSeriesExpansion<Scalar>::template precision<3>())
                        ? theta / sin(theta)
                        : Scalar(1)) / Scalar(2);
      res(0) = t * (R (2, 1) - R (1, 2));
      res(1) = t * (R (0, 2) - R (2, 0));
      res(2) = t * (R (1, 0) - R (0, 1));
    }
    else
    {
      // 1e-2: A low value is not required since the computation is
      // using explicit formula. However, the precision of this method
      // is the square root of the precision with the antisymmetric
      // method (Nominal case).
      const Scalar cphi = cos(theta - PI_value);
      const Scalar beta  = theta*theta / ( Scalar(1) + cphi );
      Vector3 tmp((R.diagonal().array() + cphi) * beta);
      res(0) = (R (2, 1) > R (1, 2) ? Scalar(1) : Scalar(-1)) * (tmp[0] > Scalar(0) ? sqrt(tmp[0]) : Scalar(0));
      res(1) = (R (0, 2) > R (2, 0) ? Scalar(1) : Scalar(-1)) * (tmp[1] > Scalar(0) ? sqrt(tmp[1]) : Scalar(0));
      res(2) = (R (1, 0) > R (0, 1) ? Scalar(1) : Scalar(-1)) * (tmp[2] > Scalar(0) ? sqrt(tmp[2]) : Scalar(0));
    }

    return res;
  }

  /// \brief Log: SO3 -> so3.
  ///
  /// Pseudo-inverse of log from \f$ SO3 -> { v \in so3, ||v|| \le pi } \f$.
  ///
  /// \param[in] R The rotation matrix.
  ///
  /// \return The angular velocity vector associated to the rotation matrix.
  ///
  template<typename Matrix3Like>
  Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
  log3(const Eigen::MatrixBase<Matrix3Like> & R)
  {
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, R, 3, 3);

    typename Matrix3Like::Scalar theta;
    return log3(R.derived(),theta);
  }

  ///
  /// \brief Derivative of \f$ \exp{r} \f$
  /// \f[
  ///     \frac{\sin{||r||}}{||r||}                       I_3
  ///   - \frac{1-\cos{||r||}}{||r||^2}                   \left[ r \right]_x
  ///   + \frac{1}{||n||^2} (1-\frac{\sin{||r||}}{||r||}) r r^T
  /// \f]
  ///
  template<typename Vector3Like, typename Matrix3Like>
  void Jexp3(const Eigen::MatrixBase<Vector3Like> & r,
             const Eigen::MatrixBase<Matrix3Like> & Jexp)
  {
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, r   , 3, 1);
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, Jexp, 3, 3);

    Matrix3Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jexp);
    typedef typename Matrix3Like::Scalar Scalar;

    Scalar n2 = r.squaredNorm(),a,b,c;
    Scalar n = math::sqrt(n2);

    if (n < TaylorSeriesExpansion<Scalar>::template precision<3>())
    {
      a =   Scalar(1)           - n2/Scalar(6);
      b = - Scalar(1)/Scalar(2) - n2/Scalar(24);
      c =   Scalar(1)/Scalar(6) - n2/Scalar(120);
    }
    else
    {
      Scalar n_inv = Scalar(1)/n;
      Scalar n2_inv = n_inv * n_inv;
      Scalar cn,sn; SINCOS(n,&sn,&cn);

      a = sn*n_inv;
      b = - (1-cn)*n2_inv;
      c = n2_inv * (1 - a);
    }

    Jout.diagonal().setConstant(a);

    Jout(0,1) = -b*r[2]; Jout(1,0) = -Jout(0,1);
    Jout(0,2) =  b*r[1]; Jout(2,0) = -Jout(0,2);
    Jout(1,2) = -b*r[0]; Jout(2,1) = -Jout(1,2);

    Jout.noalias() += c * r * r.transpose();
  }

  template<typename Scalar, typename Vector3Like, typename Matrix3Like>
  void Jlog3(const Scalar & theta,
             const Eigen::MatrixBase<Vector3Like> & log,
             const Eigen::MatrixBase<Matrix3Like> & Jlog)
  {
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like,  log, 3, 1);
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, Jlog, 3, 3);

    Matrix3Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog);

    if (theta < TaylorSeriesExpansion<Scalar>::template precision<3>())
    {
      const Scalar alpha = Scalar(1)/Scalar(12) + theta*theta / Scalar(720);
      Jout.noalias() = alpha * log * log.transpose();

      Jout.diagonal().array() += Scalar(0.5) * (2 - theta*theta / Scalar(6));

      // Jlog += r_{\times}/2
      addSkew(0.5 * log, Jlog);
    }
    else
    {
      // Jlog = alpha I
      Scalar ct,st; SINCOS(theta,&st,&ct);
      const Scalar st_1mct = st/(Scalar(1)-ct);

      const Scalar alpha = Scalar(1)/(theta*theta) - st_1mct/(Scalar(2)*theta);
      Jout.noalias() = alpha * log * log.transpose();

      Jout.diagonal().array() += Scalar(0.5) * (theta*st_1mct);

      // Jlog += r_{\times}/2
      addSkew(0.5 * log, Jlog);
    }
  }

  template<typename Matrix3Like1, typename Matrix3Like2>
  void Jlog3(const Eigen::MatrixBase<Matrix3Like1> & R,
             const Eigen::MatrixBase<Matrix3Like2> & Jlog)
  {
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like1,    R, 3, 3);
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like2, Jlog, 3, 3);

    typedef typename Matrix3Like1::Scalar Scalar;
    typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like1)::Options> Vector3;

    Scalar t;
    Vector3 w(log3(R,t));
    Jlog3(t,w,PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like2,Jlog));
  }

  ///
  /// \brief Exp: se3 -> SE3.
  ///
  /// Return the integral of the input twist during time 1.
  ///
  /// \param[in] nu The input twist.
  ///
  /// \return The rigid transformation associated to the integration of the twist during time 1.
  ///
  template<typename MotionDerived>
  SE3Tpl<typename MotionDerived::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options>
  exp6(const MotionDense<MotionDerived> & nu)
  {
    typedef typename MotionDerived::Scalar Scalar;
    enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options };

    typedef SE3Tpl<Scalar,Options> SE3;

    SE3 res;
    typename SE3::LinearType & trans = res.translation();
    typename SE3::AngularType & rot = res.rotation();

    const typename MotionDerived::ConstAngularType & w = nu.angular();
    const typename MotionDerived::ConstLinearType & v = nu.linear();

    Scalar alpha_wxv, alpha_v, alpha_w, diagonal_term;
    const Scalar t2 = w.squaredNorm();
    const Scalar t = math::sqrt(t2);
    Scalar ct,st; SINCOS(t,&st,&ct);
    const Scalar inv_t2 = Scalar(1)/t2;

    alpha_wxv = internal::if_then_else(t<TaylorSeriesExpansion<Scalar>::template precision<3>(),
                                       Scalar(1)/Scalar(2) - t2/24,
                                       (Scalar(1) - ct)*inv_t2);

    alpha_v = internal::if_then_else(t<TaylorSeriesExpansion<Scalar>::template precision<3>(),
                                     Scalar(1) - t2/6,
                                     (st)/t);

    alpha_w = internal::if_then_else(t<TaylorSeriesExpansion<Scalar>::template precision<3>(),
                                     (Scalar(1)/Scalar(6) - t2/120),
                                     (Scalar(1) - alpha_v)*inv_t2);

    diagonal_term = internal::if_then_else(t<TaylorSeriesExpansion<Scalar>::template precision<3>(),
                                           Scalar(1) - t2/2,
                                           ct);

    // Linear
    trans.noalias() = (alpha_v*v + (alpha_w*w.dot(v))*w + alpha_wxv*w.cross(v));

    // Rotational
    rot.noalias() = alpha_wxv * w * w.transpose();
    rot.coeffRef(0,1) -= alpha_v * w[2]; rot.coeffRef(1,0) += alpha_v * w[2];
    rot.coeffRef(0,2) += alpha_v * w[1]; rot.coeffRef(2,0) -= alpha_v * w[1];
    rot.coeffRef(1,2) -= alpha_v * w[0]; rot.coeffRef(2,1) += alpha_v * w[0];
    rot.diagonal().array() += diagonal_term;

    return res;
  }

  /// \brief Exp: se3 -> SE3.
  ///
  /// Return the integral of the input spatial velocity during time 1.
  ///
  /// \param[in] v The twist represented by a vector.
  ///
  /// \return The rigid transformation associated to the integration of the twist vector during time 1..
  ///
  template<typename Vector6Like>
  SE3Tpl<typename Vector6Like::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options>
  exp6(const Eigen::MatrixBase<Vector6Like> & v)
  {
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector6Like, v, 6, 1);

    MotionRef<const Vector6Like> nu(v.derived());
    return exp6(nu);
  }

  /// \brief Log: SE3 -> se3.
  ///
  /// Pseudo-inverse of exp from SE3 -> { v,w \in se3, ||w|| < 2pi }.
  ///
  /// \param[in] M The rigid transformation.
  ///
  /// \return The twist associated to the rigid transformation during time 1.
  ///
  template <typename Scalar, int Options>
  MotionTpl<Scalar,Options>
  log6(const SE3Tpl<Scalar,Options> & M)
  {
    typedef SE3Tpl<Scalar,Options> SE3;
    typedef MotionTpl<Scalar,Options> Motion;
    typedef typename SE3::Vector3 Vector3;

    typename SE3::ConstAngularRef R = M.rotation();
    typename SE3::ConstLinearRef p = M.translation();

    Scalar t;
    Vector3 w(log3(R,t)); // t in [0,π]
    const Scalar t2 = t*t;
    Scalar alpha, beta;
    if (t < TaylorSeriesExpansion<Scalar>::template precision<3>())
    {
      alpha = Scalar(1) - t2/Scalar(12) - t2*t2/Scalar(720);
      beta = Scalar(1)/Scalar(12) + t2/Scalar(720);
    }
    else
    {
      Scalar st,ct; SINCOS(t,&st,&ct);
      alpha = t*st/(Scalar(2)*(Scalar(1)-ct));
      beta = Scalar(1)/t2 - st/(Scalar(2)*t*(Scalar(1)-ct));
    }

    return Motion(alpha * p - 0.5 * w.cross(p) + beta * w.dot(p) * w,
                  w);
  }

  /// \brief Log: SE3 -> se3.
  ///
  /// Pseudo-inverse of exp from SE3 -> { v,w \in se3, ||w|| < 2pi }.
  ///
  /// \param[in] R The rigid transformation represented as an homogenous matrix.
  ///
  /// \return The twist associated to the rigid transformation during time 1.
  ///
  template<typename Matrix4Like>
  MotionTpl<typename Matrix4Like::Scalar,Eigen::internal::traits<Matrix4Like>::Options>
  log6(const Eigen::MatrixBase<Matrix4Like> & M)
  {
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix4Like, M, 4, 4);

    SE3Tpl<typename Matrix4Like::Scalar,Eigen::internal::traits<Matrix4Like>::Options> m(M);
    return log6(m);
  }

  /// \brief Derivative of exp6
  /// Computed as the inverse of Jlog6
  template<typename MotionDerived, typename Matrix6Like>
  void Jexp6(const MotionDense<MotionDerived>     & nu,
             const Eigen::MatrixBase<Matrix6Like> & Jexp)
  {
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix6Like, Jexp, 6, 6);

    typedef typename MotionDerived::Scalar Scalar;
    typedef typename MotionDerived::Vector3 Vector3;
    typedef Eigen::Matrix<Scalar, 3, 3, Vector3::Options> Matrix3;
    Matrix6Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jexp);

    const typename MotionDerived::ConstLinearType  & v = nu.linear();
    const typename MotionDerived::ConstAngularType & w = nu.angular();
    const Scalar t2 = w.squaredNorm();
    const Scalar t = math::sqrt(t2);

    // Matrix3 J3;
    // Jexp3(w, J3);
    Jexp3(w, Jout.template bottomRightCorner<3,3>());
    Jout.template topLeftCorner<3,3>() = Jout.template bottomRightCorner<3,3>();

    Scalar beta, beta_dot_over_theta;
    if (t < TaylorSeriesExpansion<Scalar>::template precision<3>())
    {
      beta                = Scalar(1)/Scalar(12) + t2/Scalar(720);
      beta_dot_over_theta = Scalar(1)/Scalar(360);
    }
    else
    {
      const Scalar tinv = Scalar(1)/t,
                   t2inv = tinv*tinv;
      Scalar st,ct; SINCOS (t, &st, &ct);
      const Scalar inv_2_2ct = Scalar(1)/(Scalar(2)*(Scalar(1)-ct));

      beta = t2inv - st*tinv*inv_2_2ct;
      beta_dot_over_theta = -Scalar(2)*t2inv*t2inv +
        (Scalar(1) + st*tinv) * t2inv * inv_2_2ct;
    }

    Vector3 p (Jout.template topLeftCorner<3,3>().transpose() * v);
    Scalar wTp (w.dot (p));
    Matrix3 J (alphaSkew(.5, p) +
          (beta_dot_over_theta*wTp)                *w*w.transpose()
          - (t2*beta_dot_over_theta+Scalar(2)*beta)*p*w.transpose()
          + wTp * beta                             * Matrix3::Identity()
          + beta                                   *w*p.transpose());

    Jout.template topRightCorner<3,3>().noalias() =
      - Jout.template topLeftCorner<3,3>() * J;
    Jout.template bottomLeftCorner<3,3>().setZero();
  }

  /** \brief Derivative of log6
   *  \f[
   *  \left(\begin{array}{cc}
   *  \text{Jlog3}(R) & J * \text{Jlog3}(R) \\
   *            0     &     \text{Jlog3}(R) \\
   *  \end{array}\right)
   *  \f]
   *  where
   *  \f[
   *  \def\rot{R}
   *  \begin{eqnarray}
   *  J &=&
   *  \left.\frac{1}{2}[\mathbf{p}]_{\times} + \dot{\beta} (||r||) \frac{\rot^T\mathbf{p}}{||r||}\rot\rot^T
   *  - (||r||\dot{\beta} (||r||) + 2 \beta(||r||)) \mathbf{p}\rot^T\right.\\
   *  &&\left. + \rot^T\mathbf{p}\beta (||r||)I_3 + \beta (||r||)\rot\mathbf{p}^T\right.
   *  \end{eqnarray}
   *  \f]
   *  and
   *  \f[ \beta(x)=\left(\frac{1}{x^2} - \frac{\sin x}{2x(1-\cos x)}\right) \f]
   */
  template<typename Scalar, int Options, typename Matrix6Like>
  void Jlog6(const SE3Tpl<Scalar, Options> & M,
             const Eigen::MatrixBase<Matrix6Like> & Jlog)
  {
    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix6Like, Jlog, 6, 6);

    typedef SE3Tpl<Scalar,Options> SE3;
    typedef typename SE3::Vector3 Vector3;
    Matrix6Like & value = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jlog);

    typename SE3::ConstAngularRef R = M.rotation();
    typename SE3::ConstLinearRef p = M.translation();

    Scalar t;
    Vector3 w(log3(R,t));

    // value is decomposed as following:
    // value = [ A, B;
    //           C, D ]
    typedef Eigen::Block<Matrix6Like,3,3> Block33;
    Block33 A = value.template topLeftCorner<3,3>();
    Block33 B = value.template topRightCorner<3,3>();
    Block33 C = value.template bottomLeftCorner<3,3>();
    Block33 D = value.template bottomRightCorner<3,3>();

    Jlog3(t, w, A);
    D = A;

    const Scalar t2 = t*t;
    Scalar beta, beta_dot_over_theta;
    if(t < TaylorSeriesExpansion<Scalar>::template precision<3>())
    {
      beta                = Scalar(1)/Scalar(12) + t2/Scalar(720);
      beta_dot_over_theta = Scalar(1)/Scalar(360);
    }
    else
    {
      const Scalar tinv = Scalar(1)/t,
                   t2inv = tinv*tinv;
      Scalar st,ct; SINCOS (t, &st, &ct);
      const Scalar inv_2_2ct = Scalar(1)/(Scalar(2)*(Scalar(1)-ct));

      beta = t2inv - st*tinv*inv_2_2ct;
      beta_dot_over_theta = -Scalar(2)*t2inv*t2inv +
        (Scalar(1) + st*tinv) * t2inv * inv_2_2ct;
    }

    Scalar wTp = w.dot(p);

    Vector3 v3_tmp((beta_dot_over_theta*wTp)*w - (t2*beta_dot_over_theta+Scalar(2)*beta)*p);
    // C can be treated as a temporary variable
    C.noalias() = v3_tmp * w.transpose();
    C.noalias() += beta * w * p.transpose();
    C.diagonal().array() += wTp * beta;
    addSkew(.5*p,C);

    B.noalias() = C * A;
    C.setZero();
  }
} // namespace pinocchio

#include "pinocchio/spatial/explog-quaternion.hpp"

#endif //#ifndef __spatial_explog_hpp__


/*  Example code adapted from adept-1.0 advection example

    Copyright (C) 2012-2013 Robin Hogan and the University of Reading

    Contact email address: r.j.hogan@reading.ac.uk

    This file is part of the Adept library.

    This library is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/
#include <cmath>
#include <iostream>
#include <boost/format.hpp>
#include <vector>

#include <adept.h>

#define NX 100
using namespace adept;

// Lax-Wendroff scheme applied to linear advection
void
lax_wendroff(int nt, double c, const adouble q_init[NX], adouble q[NX])
{
    preallocate_statements((nt+1)*NX*3);
    preallocate_operations((nt+1)*NX*7);
    adouble flux[NX-1];                        // Fluxes between boxes
    for (int i=0; i<NX; i++) {
        q[i] = q_init[i]; // Initialize q
    }
    for (int j=0; j<nt; j++)
    {
        for (int i=0; i<NX-1; i++) {
            flux[i] = 0.5*c*(q[i]+q[i+1]+c*(q[i]-q[i+1]));
        }
        for (int i=1; i<NX-1; i++) {
            q[i] += flux[i-1]-flux[i];
        }
        q[0] = q[NX-2];
        q[NX-1] = q[1];          // Treat boundary conditions
    }
}

// Toon advection scheme applied to linear advection
void
toon(int nt, double c, const adouble q_init[NX], adouble q[NX])
{
    preallocate_statements((nt+1)*NX*3);
    preallocate_operations((nt+1)*NX*9);
    adouble flux[NX-1];                        // Fluxes between boxes
    for (int i=0; i<NX; i++) {
        q[i] = q_init[i]; // Initialize q
    }
    for (int j=0; j<nt; j++) {                 // Main loop in time
        for (int i=0; i<NX-1; i++) {
            flux[i] = (exp(c*log(q[i]/q[i+1]))-1.0)
                * q[i]*q[i+1] / (q[i]-q[i+1]);
        }
        for (int i=1; i<NX-1; i++) {
            q[i] += flux[i-1]-flux[i];
        }
        q[0] = q[NX-2]; q[NX-1] = q[1];          // Treat boundary
                                                 // conditions
    }
}

int
main(int argc, char** argv)
{
    double pi = 4.0*atan(1.0);
    double q_init[NX];

    int nt = 100;
    int nr = 500;
    double dt = 0.125;
    adept::Stack adept_stack;

    for (int i = 0; i < NX; i++) {
        q_init[i] = (0.5+0.5*sin((i*2.0*pi)/(NX-1.5)))+0.0001;
    }

    for (int j = 0; j < nr; j++)
    {
        adouble adept_q_init[NX];
        adouble adept_q[NX];

        adept::set_values(adept_q_init, NX, q_init);
        adept_stack.new_recording();
        toon(nt, dt, adept_q_init, adept_q);

        adept_stack.independent(adept_q_init, NX);
        adept_stack.dependent(adept_q, NX);

        double jacobian[NX*NX];
        adept_stack.jacobian(jacobian);

        if (j == 1) {
            for (int p = 0; p < 5; ++p) {
                std::cout << "  ";
                for (int q = 0; q < 5; ++q) {
                    std::cout << boost::format("%.16e") % jacobian[p + q*NX] << " ";
                }
                std::cout << std::endl;
            }
        }
    }

    return 0;
}


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXd ones = MatrixXd::Ones(3,3);
VectorXcd eivals = ones.eigenvalues();
cout << "The eigenvalues of the 3x3 matrix of ones are:" << endl << eivals << endl;

  return 0;
}


#include "tetrahedron.h"

#include <Eigen/Eigen>

Tetrahedron::Tetrahedron(const Eigen::MatrixXd& V, const Eigen::VectorXi& indices, const Eigen::VectorXi& faces, double shear, double bulk)
: FiniteElement(V, indices, faces, shear, bulk)
{
	// Compute inverse initial deformation
	Rotation dX;
	const auto root = _X.row(3);
	dX << _X.row(0) - root, _X.row(1) - root, _X.row(2) - root;
	_invdX = dX.inverse();

	// Compute volume
	_volume = dX.determinant() / 6;
	if (_volume < 0.0)
	{
		std::cout << "Negative volume, invalid configuration" << std::endl;
	}
}

Tetrahedron::~Tetrahedron()
{

}

double Tetrahedron::compute_U(const Eigen::MatrixXd& E) const
{
	const double trE = E.trace();
	const double trSqr = E.cwiseProduct(E).sum();
	return (_shear / 2 * trE * trE + _bulk * trSqr) * _volume;
}

Eigen::MatrixXd Tetrahedron::compute_F(const Eigen::MatrixXd& x) const
{
	Rotation du;
	const auto& root = x.row(3);
	du << x.row(0) - root, x.row(1) - root, x.row(2) - root;
	return du * _invdX;
}

Eigen::MatrixXd Tetrahedron::compute_E(const Eigen::MatrixXd& F) const
{
	return (F.transpose() * F - Rotation::Identity()) / 2;
}

Eigen::MatrixXd Tetrahedron::compute_dUdE(const Eigen::MatrixXd& E) const
{
	Eigen::MatrixXd I = Rotation::Identity();
	return (_shear * E.trace() * I + 2 * _bulk * E) * _volume;
}

Eigen::MatrixXd Tetrahedron::compute_dEdF(const Eigen::MatrixXd& F) const
{
	return  F.transpose();
}

Eigen::MatrixXd Tetrahedron::compute_dFdx(const Eigen::MatrixXd& x, int32_t i, int32_t j) const
{
	Rotation dF = Rotation::Zero();
	if (i < 3)
	{
		dF(i, j) = 1.0;
	}
	else
	{
		dF.col(j) = Vec3(-1.0, -1.0, -1.0);
	}
	return dF * _invdX;
}

Eigen::MatrixXd Tetrahedron::compute_ddFddx(const Eigen::MatrixXd& x) const
{
	return _invdX;
}

Eigen::MatrixXd Tetrahedron::compute_ddEddF(const Eigen::MatrixXd& F) const
{
	return Eigen::Matrix3d::Identity();
}

Tensor Tetrahedron::compute_ddUddE(const Eigen::MatrixXd& E) const
{
	Tensor out(E.rows(), E.cols(), Eigen::Matrix3d::Zero());

	// Edge/Corner entries
	for (int i = 0; i < E.rows(); i++)
	{
		for (int j = 0; j < E.cols(); j++)
		{
			out(i, j)(i, j) += 2 * _bulk * _volume;
		}
	}

	// Diagonal entries
	for (int i = 0; i < E.rows(); i++)
	{
		for (int j = 0; j < E.cols(); j++)
		{
			out(i, i)(j, j) += _shear * _volume;
		}
	}

	return out;
}


#include "smooth_alg.hpp"

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

#include <Eigen/QR>
#include <Eigen/LU>
#include <Eigen/SparseLU>

// -----------------------------------------------------------------------------

typedef Eigen::Triplet<double, int> Triplet;
/// declares a column-major sparse matrix type of double
typedef Eigen::SparseMatrix<double> Sparse_mat;

// -----------------------------------------------------------------------------

/// @return A sparse representation of the normalized Laplacian
/// list[ith_row][list of columns] = Triplet(ith_row, jth_column, matrix value)
static
std::vector<std::vector<Triplet>>
get_normalized_laplacian(const std::vector< Vec3 >& vertices,
                         const std::vector< std::vector<int> >& edges )
{
    unsigned nv = unsigned(vertices.size());
    std::vector<std::vector<Triplet>> mat_elemts(nv);
    for(int i = 0; i < nv; ++i)
        mat_elemts[i].reserve(10);

    for(int i = 0; i < nv; ++i)
    {
        const Vec3 c_pos = vertices[i];

        //get laplacian
        double sum = 0.;
        int nb_edges = edges[i].size();
        for(int e = 0; e < nb_edges; ++e)
        {
            int next_edge = (e + 1           ) % nb_edges;
            int prev_edge = (e + nb_edges - 1) % nb_edges;

            Vec3 v1 = c_pos                 - vertices[edges[i][prev_edge]];
            Vec3 v2 = vertices[edges[i][e]] - vertices[edges[i][prev_edge]];
            Vec3 v3 = c_pos                 - vertices[edges[i][next_edge]];
            Vec3 v4 = vertices[edges[i][e]] - vertices[edges[i][next_edge]];

            double cotan1 = (v1.dot(v2)) / (1e-6 + (v1.cross(v2)).norm() );
            double cotan2 = (v3.dot(v4)) / (1e-6 + (v3.cross(v4)).norm() );

            double w = (cotan1 + cotan2)*0.5;
            sum += w;
            mat_elemts[i].push_back( Triplet(i, edges[i][e], w) );
        }

        for( Triplet& t : mat_elemts[i] )
            t = Triplet( t.row(), t.col(), t.value() / sum);

        mat_elemts[i].push_back( Triplet(i, i, -1.0) );
    }
    return mat_elemts;
}

// -----------------------------------------------------------------------------

static
std::vector< std::vector<float> >
get_cotan_weights(const std::vector< Vec3 >& vertices,
                  const std::vector< std::vector<int> >& edges)
{
    unsigned nv = unsigned(vertices.size());
    std::vector< std::vector<float> > weights(nv);

    for(int i = 0; i < nv; ++i)
    {
        const Vec3 c_pos = vertices[i];
        double sum = 0.;
        int nb_edges = edges[i].size();
        weights[i].resize( nb_edges );
        for(int e = 0; e < nb_edges; ++e)
        {
            int next_edge = (e + 1           ) % nb_edges;
            int prev_edge = (e + nb_edges - 1) % nb_edges;

            Vec3 v1 = c_pos                 - vertices[edges[i][prev_edge]];
            Vec3 v2 = vertices[edges[i][e]] - vertices[edges[i][prev_edge]];
            Vec3 v3 = c_pos                 - vertices[edges[i][next_edge]];
            Vec3 v4 = vertices[edges[i][e]] - vertices[edges[i][next_edge]];

            double cotan1 = (v1.dot(v2)) / (1e-6 + (v1.cross(v2)).norm() );
            double cotan2 = (v3.dot(v4)) / (1e-6 + (v3.cross(v4)).norm() );

            double w = (cotan1 + cotan2)*0.5;
            weights[i][e] = w;
        }
    }
    return weights;
}

//------------------------------------------------------------------------------

std::vector< Vec3 >
smooth_iterative(const std::vector< Vec3 >& in_vertices,
                 const std::vector< std::vector<int> >& edges,
                 int nb_iter,
                 float alpha)
{
    unsigned nb_vertices = unsigned(in_vertices.size());
    std::vector< std::vector<float> > cotan_weights = get_cotan_weights(in_vertices, edges);

    std::vector< Vec3 > buffer_vertices( nb_vertices );
    std::vector< Vec3 > source = in_vertices;

    if(nb_iter == 0){
        return in_vertices;
    }

    Vec3* src_vertices = source.data();
    Vec3* dst_vertices = buffer_vertices.data();
    for(int k = 0; k < nb_iter; k++)
    {
        for( int i = 0; i < nb_vertices; i++)
        {
#if 0
            if( _topo->is_vert_on_side( i ) ){
                dst_vertices[i] = src_vertices[i];
                continue;
            }
#endif

            Vec3 cog(0.f);
            float sum = 0.f;
            size_t nb_neighs = edges[i].size();
            for(size_t n = 0; n < nb_neighs; n++)
            {
                Vert_idx neigh = edges[i][n];
                float w = cotan_weights[i][n];
                cog += src_vertices[neigh] * w;
                sum += w;
            }
            float t = alpha;
            dst_vertices[i] = (cog / sum) * t + src_vertices[i] * (1.f - t);

        }

        std::swap(dst_vertices, src_vertices);
    }

    return (nb_iter%2 == 1) ? buffer_vertices : source;
}

//------------------------------------------------------------------------------

//explicit
std::vector< Vec3 >
explicit_laplacian_smoothing(const std::vector< Vec3 >& in_vertices,
                             const std::vector< std::vector<int> >& edges,
                             int nb_iter,
                             float alpha)
{
    unsigned nb_vertices = unsigned(in_vertices.size());

    std::vector<Eigen::VectorXd> xyz;
    std::vector<Eigen::VectorXd> rhs;

    xyz.resize(3, Eigen::VectorXd::Zero(nb_vertices));
    rhs.resize(3, Eigen::VectorXd::Zero(nb_vertices));

    for(int i = 0; i < nb_vertices; ++i)
    {
        Vec3 pos = in_vertices[i];
        xyz[0][i] = pos.x;
        xyz[1][i] = pos.y;
        xyz[2][i] = pos.z;
    }

    // Build laplacian
    std::vector<std::vector<Triplet>> mat_elemts = get_normalized_laplacian(in_vertices, edges);
    Eigen::SparseMatrix<double> L(nb_vertices, nb_vertices);
    std::vector<Triplet> triplets;
    triplets.reserve(nb_vertices * 10);
    for( const std::vector<Triplet>& row : mat_elemts)
        for( const Triplet& elt : row )
            triplets.push_back( elt );

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

    Eigen::SparseMatrix<double> I = Eigen::MatrixXd::Identity(nb_vertices, nb_vertices).sparseView();
    L = I + L*alpha;

    //L = L*L*L*L*L*L*L*L*L*L;
    rhs = xyz;
    for(int n = 0; n < nb_iter; n++){
        for(int k = 0; k < 3; k++){
            xyz[k] = (L * xyz[k]);
        }
    }

    std::vector< Vec3 > out_verts(nb_vertices);
    for(int i = 0; i < nb_vertices; ++i){
        Vec3 v;
        v.x = xyz[0][i];
        v.y = xyz[1][i];
        v.z = xyz[2][i];
        out_verts[i] = v;
    }

    return out_verts;
}

//------------------------------------------------------------------------------

//implicit
std::vector< Vec3 >
implicit_laplacian_smoothing(const std::vector< Vec3 >& in_vertices,
                             const std::vector< std::vector<int> >& edges,
                             int nb_iter,
                             float alpha)
{
    unsigned nb_vertices = unsigned(in_vertices.size());

    std::vector<Eigen::VectorXd> xyz;
    std::vector<Eigen::VectorXd> rhs;

    xyz.resize(3, Eigen::VectorXd::Zero(nb_vertices));
    rhs.resize(3, Eigen::VectorXd::Zero(nb_vertices));

    for(int i = 0; i < nb_vertices; ++i)
    {
        Vec3 pos = in_vertices[i];
        rhs[0][i] = pos.x;
        rhs[1][i] = pos.y;
        rhs[2][i] = pos.z;
    }

    // Build laplacian
    std::vector<std::vector<Triplet>> mat_elemts = get_normalized_laplacian(in_vertices, edges);
    Eigen::SparseMatrix<double> L(nb_vertices, nb_vertices);
    std::vector<Triplet> triplets;
    triplets.reserve(nb_vertices * 10);
    for( const std::vector<Triplet>& row : mat_elemts)
        for( const Triplet& elt : row )
            triplets.push_back( elt );

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

    Eigen::SparseMatrix<double> I = Eigen::MatrixXd::Identity(nb_vertices, nb_vertices).sparseView();
    L = I - L*alpha;

    L = L*L*L;

    // Solve for x, y, z
    Eigen::SparseLU<Sparse_mat> solver;
    solver.compute( L );

    for(int k = 0; k < 3; k++){
        xyz[k] = solver.solve(rhs[k]);
    }

    std::vector< Vec3 > out_verts(nb_vertices);
    for(int i = 0; i < nb_vertices; ++i){
        Vec3 v;
        v.x = xyz[0][i];
        v.y = xyz[1][i];
        v.z = xyz[2][i];
        out_verts[i] = v;
    }

    return out_verts;
}


#include <exception>
#include <NTL/ZZ.h>
#include "RSA.h"

const char *KeyError::what() {
	return "Tried to decrypt using a public key.";
}

// Input: m, integer representation of plaintext
// key, the RSA public key
// Output: c, integer representation of ciphertext
void encrypt(NTL::ZZ& c, const NTL::ZZ& m, const RSAkey& key) {
	PowerMod(c, m, key.get_param("e"), key.get_param("n"));
}

// Input: c, integer representation of ciphertext
// key, the RSA private key
// Output: m, integer representation of plaintext
void decrypt(NTL::ZZ& m, const NTL::ZZ& c, const RSAkey& key) {
	if(key.is_private()) {
		PowerMod(m, c, key.get_param("d"), key.get_param("n"));
	} else {
		throw KeyError();
	}
}


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "MathExtensions.h"

#include <boost/math/special_functions/factorials.hpp>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_sf.h>

#include <stdexcept>

const double cppqedutils::PI(M_PI);
const double cppqedutils::SQRTPI(M_SQRTPI);
const double cppqedutils::EULER(M_E);

int cppqedutils::sign(double x) {return GSL_SIGN(x);}
int cppqedutils::fcmp(double x, double y, double eps) {return gsl_fcmp(x,y,eps);}

double cppqedutils::sqr(double x) {return gsl_pow_2(x);}

double cppqedutils::sqrAbs(dcomp x) {return sqr(real(x))+sqr(imag(x));} // saves the sqrt

double cppqedutils::fact(unsigned n)
{
  if (n>GSL_SF_FACT_NMAX) throw std::out_of_range("Factorial of"+std::to_string(n));
  return gsl_sf_fact(n);
}

double cppqedutils::choose(unsigned n, unsigned m)
{
  return gsl_sf_choose(n,m);
}

dcomp cppqedutils::coherentElement(unsigned long n, dcomp alpha)
{
  using namespace boost::math;
  return n ? n<max_factorial<double>::value ? pow(alpha,n)/sqrt(factorial<double>(n))
                                            : pow(2*n*PI,-.25)*pow(alpha/sqrt(n/EULER),n)
           : 1.;
}


/**
 * @file activation_functions_test.cpp
 * @author Marcus Edel
 * @author Dhawal Arora
 *
 * Tests for the various activation functions.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/activation_functions/logistic_function.hpp>
#include <mlpack/methods/ann/activation_functions/identity_function.hpp>
#include <mlpack/methods/ann/activation_functions/softsign_function.hpp>
#include <mlpack/methods/ann/activation_functions/tanh_function.hpp>
#include <mlpack/methods/ann/activation_functions/rectifier_function.hpp>

#include <mlpack/methods/ann/ffn.hpp>
#include <mlpack/methods/ann/init_rules/random_init.hpp>
#include <mlpack/methods/ann/performance_functions/mse_function.hpp>

#include <mlpack/methods/ann/layer/bias_layer.hpp>
#include <mlpack/methods/ann/layer/linear_layer.hpp>
#include <mlpack/methods/ann/layer/base_layer.hpp>
#include <mlpack/methods/ann/layer/binary_classification_layer.hpp>
#include <mlpack/methods/ann/layer/leaky_relu_layer.hpp>
#include <mlpack/methods/ann/layer/hard_tanh_layer.hpp>

#include <boost/test/unit_test.hpp>
#include "old_boost_test_definitions.hpp"

using namespace mlpack;
using namespace mlpack::ann;

BOOST_AUTO_TEST_SUITE(ActivationFunctionsTest);

// Be careful!  When writing new tests, always get the boolean value and store
// it in a temporary, because the Boost unit test macros do weird things and
// will cause bizarre problems.

// Generate dataset for activation function tests.
const arma::colvec activationData("-2 3.2 4.5 -100.2 1 -1 2 0");

/*
 * Implementation of the activation function test.
 *
 * @param input Input data used for evaluating the activation function.
 * @param target Target data used to evaluate the activation.
 *
 * @tparam ActivationFunction Activation function used for the check.
 */
template<class ActivationFunction>
void CheckActivationCorrect(const arma::colvec input, const arma::colvec target)
{
  // Test the activation function using a single value as input.
  for (size_t i = 0; i < target.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(ActivationFunction::fn(input.at(i)),
        target.at(i), 1e-3);
  }

  // Test the activation function using the entire vector as input.
  arma::colvec activations;
  ActivationFunction::fn(input, activations);
  for (size_t i = 0; i < activations.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(activations.at(i), target.at(i), 1e-3);
  }
}

/*
 * Implementation of the activation function derivative test.
 *
 * @param input Input data used for evaluating the activation function.
 * @param target Target data used to evaluate the activation.
 *
 * @tparam ActivationFunction Activation function used for the check.
 */
template<class ActivationFunction>
void CheckDerivativeCorrect(const arma::colvec input, const arma::colvec target)
{
  // Test the calculation of the derivatives using a single value as input.
  for (size_t i = 0; i < target.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(ActivationFunction::deriv(input.at(i)),
        target.at(i), 1e-3);
  }

  // Test the calculation of the derivatives using the entire vector as input.
  arma::colvec derivatives;
  ActivationFunction::deriv(input, derivatives);
  for (size_t i = 0; i < derivatives.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(derivatives.at(i), target.at(i), 1e-3);
  }
}

/*
 * Implementation of the activation function inverse test.
 *
 * @param input Input data used for evaluating the activation function.
 * @param target Target data used to evaluate the activation.
 *
 * @tparam ActivationFunction Activation function used for the check.
 */
template<class ActivationFunction>
void CheckInverseCorrect(const arma::colvec input)
{
    // Test the calculation of the inverse using a single value as input.
  for (size_t i = 0; i < input.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(ActivationFunction::inv(ActivationFunction::fn(
        input.at(i))), input.at(i), 1e-3);
  }

  // Test the calculation of the inverse using the entire vector as input.
  arma::colvec activations;
  ActivationFunction::fn(input, activations);
  ActivationFunction::inv(activations, activations);

  for (size_t i = 0; i < input.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(activations.at(i), input.at(i), 1e-3);
  }
}

/*
 * Implementation of the HardTanH activation function test. The function is
 * implemented as a HardTanH Layer in hard_tanh_layer.hpp
 *
 * @param input Input data used for evaluating the HardTanH activation function.
 * @param target Target data used to evaluate the HardTanH activation.
 */
void CheckHardTanHActivationCorrect(const arma::colvec input,
                                    const arma::colvec target)
{
  HardTanHLayer<> htf;

  // Test the activation function using the entire vector as input.
  arma::colvec activations;
  htf.Forward(input, activations);
  for (size_t i = 0; i < activations.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(activations.at(i), target.at(i), 1e-3);
  }
}

/*
 * Implementation of the HardTanH activation function derivative test. The
 * derivative is implemented as HardTanH Layer in hard_tanh_layer.hpp
 *
 * @param input Input data used for evaluating the HardTanH activation function.
 * @param target Target data used to evaluate the HardTanH activation.
 */
void CheckHardTanHDerivativeCorrect(const arma::colvec input,
                                    const arma::colvec target)
{
  HardTanHLayer<> htf;

  // Test the calculation of the derivatives using the entire vector as input.
  arma::colvec derivatives;

  // This error vector will be set to 1 to get the derivatives.
  arma::colvec error(input.n_elem);
  htf.Backward(input, (arma::colvec)error.ones(), derivatives);
  for (size_t i = 0; i < derivatives.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(derivatives.at(i), target.at(i), 1e-3);
  }
}

/*
 * Implementation of the LeakyReLU activation function test. The function is
 * implemented as LeakyReLU layer in the file leaky_relu_layer.hpp
 *
 * @param input Input data used for evaluating the LeakyReLU activation function.
 * @param target Target data used to evaluate the LeakyReLU activation.
 */
void CheckLeakyReLUActivationCorrect(const arma::colvec input,
                                     const arma::colvec target)
{
  LeakyReLULayer<> lrf;

  // Test the activation function using the entire vector as input.
  arma::colvec activations;
  lrf.Forward(input, activations);
  for (size_t i = 0; i < activations.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(activations.at(i), target.at(i), 1e-3);
  }
}

/*
 * Implementation of the LeakyReLU activation function derivative test.
 * The derivative function is implemented as LeakyReLU layer in the file
 * leaky_relu_layer.hpp
 *
 * @param input Input data used for evaluating the LeakyReLU activation function.
 * @param target Target data used to evaluate the LeakyReLU activation.
 */

void CheckLeakyReLUDerivativeCorrect(const arma::colvec input,
                                     const arma::colvec target)
{
  LeakyReLULayer<> lrf;

  // Test the calculation of the derivatives using the entire vector as input.
  arma::colvec derivatives;

  // This error vector will be set to 1 to get the derivatives.
  arma::colvec error(input.n_elem);
  lrf.Backward(input, (arma::colvec)error.ones(), derivatives);
  for (size_t i = 0; i < derivatives.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(derivatives.at(i), target.at(i), 1e-3);
  }
}

/**
 * Basic test of the tanh function.
 */
BOOST_AUTO_TEST_CASE(TanhFunctionTest)
{
  const arma::colvec desiredActivations("-0.96402758 0.9966824 0.99975321 -1 \
                                         0.76159416 -0.76159416 0.96402758 0");

  const arma::colvec desiredDerivatives("0.07065082 0.00662419 0.00049352 0 \
                                         0.41997434 0.41997434 0.07065082 1");

  CheckActivationCorrect<TanhFunction>(activationData, desiredActivations);
  CheckDerivativeCorrect<TanhFunction>(desiredActivations, desiredDerivatives);
  CheckInverseCorrect<TanhFunction>(desiredActivations);
}

/**
 * Basic test of the logistic function.
 */
BOOST_AUTO_TEST_CASE(LogisticFunctionTest)
{
  const arma::colvec desiredActivations("1.19202922e-01 9.60834277e-01 \
                                         9.89013057e-01 3.04574e-44 \
                                         7.31058579e-01 2.68941421e-01 \
                                         8.80797078e-01 0.5");

  const arma::colvec desiredDerivatives("0.10499359 0.03763177 0.01086623 \
                                         3.04574e-44 0.19661193 0.19661193 \
                                         0.10499359 0.25");

  CheckActivationCorrect<LogisticFunction>(activationData, desiredActivations);
  CheckDerivativeCorrect<LogisticFunction>(desiredActivations,
      desiredDerivatives);
  CheckInverseCorrect<LogisticFunction>(activationData);
}

/**
 * Basic test of the softsign function.
 */
BOOST_AUTO_TEST_CASE(SoftsignFunctionTest)
{
  const arma::colvec desiredActivations("-0.66666667 0.76190476 0.81818182 \
                                         -0.99011858 0.5 -0.5 0.66666667 0");

  const arma::colvec desiredDerivatives("0.11111111 0.05668934 0.03305785 \
                                         9.7642e-05 0.25 0.25 0.11111111 1");

  CheckActivationCorrect<SoftsignFunction>(activationData, desiredActivations);
  CheckDerivativeCorrect<SoftsignFunction>(desiredActivations,
      desiredDerivatives);
  CheckInverseCorrect<SoftsignFunction>(desiredActivations);
}

/**
 * Basic test of the identity function.
 */
BOOST_AUTO_TEST_CASE(IdentityFunctionTest)
{
  const arma::colvec desiredDerivatives = arma::ones<arma::colvec>(
      activationData.n_elem);

  CheckActivationCorrect<IdentityFunction>(activationData, activationData);
  CheckDerivativeCorrect<IdentityFunction>(activationData, desiredDerivatives);
}

/**
 * Basic test of the rectifier function.
 */
BOOST_AUTO_TEST_CASE(RectifierFunctionTest)
{
  const arma::colvec desiredActivations("0 3.2 4.5 0 1 0 2 0");

  const arma::colvec desiredDerivatives("0 1 1 0 1 0 1 0");

  CheckActivationCorrect<RectifierFunction>(activationData, desiredActivations);
  CheckDerivativeCorrect<RectifierFunction>(desiredActivations,
      desiredDerivatives);
}

/**
 * Basic test of the LeakyReLU function.
 */
BOOST_AUTO_TEST_CASE(LeakyReLUFunctionTest)
{
  const arma::colvec desiredActivations("-0.06 3.2 4.5 -3.006 \
                                         1 -0.03 2 0");

  const arma::colvec desiredDerivatives("0.03 1 1 0.03 \
                                         1 0.03 1 1");

  CheckLeakyReLUActivationCorrect(activationData, desiredActivations);
  CheckLeakyReLUDerivativeCorrect(desiredActivations, desiredDerivatives);
}

/**
 * Basic test of the HardTanH function.
 */
BOOST_AUTO_TEST_CASE(HardTanHFunctionTest)
{
  const arma::colvec desiredActivations("-1 1 1 -1 \
                                         1 -1 1 0");

  const arma::colvec desiredDerivatives("0 0 0 0 \
                                         1 1 0 1");

  CheckHardTanHActivationCorrect(activationData, desiredActivations);
  CheckHardTanHDerivativeCorrect(activationData, desiredDerivatives);
}

BOOST_AUTO_TEST_SUITE_END();


/*
 * Using Erasthostenes' sieve, print the first 2,000,000 numbers
 * darkturo 2014
 */
#include <iostream>
#include <fstream>
#include <bitset>
#include <ctime>
#include <limits>
#include <boost/spirit/include/karma.hpp>

#define TIMEDIFF(start, stop) 1000.0 * (stop - start)/CLOCKS_PER_SEC
#define MAXNUMBER 32452843
#define BUFFER_SIZE 8l * MAXNUMBER

class ErasthostenesSieve
{
   std::bitset<MAXNUMBER> listOfNaturals;
   std::ofstream output;
   int counter;
   char * buffer;
   char * p_input;

   public:
   ErasthostenesSieve() :
           output("primesEveryWhere.txt", std::ios::out | std::ios::trunc),
           counter(0)
   {
      listOfNaturals.set();
      listOfNaturals.set(0, false); // 1 is not prime

      buffer = new char[BUFFER_SIZE];
      p_input = buffer;
   }

   ~ErasthostenesSieve()
   {
      if (p_input != buffer)
         output.write(buffer, p_input - buffer);  // flush
      output.close();
   }

   int applyTheSieve()
   {
      int base = 2;
      print( base );

      for (base = 3; base * base < MAXNUMBER; base += 2 )
      {
         if (not listOfNaturals[base - 1])
            continue;

         print( base );

         for (int pivot = base + base ; pivot <= MAXNUMBER; pivot += base)
         {
            listOfNaturals.set(pivot - 1, false);
         }
      }

      for (; base <= MAXNUMBER; base += 2)
      {
         if (listOfNaturals[base - 1])
            print( base );
      }
      return counter;
   }

   private:
   inline void print(int number)
   {
      doPrint(number);
      counter ++;
   }

   inline void doPrint(int number)
   {
      boost::spirit::karma::generate(p_input, boost::spirit::int_, number);
#if BOOST_VERSION > 103600
      *p_input++ = '\n';
#else
      p_input += strlen(p_input);
      *p_input++ = '\n';
#endif
   }
};

int main(int argc, char ** argv)
{
   clock_t t1, t2;

   t1 = std::clock();

   ErasthostenesSieve siever;
   int printedPrimes = siever.applyTheSieve();

   t2 = std::clock();

   // Summary
   std::cout.precision(std::numeric_limits<double>::digits10);
   std::cerr << "Used " << std::fixed << TIMEDIFF(t1, t2)
             << " msecs to calculate " << printedPrimes
             << " primes." << std::endl;
}


/* test_derivatives.cpp - Test derivatives of mathematical functions

    Copyright (C) 2017 European Centre for Medium-Range Weather Forecasts

    Author: Robin Hogan <r.j.hogan@ecmwf.int>

  Copying and distribution of this file, with or without modification,
  are permitted in any medium without royalty provided the copyright
  notice and this notice are preserved.  This file is offered as-is,
  without any warranty.

*/

#include <adept_arrays.h>

#define TEST_UNARY_FUNC(FUNC)					\
  {								\
    std::cout << "  Checking " << #FUNC << "... \t";		\
    aVector x = x_save;					\
    stack.new_recording();					\
    aVector y = FUNC(x);					\
    Vector dy_dx_num  = (FUNC(x_save+dx)-FUNC(x_save)) / dx;	\
    Vector dy_dx_adept(N);					\
    for (int i = 0; i < N; ++i) {				\
      x[i].set_gradient(1.0);					\
      stack.forward();						\
      y[i].get_gradient(dy_dx_adept[i]);			\
    }								\
    Real max_err						\
      = maxval(abs(dy_dx_adept-dy_dx_num));			\
    Real max_frac_err						\
      = maxval(abs(dy_dx_adept-dy_dx_num)/dy_dx_adept);		\
    if (max_err == 0) {						\
      std::cout << "max error = 0: PASSED\n";			\
    }								\
    if (max_frac_err <= MAX_FRAC_ERR) {				\
      std::cout << "max fractional error = " << max_frac_err	\
		<< ": PASSED\n";				\
    }								\
    else {							\
      std::cout << "max fractional error = "			\
		<< max_frac_err << ": FAILED\n";		\
      std::cout << "    Adept     dy/dx = "			\
		<< dy_dx_adept << "\n";				\
      std::cout << "    Numerical dy/dx = " << dy_dx_num << "\n";	\
      error_too_large = true;					\
    }								\
  }

#define TEST_BINARY_FUNC(FUNC)					\
  {								\
    std::cout << "  Checking " << #FUNC << "... \t";		\
    aVector x = x_save;					\
    aVector y = y_save;						\
    stack.new_recording();					\
    aVector z = FUNC(x,y);					\
    Vector dz_dx_num						\
      = (FUNC(x_save+dx,y_save)-FUNC(x_save,y_save)) / dx; \
    Vector dz_dy_num						\
      = (FUNC(x_save,y_save+dy)-FUNC(x_save,y_save)) / dy;	\
    Vector dz_dx_adept(N);					\
    Vector dz_dy_adept(N);					\
    for (int i = 0; i < N; ++i) {				\
      z[i].set_gradient(1.0);					\
      stack.reverse();						\
      x[i].get_gradient(dz_dx_adept[i]);			\
      y[i].get_gradient(dz_dy_adept[i]);			\
    }								\
    Real max_err						\
      = std::max(maxval(abs(dz_dx_adept-dz_dx_num)),		\
		 maxval(abs(dz_dy_adept-dz_dy_num)));		\
    Real max_frac_err						\
      = std::max(maxval(abs(dz_dx_adept-dz_dx_num)/dz_dx_adept),	\
		 maxval(abs(dz_dy_adept-dz_dy_num)/dz_dy_adept));	\
    if (max_err == 0) {						\
      std::cout << "max error = 0: PASSED\n";			\
    }								\
    if (max_frac_err <= MAX_FRAC_ERR) {				\
      std::cout << "max fractional error = " << max_frac_err	\
		<< ": PASSED\n";				\
    }								\
    else {							\
      std::cout << "max fractional error = "			\
		<< max_frac_err << ": FAILED\n";		\
      std::cout << "    Adept     dz/dx = " << dz_dx_adept << "\n";	\
      std::cout << "    Adept     dz/dy = " << dz_dy_adept << "\n";	\
      std::cout << "    Numerical dz/dx = " << dz_dx_num << "\n";	\
      std::cout << "    Numerical dz/dy = " << dz_dy_num << "\n";	\
      error_too_large = true;					\
    }								\
  }

int
main(int argc, const char** argv) {
  using namespace adept;

  Stack stack;

  static const int N             = 12;
  static const Real MAX_FRAC_ERR = 1.0e-5;

  Vector x_save(N);
  x_save = 0.2;
  x_save << 0.01, 0.4, 0.99;

  Vector y_save(N);
  y_save = 0.7;
  y_save << 0.9, 0.6, 0.1;

  Real dx = 1.0e-8;

  if (sizeof(Real) < 8) {
    // Single precision only works with larger perturbations
    dx = 1.0e-5;
  }

  Real dy = dx;

  bool error_too_large = false;

  std::cout << "EVALUATING UNARY FUNCTIONS\n";
  std::cout << "For functions of the form y=FUNC(x), where x=" << x_save << ",\n";
  std::cout << "checking that fractional difference between dy/dx computed using Adept\n";
  std::cout << "and numerically by perturbing x by " << dx << " is less than " << MAX_FRAC_ERR << ".\n";

  TEST_UNARY_FUNC(-); // Unary minus
  TEST_UNARY_FUNC(+); // Unary plus
  TEST_UNARY_FUNC(log);
  TEST_UNARY_FUNC(log10);
  TEST_UNARY_FUNC(sin);
  TEST_UNARY_FUNC(cos);
  TEST_UNARY_FUNC(tan);
  TEST_UNARY_FUNC(asin);
  TEST_UNARY_FUNC(acos);
  TEST_UNARY_FUNC(atan);
  TEST_UNARY_FUNC(sinh);
  TEST_UNARY_FUNC(cosh);
  TEST_UNARY_FUNC(tanh);
  TEST_UNARY_FUNC(abs);
  TEST_UNARY_FUNC(fabs);
  TEST_UNARY_FUNC(exp);
  TEST_UNARY_FUNC(sqrt);
  TEST_UNARY_FUNC(ceil);
  TEST_UNARY_FUNC(floor);
  TEST_UNARY_FUNC(log2);
  TEST_UNARY_FUNC(expm1);
  TEST_UNARY_FUNC(exp2);
  TEST_UNARY_FUNC(log1p);
  TEST_UNARY_FUNC(asinh);
  TEST_UNARY_FUNC(acosh);
  TEST_UNARY_FUNC(atanh);
  TEST_UNARY_FUNC(erf);
  TEST_UNARY_FUNC(erfc);
  TEST_UNARY_FUNC(cbrt);
  TEST_UNARY_FUNC(round);
  TEST_UNARY_FUNC(trunc);
  TEST_UNARY_FUNC(rint);
  TEST_UNARY_FUNC(nearbyint);

  std::cout << "EVALUATING BINARY FUNCTIONS\n";
  std::cout << "For functions of the form z=FUNC(x,y), where x=" << x_save << ",\n";
  std::cout << "and y=" << y_save << ", checking that fractional difference between\n";
  std::cout << "dz/dx and dz/dy computed using Adept and numerically by perturbing\n";
  std::cout << "x and y by " << dx << " is less than " << MAX_FRAC_ERR << ".\n";

  TEST_BINARY_FUNC(pow);
  TEST_BINARY_FUNC(atan2);
  TEST_BINARY_FUNC(max);
  TEST_BINARY_FUNC(min);
  TEST_BINARY_FUNC(fmax);
  TEST_BINARY_FUNC(fmin);

  if (error_too_large) {
    std::cerr << "*** Error: fractional error in the derivatives of some functions too large\n";

    if (sizeof(Real) < 8) {
      std::cerr << "*** (but you are using less than double precision so it is not surprising)\n";
    }

    return 1;
  }
  else {
    return 0;
  }
}


//
//  Eigenvalues of the radial part of a 3D exponential well
//
// compile with:
//     g++ -std=c++11 exp_well_3d.cpp -o exp_well_3d -I$HOME/miniconda3/include/ -O3
//
// using the a Miniconda install of boost for the headers

#include <cstdio>
#include <iostream>
#include <cmath>
#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>

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

typedef boost::array< long double , 2 > vec_type;
typedef runge_kutta_cash_karp54< vec_type > error_stepper_type;
typedef controlled_runge_kutta< error_stepper_type > controlled_stepper_type;
typedef runge_kutta4< vec_type > stepper;

struct ode{

    long double m;
    long double energy;
    long double a;
    long double k;
    long double l;     // angluar quantum number

    void operator()( const vec_type &vec , vec_type &dvecdx , double r)
    {
        dvecdx[0] = vec[1];
        dvecdx[1] =  -2.0*vec[1]/r - (2*m*(energy - k*(exp(a*r) - 1.0)) - (l*(l+1)/(r*r)))*vec[0];
    }
};

struct nothing{

    void operator()( const vec_type &vec , const double r){

    }
};

void print_exp_well_psi_final(double k, double a, double m){
    controlled_stepper_type controlled_stepper;

    // Energy sweep range
    double min_energy = 0.000001;
    double max_energy = 10 * 0.00094;    // kBT = 0.00094 Ha

    int l_max = 50;                       // Maximum value of the l qunatum number

    double r_init = 0.001;               // Where to start the propogation from
    double r_final = 1.7;                // Where to end   '   '  as there is a divergence at r=0

    int n_steps = 20000;

    for (int l = 0; l <= l_max; l++ ){

        double prev_psi_final = 0.0;         // Final value of Psi i.e. Psi(r_final) for each eigenvalue

        for (int i = 0; i < n_steps; i++){

            double energy = min_energy + ((max_energy - min_energy) * i) / n_steps;

            vec_type vec = { 1.0 , 1E-20 }; // initial conditions
            integrate_adaptive( make_controlled< error_stepper_type >( 1.0e-10 , 1.0e-10 ),
                               ode {m , energy, a, k, static_cast<long double>(l)},
                               vec,
                               r_init,
                               r_final,
                               1E-10,
                               nothing {}
                               );

            // If the product of the previous Psi(r_final) and the current one is negative
            // there must be a zero between them, i.e. a normalaisable WF
            if (prev_psi_final * vec[0] < 0){
                // cout << energy << endl;
                cout << energy << '\t' << l << endl;

            }

            // Reset the previous Psi(r_final)
            prev_psi_final = vec[0];

        } // Energy
    }  // l
}

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

    freopen("eigenvals_co2.txt","w", stdout);
    cout << "Eigenvalue / Ha    l number" << endl;

    double k_co2 = 0.00220873;                   // prefactor for the exponential well

    double k = 0.0001;
    double a = 2.832 * 0.529177;             // exponent in the expoential well
    double m = 48 * 1822.888486;             // mass in atomic units from amu

    print_exp_well_psi_final(k_co2, a, m);

    return 0;
}


// Copyright (c) 2020 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#include <gtest/gtest.h>
#include <boost/geometry.hpp>
#include "pyinterp/detail/math/radial_basis_functions.hpp"

namespace math = pyinterp::detail::math;

static void test_1d(math::RadialBasisFunction function) {
  auto x = Eigen::Matrix<double, 1, 9>::LinSpaced(9, 0, 10);
  auto y = x.array().sin();
  auto rbf =
      math::RBF<double>(std::numeric_limits<double>::quiet_NaN(), 0, function);
  auto yi = rbf.interpolate(x, y, x);

  ASSERT_EQ(y.size(), yi.size());
  for (Eigen::Index ix = 0; ix < yi.size(); ++ix) {
    EXPECT_NEAR(y(ix), yi(ix), 1e-9);
  }
}

static void test_2d(math::RadialBasisFunction function) {
  Eigen::Matrix<double, 2, 50> x = Eigen::Matrix<double, 2, 50>::Random();
  Eigen::Matrix<double, 50, 1> y =
      (x.row(0).array().pow(2) - x.row(1).array().pow(2)).array().exp();
  auto rbf =
      math::RBF<double>(std::numeric_limits<double>::quiet_NaN(), 0, function);
  auto yi = rbf.interpolate(x, y, x);

  ASSERT_EQ(y.size(), yi.size());
  for (Eigen::Index ix = 0; ix < yi.size(); ++ix) {
    EXPECT_NEAR(y(ix), yi(ix), 1e-9);
  }
}

static void test_3d(math::RadialBasisFunction function) {
  Eigen::Matrix<double, 3, 50> x = Eigen::Matrix<double, 3, 50>::Random();
  Eigen::Matrix<double, 50, 1> y =
      (x.row(0).array().pow(2) - x.row(1).array().pow(2)).array().exp();
  auto rbf =
      math::RBF<double>(std::numeric_limits<double>::quiet_NaN(), 0, function);
  auto yi = rbf.interpolate(x, y, x);

  ASSERT_EQ(y.size(), yi.size());
  for (Eigen::Index ix = 0; ix < yi.size(); ++ix) {
    EXPECT_NEAR(y(ix), yi(ix), 1e-9);
  }
}

TEST(math_rbf, 1d) {
  test_1d(math::RadialBasisFunction::Cubic);
  test_1d(math::RadialBasisFunction::Gaussian);
  test_1d(math::RadialBasisFunction::InverseMultiquadric);
  test_1d(math::RadialBasisFunction::Linear);
  test_1d(math::RadialBasisFunction::Multiquadric);
  test_1d(math::RadialBasisFunction::ThinPlate);
}

TEST(math_rbf, 2d) {
  test_2d(math::RadialBasisFunction::Cubic);
  test_2d(math::RadialBasisFunction::Gaussian);
  test_2d(math::RadialBasisFunction::InverseMultiquadric);
  test_2d(math::RadialBasisFunction::Linear);
  test_2d(math::RadialBasisFunction::Multiquadric);
  test_2d(math::RadialBasisFunction::ThinPlate);
}

TEST(math_rbf, 3d) {
  test_3d(math::RadialBasisFunction::Cubic);
  test_3d(math::RadialBasisFunction::Gaussian);
  test_3d(math::RadialBasisFunction::InverseMultiquadric);
  test_3d(math::RadialBasisFunction::Linear);
  test_3d(math::RadialBasisFunction::Multiquadric);
  test_3d(math::RadialBasisFunction::ThinPlate);
}

TEST(math_rbf, point) {
  Eigen::Matrix<double, 3, 50> x = Eigen::Matrix<double, 3, 50>::Random();
  Eigen::Matrix<double, 50, 1> y =
      (x.row(0).array().pow(2) - x.row(1).array().pow(2)).array().exp();
  auto rbf = math::RBF<double>(std::numeric_limits<double>::quiet_NaN(), 0,
                               math::RadialBasisFunction::Cubic);

  auto xi = Eigen::Matrix<double, 3, 1>();
  for (Eigen::Index ix = 0; ix < xi.cols(); ++ix) {
    xi << x.col(ix);
    auto yi = rbf.interpolate(x, y, xi);

    ASSERT_EQ(yi.size(), 1);
    EXPECT_NEAR(y(ix), yi(0), 1e-9);
  }
}


/*
 * Copyright 2018-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <eigen-osqp/CSCMatrix.h>
#include <eigen-osqp/OSQP.h>
#include <iostream>

int main()
{
    int nrVar = 6;
    int nrConstr = 5;
    auto inf = std::numeric_limits<double>::infinity();

    Eigen::SparseMatrix<double> Q(nrVar, nrVar);
    Q.setIdentity();
    Eigen::SparseMatrix<double> A(nrConstr, nrVar);
    std::vector<Eigen::Triplet<double>> nz = {
        { 0, 0, 1. },
        { 0, 1, -1. },
        { 0, 2, 1. },
        { 0, 4, 3. },
        { 0, 5, 1. },
        { 1, 0, -1. },
        { 1, 2, -3. },
        { 1, 3, -4. },
        { 1, 4, 5. },
        { 1, 5, 6. },
        { 2, 0, 2. },
        { 2, 1, 5. },
        { 2, 2, 3. },
        { 2, 4, 1. },
        { 3, 1, 1. },
        { 3, 3, 1. },
        { 3, 4, 2. },
        { 3, 5, -1. },
        { 4, 0, -1. },
        { 4, 2, 2. },
        { 4, 3, 1. },
        { 4, 4, 1. },
    };

    A.setFromTriplets(nz.cbegin(), nz.cend());

    Eigen::VectorXd c(nrVar);
    Eigen::VectorXd AL(nrConstr);
    Eigen::VectorXd AU(nrConstr);
    Eigen::VectorXd XL(nrVar);
    Eigen::VectorXd XU(nrVar);
    c << 1., 2., 3., 4., 5., 6.;
    AL << 1., 2., 3., -inf, -inf;
    AU << 1., 2., 3., -1., 2.5;
    XL << -1000., -10000., 0., -1000., -1000., -1000.;
    XU << 10000., 100., 1.5, 100., 100., 1000.;

    Eigen::OSQP qp;

    qp.problem(nrVar, nrConstr);
    bool success = qp.solve(Q, c, A, AL, AU, XL, XU);

    Eigen::VectorXd result = qp.result();

    std::cout << "Problem:"
              << "\n\tminimize 0.5*x'*Q*x + c'*x"
              << "\n\twith     AL <= A <= AU"
              << "\n\t         XL <= x <= XU"
              << "\n\nQ:\n"
              << Q
              << "\nc:\n"
              << c.transpose()
              << "\nA:\n"
              << A
              << "\nAL:\n"
              << AL.transpose()
              << "\nAU:\n"
              << AU.transpose()
              << "\nXL:\n"
              << XL.transpose()
              << "\nXU:\n"
              << XU.transpose()
              << "\n\n\nSolution:\n"
              << result.transpose() << std::endl;

    std::cout << "\nPress enter to quit" << std::endl;
    std::cin.get();
}


/**
 * @file mirk_test.cc
 * @brief NPDE homework MIRK code
 * @author Philippe Peter
 * @date 14.04.2021
 * @copyright Developed at ETH Zurich
 */

#include "../mirk.h"

#include <gtest/gtest.h>

#include <Eigen/Core>

namespace MIRK::test {

TEST(MIRK, Newton2Steps) {
  Eigen::Vector2d x(-1.0, 1.0);

  // f(x,y) = [e^x,e^y]
  auto f = [](Eigen::Vector2d x) {
    return Eigen::Vector2d(std::exp(x(0)), std::exp(x(1)));
  };

  auto df = [](Eigen::Vector2d x) {
    Eigen::Matrix2d df;
    df << std::exp(x(0)), 0, 0, std::exp(x(1));
    return df;
  };

  // For exp(y): f(y) =  f'(y) for all y and one Newton step satisfies
  // y_{n+1} = y_n - 1
  Eigen::Vector2d x_2ref(-3.0, -1.0);

  // Compute error
  Eigen::Vector2d x_2 = Newton2Steps(f, df, x);
  double err = (x_2 - x_2ref).norm();
  EXPECT_NEAR(0.0, err, 1E-7);
}

}  // namespace MIRK::test


#include <gtest/gtest.h>

#include "mfem.hpp"
using namespace mfem;

#include <iostream>
#include <fstream>
#include <chrono>
#include <random>

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

#include "../include/core/config.hpp"
#include "../include/mymfem/utilities.hpp"

using namespace std;

TEST(MfemUtil, getUpperTriangle)
{
    // CSR sparse matrix
    int sizeA = 8;
    int rowPtrA[9] = { 0, 4, 7, 9, 11, 12, 15, 17, 20 };
    int colIdA[20] = { 0,    2,       5, 6,
                          1, 2,    4,
                             2,             7,
                                   3,       6,
                          1,
                             2,       5,    7,
                          1,             6,
                             2,          6, 7 };
    double dataA[20] = { 7.0,      1.0,           2.0, 7.0,
                             -4.0, 8.0,      2.0,
                                   1.0,                     5.0,
                                        7.0,           9.0,
                             -4.0,
                                   7.0,           3.0,      8.0,
                              1.0,                    11.0,
                                  -3.0,                2.0, 5.0 };

    // create a Sparse matrix
    SparseMatrix trueA(rowPtrA, colIdA, dataA, sizeA, sizeA,
                   false, false, true);

    // get upper triangle
    SparseMatrix& UA = get_upper_triangle (trueA);

    // diagA = UA + UA.transpose() - trueA
    SparseMatrix diagA (*Add(UA, *Transpose(UA)));
    diagA.Add(-1, trueA);

    Vector trueDiagA, errDiagA;
    trueA.GetDiag(trueDiagA);
    diagA.GetDiag(errDiagA);
    errDiagA -= trueDiagA;

    double TOL = 1E-8;
    ASSERT_LE(errDiagA.Normlinf(), TOL);
}

/* Test the point location algorithm implemented in class PointLocator
 * for meshes with local refinement, generated by _serial_ code
*/
TEST(MfemUtil, pointLocator1)
{
    std::string input_dir
            = "../input/poisson_singular_gammaShaped_bisecRefine/";

    int lx1 = 1;
    const std::string mesh_file1 = input_dir+"mesh_lx"+to_string(lx1);

    int lx2 = 6;
    const std::string mesh_file2 = input_dir+"mesh_lx"+to_string(lx2);

    //std::cout << mesh_file1 << std::endl;
    //std::cout << mesh_file2 << std::endl;

    Mesh mesh1(mesh_file1.c_str());
    Mesh mesh2(mesh_file2.c_str());

    const IntegrationRule *ir = nullptr;
    ir = &IntRules.Get(2, 3);

    std::random_device rd;     // initialise (seed) engine
    std::mt19937 rng(rd());    // random-number engine used
    std::uniform_int_distribution<int> uni(0,mesh1.GetNE()-1);

    auto elId1 = uni(rng);
    //std::cout << "Generate in element:" << elId1 << std::endl;
    //elId1 = 3;
    ElementTransformation *trans1 = mesh1.GetElementTransformation(elId1);
    DenseMatrix true_x(mesh1.Dimension(), ir->GetNPoints());
    Vector xk;
    for (int k=0; k < true_x.NumCols(); k++) {
        true_x.GetColumnReference(k,xk);
        trans1->Transform(ir->IntPoint(k), xk);
    }

    Array <int> elIds1(true_x.NumCols());
    Array <IntegrationPoint> ips1(true_x.NumCols());

    Array <int> elIds2(true_x.NumCols());
    Array <IntegrationPoint> ips2(true_x.NumCols());

    auto start1 = std::chrono::high_resolution_clock::now();
    mesh2.FindPoints(true_x, elIds1, ips1);
    auto end1 = std::chrono::high_resolution_clock::now();

    auto start2 = std::chrono::high_resolution_clock::now();
    PointLocator point_locator(&mesh2);
    int init_elId = 0;
    for (int k=0; k < true_x.NumCols(); k++) {
        Vector xk;
        true_x.GetColumn(k, xk);
        std::tie (elIds2[k],ips2[k]) = point_locator(xk, init_elId);
        init_elId = elIds2[k];
    }
    auto end2 = std::chrono::high_resolution_clock::now();

    ElementTransformation *trans2 = nullptr;
    Vector x1, x2, x;
    double TOL = 1E-8;
    for (int k=0; k < true_x.NumCols(); k++)
    {
        trans2 = mesh2.GetElementTransformation(elIds1[k]);
        trans2->Transform(ips1[k], x1);

        trans2 = mesh2.GetElementTransformation(elIds2[k]);
        trans2->Transform(ips2[k], x2);

        true_x.GetColumn(k,x);

        Vector xmx1(x.Size()), xmx2(x.Size());
        subtract(x, x1, xmx1);
        subtract(x, x2, xmx2);

        ASSERT_LE(xmx1.Norml1(), TOL);
        ASSERT_LE(xmx2.Norml1(), TOL);
    }

    auto duration1 = std::chrono::duration_cast
            <std::chrono::microseconds>(end1 - start1);
    auto duration2 = std::chrono::duration_cast
            <std::chrono::microseconds>(end2 - start2);

    std::cout << "Run times: "
              << "\t slow  " << duration1.count()
              << "\t fast  " << duration2.count() << std::endl;
}

/* Test the point location algorithm implemented in class PointLocator
 * for quasi-uniform meshes, generated by _serial_ code.
 * The shared vertices table is generated, but expected to be empty.
*/
TEST(MfemUtil, pointLocator2)
{
    std::string input_dir = "../input/poisson_smooth_unitSquare/";

    int lx1 = 1;
    const std::string mesh_file1 = input_dir+"mesh_lx"+to_string(lx1);

    int lx2 = 6;
    const std::string mesh_file2 = input_dir+"mesh_lx"+to_string(lx2);

    //std::cout << mesh_file1 << std::endl;
    //std::cout << mesh_file2 << std::endl;

    Mesh mesh1(mesh_file1.c_str());
    Mesh mesh2(mesh_file2.c_str());

    const IntegrationRule *ir = nullptr;
    ir = &IntRules.Get(2, 3);

    std::random_device rd;     // initialise (seed) engine
    std::mt19937 rng(rd());    // random-number engine used
    std::uniform_int_distribution<int> uni(0,mesh1.GetNE()-1);

    auto elId1 = uni(rng);
    ElementTransformation *trans1 = mesh1.GetElementTransformation(elId1);
    DenseMatrix true_x(mesh1.Dimension(), ir->GetNPoints());
    Vector xk;
    for (int k=0; k < true_x.NumCols(); k++) {
        true_x.GetColumnReference(k,xk);
        trans1->Transform(ir->IntPoint(k), xk);
    }

    Array <int> elIds1(true_x.NumCols());
    Array <IntegrationPoint> ips1(true_x.NumCols());

    Array <int> elIds2(true_x.NumCols());
    Array <IntegrationPoint> ips2(true_x.NumCols());

    auto start1 = std::chrono::high_resolution_clock::now();
    mesh2.FindPoints(true_x, elIds1, ips1);
    auto end1 = std::chrono::high_resolution_clock::now();

    bool has_shared_vertices = true;
    auto start2 = std::chrono::high_resolution_clock::now();
    PointLocator point_locator(&mesh2, has_shared_vertices);
    int init_elId = 0;
    for (int k=0; k < true_x.NumCols(); k++) {
        Vector xk;
        true_x.GetColumn(k, xk);
        std::tie (elIds2[k],ips2[k]) = point_locator(xk, init_elId);
        init_elId = elIds2[k];
    }
    auto end2 = std::chrono::high_resolution_clock::now();

    ElementTransformation *trans2 = nullptr;
    Vector x1, x2, x;
    double TOL = 1E-8;
    for (int k=0; k < true_x.NumCols(); k++)
    {
        trans2 = mesh2.GetElementTransformation(elIds1[k]);
        trans2->Transform(ips1[k], x1);

        trans2 = mesh2.GetElementTransformation(elIds2[k]);
        trans2->Transform(ips2[k], x2);

        true_x.GetColumn(k,x);

        Vector xmx1(x.Size()), xmx2(x.Size());
        subtract(x, x1, xmx1);
        subtract(x, x2, xmx2);

        ASSERT_LE(xmx1.Norml1(), TOL);
        ASSERT_LE(xmx2.Norml1(), TOL);
    }

    auto duration1 = std::chrono::duration_cast
            <std::chrono::microseconds>(end1 - start1);
    auto duration2 = std::chrono::duration_cast
            <std::chrono::microseconds>(end2 - start2);

    std::cout << "Run times: "
              << "\t slow  " << duration1.count()
              << "\t fast  " << duration2.count() << std::endl;
}

/* Test the point location algorithm implemented in class PointLocator
 * for quasi-uniform meshes, generated by _parallel_ code.
 * The shared vertices table is generated.
*/
TEST(MfemUtil, pointLocator3)
{
    std::string input_dir = "../input/poisson_smooth_unitSquare/";

    int lx1 = 1;
    const std::string mesh_file1 = input_dir+"pmesh_lx"+to_string(lx1);

    int lx2 = 6;
    const std::string mesh_file2 = input_dir+"pmesh_lx"+to_string(lx2);

    //std::cout << mesh_file1 << std::endl;
    //std::cout << mesh_file2 << std::endl;

    Mesh mesh1(mesh_file1.c_str());
    Mesh mesh2(mesh_file2.c_str());

    const IntegrationRule *ir = nullptr;
    ir = &IntRules.Get(2, 3);

    std::random_device rd;     // initialise (seed) engine
    std::mt19937 rng(rd());    // random-number engine used
    std::uniform_int_distribution<int> uni(0,mesh1.GetNE()-1);

    auto elId1 = uni(rng);
    ElementTransformation *trans1 = mesh1.GetElementTransformation(elId1);
    DenseMatrix true_x(mesh1.Dimension(), ir->GetNPoints());
    Vector xk;
    for (int k=0; k < true_x.NumCols(); k++) {
        true_x.GetColumnReference(k,xk);
        trans1->Transform(ir->IntPoint(k), xk);
    }

    Array <int> elIds(true_x.NumCols());
    Array <IntegrationPoint> ips(true_x.NumCols());

    bool has_shared_vertices = true;
    auto start = std::chrono::high_resolution_clock::now();
    PointLocator point_locator(&mesh2, has_shared_vertices);
    int init_elId = 0;
    for (int k=0; k < true_x.NumCols(); k++) {
        Vector xk;
        true_x.GetColumn(k, xk);
        std::tie (elIds[k],ips[k]) = point_locator(xk, init_elId);
        init_elId = elIds[k];
    }
    auto end = std::chrono::high_resolution_clock::now();

    ElementTransformation *trans2 = nullptr;
    Vector xp, x;
    double TOL = 1E-8;
    for (int k=0; k < true_x.NumCols(); k++)
    {
        trans2 = mesh2.GetElementTransformation(elIds[k]);
        trans2->Transform(ips[k], xp);

        true_x.GetColumn(k,x);

        Vector xmxp(x.Size());
        subtract(x, xp, xmxp);

        ASSERT_LE(xmxp.Norml1(), TOL);
    }

    auto duration = std::chrono::duration_cast
            <std::chrono::microseconds>(end - start);

    std::cout << "Run time: "
              << duration.count() << std::endl;
}


#include <PCP/Curvature/Curvature.h>
#include <PCP/Curvature/Weight.h>

#include <PCP/Geometry/Geometry.h>

#include <PCP/SpacePartitioning/KdTree.h>

#include <Eigen/Eigenvalues>

namespace pcp {

PointWiseEstimationData CurvatureComputer_PCAPlane::compute(const Geometry& points, int i, Scalar r) const
{
    Matrix3 C      = Matrix3::Zero();
    Vector3 m      = Vector3::Zero();
    Scalar  sum_w  = 0;

    int nei_count = 0;

    for(int j : points.kdtree().range_neighbors(i, r))
    {
        const Scalar w = weight(points[i], points[j], r);
        m      += w * (points[j] - points[i]);
        C      += w * (points[j] - points[i]) * (points[j] - points[i]).transpose();
        sum_w  += w;
        ++nei_count;
    }

    if(nei_count < NEI_COUNT_MIN || sum_w < SUM_WEIGHT_MIN)
        return PointWiseEstimationData::Invalid();

    m /= sum_w;
    C = C/sum_w - m * m.transpose();

    // eigenvalues are positive and ordered
    // eigenvectors are normalized
    Eigen::SelfAdjointEigenSolver<Matrix3> solver(C);
    Vector3 N = solver.eigenvectors().col(0);

    const Scalar H = 8 * m.dot(N); // *  1/r^2

    return PointWiseEstimationData(666, 666, -H, N, Vector3::Constant(666), Vector3::Constant(666), nei_count); // -1 due to convention
}

} // namespace pcp


// SPDX-License-Identifier: MIT
// Copyright (c) 2021 ETH Zurich, Luc Grosheintz-Laval

#ifndef LSQ_SOLVER_H_7JQG4
#define LSQ_SOLVER_H_7JQG4

#include <Eigen/Dense>

#include <zisa/config.hpp>
#include <zisa/grid/grid.hpp>
#include <zisa/math/poly2d.hpp>
#include <zisa/reconstruction/stencil.hpp>
#include <zisa/reconstruction/weno_poly.hpp>

namespace zisa {

/// Solve the least-squares problem for the reconstruction.
/** WENO-AO defines the reconstruction polynomial as the polynomial
 * which approximates the cell-averages on a given stencil the best, in a
 * least-squares sense.
 *
 * This class solves those LSQ problems.
 */
class LSQSolver {
private:
  using LDLT = Eigen::LDLT<Eigen::MatrixXd>;

public:
  LSQSolver() = default;
  LSQSolver(const std::shared_ptr<Grid> &grid, const Stencil &stencil);

  /// Solve the LSQ problem with right-hand side `rhs`.
  template <class Poly>
  Poly solve(const array_const_view<double, 2, row_major> &rhs) const;

  /// Indistinguishable by calls to the public interface.
  bool operator==(const LSQSolver &other) const;

  /// Can be distinguished by calls to the public interface.
  bool operator!=(const LSQSolver &other) const;

private:
  template <class Poly>
  Poly solve_impl(const array_const_view<double, 2, row_major> &rhs) const;

  int n_dims() const;

private:
  std::shared_ptr<Grid> grid;
  int_t i_cell;
  int order;

  LDLT ldlt;
  Eigen::MatrixXd A;
};

Eigen::MatrixXd assemble_weno_ao_matrix(const Grid &grid,
                                        const Stencil &stencil);

Eigen::MatrixXd assemble_weno_ao_matrix(
    const Grid &grid, const array_const_view<int_t, 1> &stencil, int order);

} // namespace zisa
#endif /* end of include guard */


#include "ugl/random/normal_distribution.h"

#include <cmath>
#include <random>

#include <Eigen/Eigenvalues>

#include "random_engine.h"

namespace ugl::random
{

ugl::MatrixD create_transform(const ugl::MatrixD& covariance)
{
    Eigen::SelfAdjointEigenSolver<ugl::MatrixD> eigen_solver(covariance);
    return eigen_solver.eigenvectors() * eigen_solver.eigenvalues().cwiseSqrt().asDiagonal();
}

NormalDistribution<1>::NormalDistribution(double mean, double variance)
    : mean_(mean)
    , variance_(variance)
    , stddev_(std::sqrt(variance))
{
}

double NormalDistribution<1>::sample() const
{
    std::normal_distribution distribution{mean_, stddev_};
    return distribution(internal::rng);
}

double NormalDistribution<1>::sample(double mean, double variance)
{
    std::normal_distribution distribution{mean, std::sqrt(variance)};
    return distribution(internal::rng);
}

} // namespace ugl::random


//
// Helper utilities for simulators.
//

#pragma once

#include <ilqr/ilqr_taylor_expansions.hh> // Definition of dynamics function.

#include <Eigen/Dense>

#include <functional>

namespace simulators
{

template<int dim>
using Vector = Eigen::Matrix<double, dim, 1>;

// Wrapper struct so we can use template paramters.
template<int xdim, int udim>
struct RK4
{
using Dynamics = std::function<Vector<xdim>(const Vector<xdim> &x, const Vector<udim> &u)>;

// 4th order Runge-Kutta integration of an ODE given by dynamics.
static Vector<xdim> rk4(const double dt,
        const Vector<xdim> state,
        const Vector<udim> control,
        const Dynamics &dynamics)
{

    // This cannot be changed
    constexpr double RK4_INTEGRATION_CONSTANT = 1.0/6.0;

    // Formula from: http://mathworld.wolfram.com/Runge-KuttaMethod.html
    const Vector<xdim> k1 = dynamics(state, control);
    const Vector<xdim> k2 = dynamics(state + 0.5*dt*k1, control);
    const Vector<xdim> k3 = dynamics(state + 0.5*dt*k2, control);
    const Vector<xdim> k4 = dynamics(state + dt*k3, control);

    const Vector<xdim> result = state + RK4_INTEGRATION_CONSTANT*dt*(k1 + 2.0*(k2 + k3) + k4);
    return result;
}

};

constexpr double INTEGRATION_FREQUENCY = 5;
constexpr double MIN_INTEGRATION_DT = 1e-2;

Eigen::VectorXd step(const Eigen::VectorXd &state, const Eigen::VectorXd &control,
                     const double dt, const ilqr::DynamicsFunc &dynamics,
                     const double min_integration_dt = MIN_INTEGRATION_DT,
                     const double integration_frequency  = INTEGRATION_FREQUENCY
                     );

} // namespace simulators


//
// Created by sam on 04/12/17.
//

#ifndef PERCEPTRON_NEURON_HPP
#define PERCEPTRON_NEURON_HPP

#include <iostream>
#include <vector>
#include <boost/numeric/ublas/vector.hpp>

class Neuron {
private:
    boost::numeric::ublas::vector<double> weights;
    double gradient;

    double (*functionAct)(double);

public:
    Neuron(const boost::numeric::ublas::vector<double> &weights, double (*functionAct)(double));

    /**
     *
     * @param entries
     * @return the output of the neuron with the function
     */
    double getOutput(boost::numeric::ublas::vector<double> const &entries);

    void correctWeitghs(boost::numeric::ublas::vector<double> const &e, double result, double expected);

    const boost::numeric::ublas::vector<double> &getWeights() const;

    void setWeights(const boost::numeric::ublas::vector<double> &weights);

    void setFunctionAct(double (*functionAct)(double));
};

#endif //PERCEPTRON_NEURON_HPP


// Boost.Geometry
// Unit Test

// Copyright (c) 2016-2017 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <sstream>

#include "test_formula.hpp"
#include "inverse_cases.hpp"

#include <boost/geometry/formulas/vincenty_inverse.hpp>
#include <boost/geometry/formulas/thomas_inverse.hpp>
#include <boost/geometry/formulas/andoyer_inverse.hpp>

void check_inverse(std::string const& name,
                   expected_results const& results,
                   bg::formula::result_inverse<double> const& result,
                   expected_result const& expected,
                   expected_result const& reference,
                   double reference_error)
{
    std::stringstream ss;
    ss << "(" << results.p1.lon << " " << results.p1.lat << ")->(" << results.p2.lon << " " << results.p2.lat << ")";

    check_one(name + "_d  " + ss.str(),
              result.distance, expected.distance, reference.distance, reference_error);
    check_one(name + "_a  " + ss.str(),
              result.azimuth, expected.azimuth, reference.azimuth, reference_error, true);
    check_one(name + "_ra " + ss.str(),
              result.reverse_azimuth, expected.reverse_azimuth, reference.reverse_azimuth, reference_error, true);
    check_one(name + "_rl " + ss.str(),
              result.reduced_length, expected.reduced_length, reference.reduced_length, reference_error);
    check_one(name + "_gs " + ss.str(),
              result.geodesic_scale, expected.geodesic_scale, reference.geodesic_scale, reference_error);
}

void test_all(expected_results const& results)
{
    double const d2r = bg::math::d2r<double>();
    double const r2d = bg::math::r2d<double>();

    double lon1r = results.p1.lon * d2r;
    double lat1r = results.p1.lat * d2r;
    double lon2r = results.p2.lon * d2r;
    double lat2r = results.p2.lat * d2r;

    // WGS84
    bg::srs::spheroid<double> spheroid(6378137.0, 6356752.3142451793);

    bg::formula::result_inverse<double> result_v, result_t, result_a;

    typedef bg::formula::vincenty_inverse<double, true, true, true, true, true> vi_t;
    result_v = vi_t::apply(lon1r, lat1r, lon2r, lat2r, spheroid);
    result_v.azimuth *= r2d;
    result_v.reverse_azimuth *= r2d;
    check_inverse("vincenty", results, result_v, results.vincenty, results.reference, 0.0000001);

    typedef bg::formula::thomas_inverse<double, true, true, true, true, true> th_t;
    result_t = th_t::apply(lon1r, lat1r, lon2r, lat2r, spheroid);
    result_t.azimuth *= r2d;
    result_t.reverse_azimuth *= r2d;
    check_inverse("thomas", results, result_t, results.thomas, results.reference, 0.00001);

    typedef bg::formula::andoyer_inverse<double, true, true, true, true, true> an_t;
    result_a = an_t::apply(lon1r, lat1r, lon2r, lat2r, spheroid);
    result_a.azimuth *= r2d;
    result_a.reverse_azimuth *= r2d;
    check_inverse("andoyer", results, result_a, results.andoyer, results.reference, 0.001);
}

int test_main(int, char*[])
{
    for (size_t i = 0; i < expected_size; ++i)
    {
        test_all(expected[i]);
    }

    return 0;
}


#include "CubicInterpolation/FindParameter.hpp"

#include <boost/math/tools/roots.hpp>
#include <cassert>

namespace cubic_splines {
namespace detail {
double _find_parameter(std::function<double(double)> const &f,
                        std::function<double(double)> const &df, double x_guess,
                        double lower, double upper) {
  assert(((void)"find_parameter call is not well defined", lower < upper));
  auto func = [&f, &df](double x) { return std::make_tuple(f(x), df(x)); };

  if (std::isnan(x_guess)) {
    auto bisec_tolerance = (upper - lower) * 1e-2;
    auto tol = [&bisec_tolerance](double min, double max) {
      return (std::abs(max - min) < bisec_tolerance) ? true : false;
    };
    std::tie(lower, upper) = boost::math::tools::bisect(f, lower, upper, tol);
    x_guess = (lower + upper) / 2;
    if (lower == upper)
      return lower;
  }

  return boost::math::tools::newton_raphson_iterate(func, x_guess, lower, upper, 20);
}
} // namespace detail
} // namespace cubic_splines


#include <boost/numeric/ublas/storage.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

std::complex<long double> ;

ComplexPlane::ComplexPlane()
{

    // Implementation of default constructor
    // set up min/max/len
    : xmin(-10)
    , xmax(10)
    , xlen(20)
    , ymin(-10)
    , ymax(10)
    , ylen(20)

{};

    using namespace boost::numeric::ublas;
    slice real (xmin, xmax, xlen);
    slice imag (ymin, ymax, ylen);

    for (unsigned r = 0; r < real.size(); ++ r) {
        for (unsigned c = 0; c < imag.size(); ++ c){
            this-> plane(real,imag) = real(r)+imag(c)*1i;
        }
    }
}


#include "NumUtils.h"

#include <NTL/RR.h>
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <cassert>
#include <cmath>

#include "CZZ.h"

void NumUtils::sampleGauss(ZZX& res, const long& size, const double& stdev) {
	static double const Pi = 4.0 * atan(1.0);
	static long const bignum = 0xfffffff;
	res.SetLength(size);

	for (long i = 0; i < size; i+=2) {
		double r1 = (1 + RandomBnd(bignum)) / ((double)bignum + 1);
		double r2 = (1 + RandomBnd(bignum)) / ((double)bignum + 1);
		double theta=2 * Pi * r1;
		double rr= sqrt(-2.0 * log(r2)) * stdev;
		assert(rr < 8 * stdev); // sanity-check, no more than 8 standard deviations
		// Generate two Gaussians RV's, rounded to integers
		long x1 = (long) floor(rr * cos(theta) + 0.5);
		SetCoeff(res, i, x1);
		if(i + 1 < size) {
			long x2 = (long) floor(rr * sin(theta) + 0.5);
			SetCoeff(res, i + 1, x2);
		}
	}
}

void NumUtils::sampleHWT(ZZX& res, const long& size, const long& h) {
	res.SetLength(size);
	long idx = 0;
	ZZ tmp = RandomBits_ZZ(h);
	while(idx < h) {
		long i = RandomBnd(size);
		if(res.rep[i] == 0) {
			res.rep[i] = (bit(tmp, idx) == 0) ? ZZ(1) : ZZ(-1);
			idx++;
		}
	}
}

void NumUtils::sampleZO(ZZX& res, const long& size) {
	res.SetLength(size);
	ZZ tmp = RandomBits_ZZ(2 * size);
	for (long i = 0; i < size; ++i) {
		res.rep[i] = (bit(tmp, 2 * i) == 0) ? ZZ(0) : (bit(tmp, 2 * i + 1) == 0) ? ZZ(1) : ZZ(-1);
	}
}

void NumUtils::sampleBinary(ZZX& res, const long& size, const long& h) {
	res.SetLength(size);
	long idx = 0;
	while(idx < h) {
		long i = RandomBnd(size);
		if(res.rep[i] == 0) {
			res.rep[i] = ZZ(1);
			idx++;
		}
	}
}

void NumUtils::sampleBinary(ZZX& res, const long& size) {
	res.SetLength(size);
	ZZ tmp = RandomBits_ZZ(size);
	for (long i = 0; i < size; ++i) {
		res.rep[i] = (bit(tmp, i) == 0) ? ZZ(0) : ZZ(1);
	}
}

void NumUtils::sampleUniform2(ZZX& res, const long& size, const long& logBnd) {
	res.SetLength(size);
	for (long i = 0; i < size; i++) {
		res.rep[i] = RandomBits_ZZ(logBnd);
	}
}

void NumUtils::fftRaw(CZZ*& vals, const long& size, SchemeAux& aux, const bool& isForward) {
	for (long i = 1, j = 0; i < size; ++i) {
		long bit = size >> 1;
		for (; j >= bit; bit>>=1) {
			j -= bit;
		}
		j += bit;
		if(i < j) {
			swap(vals[i], vals[j]);
		}
	}
	if(isForward) {
		for (long len = 2; len <= size; len <<= 1) {
			long MoverLen = aux.M / len;
			for (long i = 0; i < size; i += len) {
				for (long j = 0; j < len / 2; ++j) {
					CZZ u = vals[i + j];
					CZZ v = vals[i + j + len / 2];
					RR tmp1 = to_RR(v.r) * (aux.ksiPowsr[j * MoverLen] + aux.ksiPowsi[j * MoverLen]);
					RR tmpr = tmp1 - to_RR(v.r + v.i) * aux.ksiPowsi[j * MoverLen];
					RR tmpi = tmp1 + to_RR(v.i - v.r) * aux.ksiPowsr[j * MoverLen];
					v.r = to_ZZ(tmpr);
					v.i = to_ZZ(tmpi);
					vals[i + j] = u + v;
					vals[i + j + len / 2] = u - v;
				}
			}
		}
	} else {
		for (long len = 2; len <= size; len <<= 1) {
			long MoverLen = aux.M / len;
			for (long i = 0; i < size; i += len) {
				for (long j = 0; j < len / 2; ++j) {
					CZZ u = vals[i + j];
					CZZ v = vals[i + j + len / 2];
					RR tmp1 = to_RR(v.r) * (aux.ksiPowsr[(len - j) * MoverLen] + aux.ksiPowsi[(len - j) * MoverLen]);
					RR tmpr = tmp1 - to_RR(v.r + v.i) * aux.ksiPowsi[(len - j) * MoverLen];
					RR tmpi = tmp1 + to_RR(v.i - v.r) * aux.ksiPowsr[(len - j) * MoverLen];
					v.r = to_ZZ(tmpr);
					v.i = to_ZZ(tmpi);
					vals[i + j] = u + v;
					vals[i + j + len / 2] = u - v;
				}
			}
		}
	}
}

void NumUtils::fft(CZZ*& vals, const long& size, SchemeAux& aux) {
	fftRaw(vals, size, aux, true);
}

void NumUtils::fftInv(CZZ*& vals, const long& size, SchemeAux& aux) {
	fftRaw(vals, size, aux, false);
	long logSize = log2(size);
	for (long i = 0; i < size; ++i) {
		vals[i] >>= logSize;
	}
}

void NumUtils::fftInvLazy(CZZ*& vals, const long& size, SchemeAux& aux) {
	fftRaw(vals, size, aux, false);
}

void NumUtils::fftSpecial(CZZ*& vals, const long& size, SchemeAux& aux) {
	for (int i = 1, j = 0; i < size; ++i) {
		long bit = size >> 1;
		for (; j>=bit; bit>>=1) {
			j -= bit;
		}
		j += bit;
		if(i < j) {
			swap(vals[i], vals[j]);
		}
	}
	for (long len = 2; len <= size; len <<= 1) {
		long loglen = log2(len);
		long Mover2Len = aux.M / len / 2;
		for (long i = 0; i < size; i += len) {
			for (long j = 0; j < len / 2; ++j) {
				CZZ u = vals[i + j];
				CZZ v = vals[i + j + len / 2];
				RR tmp1 = to_RR(v.r) * (aux.ksiPowsr[(2 * j + 1) * Mover2Len] + aux.ksiPowsi[(2 * j + 1) * Mover2Len]);
				RR tmpr = tmp1 - to_RR(v.r + v.i) * aux.ksiPowsi[(2 * j + 1) * Mover2Len];
				RR tmpi = tmp1 + to_RR(v.i - v.r) * aux.ksiPowsr[(2 * j + 1) * Mover2Len];
				v.r = to_ZZ(tmpr);
				v.i = to_ZZ(tmpi);
				vals[i + j] = u + v;
				vals[i + j + len / 2] = u - v;
			}
		}
	}
}

void NumUtils::fftSpecialInv(CZZ*& vals, const long& size, SchemeAux& aux) {
	fftRaw(vals, size, aux, false);
	long logsize = log2(size);
	long Mover2size = aux.M / size / 2;
	for (long i = 0; i < size; ++i) {
		RR tmp1 = to_RR(vals[i].r) * (aux.ksiPowsr[(2 * size - i) * Mover2size] + aux.ksiPowsi[(2 * size - i) * Mover2size]);
		RR tmpr = tmp1 - to_RR(vals[i].r + vals[i].i) * aux.ksiPowsi[(2 * size - i) * Mover2size];
		RR tmpi = tmp1 + to_RR(vals[i].i - vals[i].r) * aux.ksiPowsr[(2 * size - i) * Mover2size];
		vals[i].r = to_ZZ(tmpr);
		vals[i].i = to_ZZ(tmpi);
		vals[i] >>= logsize;
	}
}


#include "glog/logging.h"
#include <armadillo>

#include "pclem_math.h"

#include "matrix33.h"

namespace pclem {
    Matrix33::Matrix33() : values{} {}

    // The values are given row-major.
    Matrix33::Matrix33(const std::array<double,9>& values) : values(values) {}

    double Matrix33::get_element(const int& i, const int& j) const {
        return values[i*3 + j];
    }

    Matrix33 Matrix33::identity() {
        return Matrix33({1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0});
    }

    bool Matrix33::operator==(const Matrix33& other) const {
        for(int i=0; i < 9; i++) {
            if(!approximatelyEqual(values[i], other.values[i], 1e-10)) {
                return false;
            }
        }
        return true;
    }

    Vector3 Matrix33::get_column(int i) const {
        return Vector3(values[i], values[3+i], values[6+i]);
    }

    double Matrix33::trace() const {
        return values[0] + values[4] + values[8];
    }

    // Returns the eigenvalues in descending order. The first column
    // of the matrix is the eigenvector of the largest eigenvalue, and
    // so on.
    std::pair<Vector3, Matrix33> Matrix33::eigen_decomposition() const {

        VLOG(10) << "Extracting eigenvalues...";

        if(symetric(1e-10)) {
            return symetrical_eigen_decomposition();
        } else {
            return general_eigen_decomposition();
        }
    }

    std::pair<Vector3, Matrix33> Matrix33::symetrical_eigen_decomposition() const {
        arma::mat33 arma_cov_mat(values.data());
        arma::vec arma_eigvals;
        arma::mat arma_eigvecs;

        if(!arma::eig_sym(arma_eigvals, arma_eigvecs, arma_cov_mat)) {
            LOG(WARNING) << "Eigenvalues decomposition failed.";
        }

        Vector3 eigvals;
        std::array<double,9> eigvecs_values;
        for(int i=0; i < 3; i++) {
            eigvals[i] = arma_eigvals[2-i];

            for(int j=0; j < 3; j++) {
                eigvecs_values[i*3 + j] = arma_eigvecs(i,2-j);
            }
        }

        return std::make_pair(eigvals, Matrix33(eigvecs_values));
    }

    std::pair<Vector3, Matrix33> Matrix33::general_eigen_decomposition() const {
        arma::mat33 arma_cov_mat(values.data());
        arma::cx_vec arma_eigvals;
        arma::cx_mat arma_eigvecs;

        if(!arma::eig_gen(arma_eigvals, arma_eigvecs, arma_cov_mat)) {
            LOG(WARNING) << "Eigenvalues decomposition failed.";
        }

        Vector3 eigvals;
        std::array<double,9> eigvecs_values;
        for(int i=0; i < 3; i++) {
            eigvals[i] = arma_eigvals[2-i].real();

            for(int j=0; j < 3; j++) {
                eigvecs_values[i*3 + j] = arma_eigvecs(j,i).real();
            }
        }

        return std::make_pair(eigvals, Matrix33(eigvecs_values));
    }

    std::array<double,9> Matrix33::inverse() const {
        VLOG(11) << "Inverting matrix...";

        arma::mat33 arma_cov_mat(values.data());
        arma::mat33 arma_inv_of_cov = arma::pinv(arma_cov_mat + arma::eye(3,3));

        VLOG(11) << "PseudoInverse error " << arma::norm(arma_cov_mat*arma_inv_of_cov - arma::eye(3,3));

        std::array<double,9> inverse;
        for(auto i = 0; i < 3; i++) {
            for(auto j = 0; j < 3; j++) {
                inverse[i*3 + j] = arma_inv_of_cov(i,j);
            }
        }

        VLOG(11) << "Done inverting matrix.";
        return inverse;
    }

    double Matrix33::det() const {
        return get_element(0,0) * (get_element(1,1) * get_element(2,2) - get_element(2, 1) * get_element(1,2)) -
            get_element(0,1) * (get_element(1,0) * get_element(2,2) - get_element(1,2) * get_element(2,0)) +
            get_element(0,2) * (get_element(1,0) * get_element(2,1) + get_element(2,0) * get_element(1,1));
    }

    std::ostream& operator<<(std::ostream& os, const Matrix33& v) {
        for(int i=0; i < 8; i++) {
            os << v.values[i] << ",";
        }
        os << v.values[8];
    }

    void Matrix33::set_element(const int& i, const int& j, const double value) {
        values[i*3 + j] = value;
    }

    std::ofstream& operator<<(std::ofstream& ofs, const Matrix33& v) {
        for(int i=0; i < 8; i++) {
            ofs << v.values[i] << ",";
        }
        ofs << v.values[8];

        return ofs;
    }

    bool Matrix33::symetric(double epsilon) const {
        for(int i = 0; i < 3; i++) {
            for(int j = 0; j < 3; j++) {
                if(!approximatelyEqual(get_element(i, j), get_element(i, j), epsilon)) {
                        return false;
                }
            }
        }

        return true;
    }

    Matrix33 Matrix33::zeros() {
        return Matrix33({0.0});
    }
}