#pragma once
#include <Eigen/Core>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>

namespace gtsam
{
  class LidarEdgeFactor2 : public NoiseModelFactor2<Pose3, Pose3>
  {

    using X = Pose3;
    using Base = NoiseModelFactor2<Pose3, Pose3>;
    using This = LidarEdgeFactor2;

  public:
    LidarEdgeFactor2(Key key1, Key key2, const Point3 &point1, const Point3 &unit, const Point3 &point2, const SharedNoiseModel &model)
        : Base(model, key1, key2), p1_(point1), p2_(point2), u_(unit.normalized())
    {
    }

    virtual ~LidarEdgeFactor2() {}

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    line_err = (p2w-p1).cross(u)
            = (T2*p2 - p1).cross(u)
    dline_err/dT2 = (-u)x * d(T2*p2)/dT2 = (-u)x * R2 * [[-p]x I3X3]
    */

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    line_err = (p2w-p1w).cross(uw)
            = (T2*p2 - T1*p1).cross(R1*u)
    dline_err/dT1 = (T2*p2 - T1*p1)x * d(R1*u)/dT1 + (R1*u)x * d(T1*p1)/dT1
    dline_err/dT2 = -(R1*u)x * d(T2*p2)/dT2
    */

    Vector evaluateError(const X &pose1, const X &pose2,
                         boost::optional<Matrix &> H1 = boost::none,
                         boost::optional<Matrix &> H2 = boost::none) const
    {
      const auto &rotation1 = pose1.rotation().matrix();
      const auto &rotation2 = pose2.rotation().matrix();
      const auto p12w = pose2.transformFrom(p2_) - pose1.transformFrom(p1_);
      const auto uw = rotation1 * u_;
      if (H1)
        *H1 = skewSymmetric(p12w[0], p12w[1], p12w[2]) * rotation1 * (Matrix36() << skewSymmetric(-u_[0], -u_[1], -u_[2]), Z_3x3).finished()
            + skewSymmetric(uw[0], uw[1], uw[2]) * rotation1 * (Matrix36() << skewSymmetric(-p1_[0], -p1_[1], -p1_[2]), I_3x3).finished();
      if (H2)
        *H2 = skewSymmetric(-uw[0], -uw[1], -uw[2]) * rotation2 * (Matrix36() << skewSymmetric(-p2_[0], -p2_[1], -p2_[2]), I_3x3).finished();
      return p12w.cross(uw);
    }

    virtual NonlinearFactor::shared_ptr clone() const
    {
      return boost::static_pointer_cast<NonlinearFactor>(
          NonlinearFactor::shared_ptr(new This(*this)));
    }

    virtual bool equals(const NonlinearFactor &expected, double tol = 1e-9) const
    {
      const This *e = dynamic_cast<const This *>(&expected);
      return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(p1_, e->p1_, tol) &&
             traits<Point3>::Equals(p2_, e->p2_, tol) && traits<Point3>::Equals(u_, e->u_, tol);
    }

    virtual void print(const std::string &s = "",
                       const KeyFormatter &keyFormatter = DefaultKeyFormatter) const
    {
      cout << s << ":\nLidarEdgeFactor2 on (" << keyFormatter(key1())
           << ", " << keyFormatter(key2()) << ")\n"
           << "  Edge Point: " << p1_.transpose() << "\n"
           << "  Edge Axis: " << u_.transpose() << "\n"
           << "  Match Point: " << p2_.transpose() << "\n";
      noiseModel_->print("  noise model: ");
    }

  private:
    Point3 p1_, p2_, u_;

  }; // class LidarEdgeFactor2

  class LidarEdgeFactor1 : public NoiseModelFactor1<Pose3>
  {

    using X = Pose3;
    using Base = NoiseModelFactor1<Pose3>;
    using This = LidarEdgeFactor1;

  public:
    LidarEdgeFactor1(Key key, const Point3 &point1, const Point3 &unit, const Point3 &point2, const SharedNoiseModel &model)
        : Base(model, key), p1_(point1), p2_(point2), u_(unit.normalized())
    {
    }

    virtual ~LidarEdgeFactor1() {}

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    line_err = (p2w-p1).cross(u)
            = (T2*p2 - p1).cross(u)
    dline_err/dT2 = (-u)x * d(T2*p2)/dT2 = (-u)x * R2 * [[-p]x I3X3]
    */

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    line_err = (p2w-p1w).cross(uw)
            = (T2*p2 - T1*p1).cross(R1*u)
    dline_err/dT1 = (T2*p2 - T1*p1)x * d(R1*u)/dT1 + (R1*u)x * d(T1*p1)/dT1
    dline_err/dT2 = -(R1*u)x * d(T2*p2)/dT2
    */

    Vector evaluateError(const X &pose,
                         boost::optional<Matrix &> H = boost::none) const
    {
      const auto &rotation = pose.rotation();
      if (H)
        *H = skewSymmetric(-u_[0], -u_[1], -u_[2]) * rotation.matrix() * (Matrix36() << skewSymmetric(-p2_[0], -p2_[1], -p2_[2]), I_3x3).finished();
      return (pose.transformFrom(p2_) - p1_).cross(u_);
    }

    virtual NonlinearFactor::shared_ptr clone() const
    {
      return boost::static_pointer_cast<NonlinearFactor>(
          NonlinearFactor::shared_ptr(new This(*this)));
    }

    virtual bool equals(const NonlinearFactor &expected, double tol = 1e-9) const
    {
      const This *e = dynamic_cast<const This *>(&expected);
      return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(p1_, e->p1_, tol) &&
             traits<Point3>::Equals(p2_, e->p2_, tol) && traits<Point3>::Equals(u_, e->u_, tol);
    }

    virtual void print(const std::string &s = "",
                       const KeyFormatter &keyFormatter = DefaultKeyFormatter) const
    {
      cout << s << ":\nLidarEdgeFactor1 on (" << keyFormatter(key()) << ")\n"
           << "  Edge Point: " << p1_.transpose() << "\n"
           << "  Edge Axis: " << u_.transpose() << "\n"
           << "  Match Point: " << p2_.transpose() << "\n";
      noiseModel_->print("  noise model: ");
    }

  private:
    Point3 p1_, p2_, u_;

  }; // class LidarEdgeFactor1

  class LidarEdgeProjectedFactor1 : public NoiseModelFactor1<Pose3>
  {

    using X = Pose3;
    using Base = NoiseModelFactor1<Pose3>;
    using This = LidarEdgeProjectedFactor1;

  public:
    LidarEdgeProjectedFactor1(Key key, const Point3 &point1, const Point3 &unit, const Point3 &point2, const SharedNoiseModel &model)
        : Base(model, key), p1_(point1), p2_(point2), u_(unit.normalized())
    {
      // std::cout << "unit:" << u_.transpose()<<"\n";
    }

    virtual ~LidarEdgeProjectedFactor1() {}

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    line_err = (p2w-p1).cross(u)
            = (T2*p2 - p1).cross(u)
    dline_err/dT2 = (-u)x * d(T2*p2)/dT2 = (-u)x * R2 * [[-p]x I3X3]
    */

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    line_err = (p2w-p1w).cross(uw)
            = (T2*p2 - T1*p1).cross(R1*u)
    dline_err/dT1 = (T2*p2 - T1*p1)x * d(R1*u)/dT1 + (R1*u)x * d(T1*p1)/dT1
    dline_err/dT2 = -(R1*u)x * d(T2*p2)/dT2
    */

    /*
    d(T^-1*p)/dT = [[T^-1*p]x -I3X3]
    d(R^-1*u)/dT = [[R^T*u]x   03x3]

    line_err = (p2 - p1).cross(u)
             = (p2 - T2^-1*p1).cross(R2^T*u1)

    point_err = (p2 - T2^-1*p1)[0,1] * R2^T*u1[2] = (p2 - c2)[0,1] * c1[2]

    dline_err/dT2 = [R2^-1*u1]x*d(T2^-1*p1)/dT2 + d(R2^-1*u1)/dT2
                  = [R2^T*u1]x*[[T2^-1*p1]x -I3X3] + [[R2^T*u1]x   03x3]

    c1 = R2^T*u1
    c2 = T2^-1*p1
    dpoint_err/dT2 = -dc2/dT2[0,1] * c1[2] + (p2 - c2)[0,1] * dc1/dT2[2]
                   = -[[c2]x -I3X3][0,1] * c1[2] +  (p2 - c2)[0,1]  *  [[c1]x  03x3][2]
    dc1/dT2 = [[R2^T*u1]x   03x3]
    dc2/dT2 = [[T2^-1*p1]x -I3X3]

   */

    Vector evaluateError(const X &pose,
                         boost::optional<Matrix &> H = boost::none) const
    {
      // const auto &rotation = pose.rotation();
      // if (H)
      // {
      //   *H = skewSymmetric(-u_[0], -u_[1], -u_[2]) * rotation.matrix() * (Matrix36() << skewSymmetric(-p2_[0], -p2_[1], -p2_[2]), I_3x3).finished();
      // H->col(0).setZero();
      // H->col(1).setZero();
      // H->col(5).setZero();
      // }
      // return (pose.transformFrom(p2_) - p1_).cross(u_);

      const auto c1 = pose.rotation().inverse() * u_;
      const auto c2 = pose.inverse().transformFrom(p1_);

      if (H)
      {
        // *H = skewSymmetric(c1[0], c1[1], c1[2]) * (Matrix36() << skewSymmetric(c2[0], c2[1], c2[2]), -I_3x3).finished()
            // + (Matrix36() << skewSymmetric(c1[0], c1[1], c1[2]), Z_3x3).finished();
        //  *H = (Matrix13() << -c1[1], +c1[0], 0.0).finished() * (Matrix36() << skewSymmetric(c2[0], c2[1], c2[2]), -I_3x3).finished() + (Matrix16() << -c1[1], +c1[0], 0.0, 0.0, 0.0, 0.0).finished();
        *H = (Matrix36() << (Matrix13() << -c1[1], +c1[0], 0.0).finished() * (Matrix36() << skewSymmetric(c2[0], c2[1], c2[2]), -I_3x3).finished() + (Matrix16() << -c1[1], +c1[0], 0.0, 0.0, 0.0, 0.0).finished(),
             -(Matrix36() << skewSymmetric(c2[0], c2[1], c2[2]), -I_3x3).finished().topRows(2) * c1[2] +  (p2_ - c2).topRows(2)  *  (Matrix16() << -c1[1], +c1[0], 0.0, 0.0, 0.0, 0.0).finished()).finished();
      }

      // return Vector1((p2_[0] - c2[0]) * c1[1] - (p2_[1] - c2[1]) * c1[0]);
      return (Vector3() << (p2_[0] - c2[0]) * c1[1] - (p2_[1] - c2[1]) * c1[0], (p2_ - c2).topRows(2) * c1[2]).finished();
      // return (p2_ - c2).cross(c1);
    }

    virtual NonlinearFactor::shared_ptr clone() const
    {
      return boost::static_pointer_cast<NonlinearFactor>(
          NonlinearFactor::shared_ptr(new This(*this)));
    }

    virtual bool equals(const NonlinearFactor &expected, double tol = 1e-9) const
    {
      const This *e = dynamic_cast<const This *>(&expected);
      return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(p1_, e->p1_, tol) &&
             traits<Point3>::Equals(p2_, e->p2_, tol) && traits<Point3>::Equals(u_, e->u_, tol);
    }

    virtual void print(const std::string &s = "",
                       const KeyFormatter &keyFormatter = DefaultKeyFormatter) const
    {
      cout << s << ":\nLidarEdgeProjectedFactor1 on (" << keyFormatter(key()) << ")\n"
           << "  Edge Point: " << p1_.transpose() << "\n"
           << "  Edge Axis: " << u_.transpose() << "\n"
           << "  Match Point: " << p2_.transpose() << "\n";
      noiseModel_->print("  noise model: ");
    }

  private:
    Point3 p1_, p2_, u_;

  }; // class LidarEdgeProjectedFactor1

class LidarEdgeProjectedFactor2 : public NoiseModelFactor2<Pose3, Pose3>
  {

    using X = Pose3;
    using Base = NoiseModelFactor2<Pose3, Pose3>;
    using This = LidarEdgeProjectedFactor2;

  public:
    LidarEdgeProjectedFactor2(Key key1, Key key2, const Point3 &point1, const Point3 &unit, const Point3 &point2, const SharedNoiseModel &model)
        : Base(model, key1, key2), p1_(point1), p2_(point2), u_(unit.normalized())
    {
    }

    virtual ~LidarEdgeProjectedFactor2() {}

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    line_err = (p2w-p1).cross(u)
            = (T2*p2 - p1).cross(u)
    dline_err/dT2 = (-u)x * d(T2*p2)/dT2 = (-u)x * R2 * [[-p]x I3X3]
    */

    /*
    d(R*u)/dT =  R*[[-u]x 03x3]
    d(T*p)/dT =  R*[[-p]x I3x3]

    line_err = (p2w-p1w).cross(uw)
            = (T2*p2 - T1*p1).cross(R1*u)
    dline_err/dT1 = (T2*p2 - T1*p1)x * d(R1*u)/dT1 + (R1*u)x * d(T1*p1)/dT1
    dline_err/dT2 = -(R1*u)x * d(T2*p2)/dT2
    */

    /*
    d(T^-1*p)/dT = [[T^-1*p]x -I3X3]
    d(R^-1*u)/dT = [[R^T*u]x   03x3]

    line_err = (T1^-1*T2*p2 - p1).cross(u1)

    point_err = (T1^-1*T2*p2 - p1)[0,1] * u1[2]

    dline_err/dT1 = -[u1]x*d(T1^-1*T2*p2)/dT1
                  = -[u1]x*[[T1^-1*T2*p2]x -I3X3]
    dline_err/dT2 = -[u1]x*d(T1^-1*T2*p2)/dT2
                  = -[u1]x*T1^-1*R2*[[-p2]x I3x3]
                  = -[u1]x*R(T1^-1)*R2*[[-p2]x I3x3]
                  = -[u1]x*R1^-1*R2*[[-p2]x I3x3]
    dpoint_err/dT1 = u1[2]*d(T1^-1*T2*p2)/dT1[0,1]
                   = u1[2]*[[T1^-1*T2*p2]x -I3X3][0,1]
    dpoint_err/dT2 = u1[2]*d(T1^-1*T2*p2)/dT2[0,1]
                   = u1[2]*T1^-1*R2*[[-p2]x I3x3][0,1]
                   = u1[2]*R(T1^-1)*R2*[[-p2]x I3x3][0,1]
                   = u1[2]*R1^-1*R2*[[-p2]x I3x3][0,1]
   */

    Vector evaluateError(const X &pose1, const X &pose2,
                         boost::optional<Matrix &> H1 = boost::none,
                         boost::optional<Matrix &> H2 = boost::none) const
    {
      // const auto ux = skewSymmetric(u_[0], u_[1], u_[2]);
      const auto p2_transformed = (pose1.inverse()*pose2).transformFrom(p2_);

      if (H1)
      {
        // *H1 = -ux * (Matrix36() << skewSymmetric(p2_transformed[0], p2_transformed[1], p2_transformed[2]), -I_3x3).finished();
        // *H1 = -(Matrix13() << -u_[1], u_[0], 0.0).finished() * (Matrix36() << skewSymmetric(p2_transformed[0], p2_transformed[1], p2_transformed[2]), -I_3x3).finished();
        *H1 = (Matrix36() << -(Matrix13() << -u_[1], u_[0], 0.0).finished() * (Matrix36() << skewSymmetric(p2_transformed[0], p2_transformed[1], p2_transformed[2]), -I_3x3).finished(),
                             u_[2] * (Matrix36() << skewSymmetric(p2_transformed[0], p2_transformed[1], p2_transformed[2]), -I_3x3).finished().topRows(2)).finished();
          //  *H1 = u_[2] * (Matrix36() << skewSymmetric(p2_transformed[0], p2_transformed[1], p2_transformed[2]), -I_3x3).finished().topRows(2);

      }
      if (H2)
      {
        // *H2 = -ux * pose1.rotation().inverse().matrix() * pose2.rotation().matrix() * (Matrix36() << skewSymmetric(-p2_[0], -p2_[1], -p2_[2]), I_3x3).finished();
        // *H2 = -(Matrix13() << -u_[1], u_[0], 0.0).finished() * pose1.rotation().inverse().matrix() * pose2.rotation().matrix() * (Matrix36() << skewSymmetric(-p2_[0], -p2_[1], -p2_[2]), I_3x3).finished();
        *H2 = (Matrix36() << -(Matrix13() << -u_[1], u_[0], 0.0).finished() * pose1.rotation().inverse().matrix() * pose2.rotation().matrix() * (Matrix36() << skewSymmetric(-p2_[0], -p2_[1], -p2_[2]), I_3x3).finished(),
              u_[2] * (pose1.rotation().inverse().matrix() * pose2.rotation().matrix() * (Matrix36() << skewSymmetric(-p2_[0], -p2_[1], -p2_[2]), I_3x3).finished()).topRows(2)).finished();
        // *H2 = u_[2] * (pose1.rotation().inverse().matrix() * pose2.rotation().matrix() * (Matrix36() << skewSymmetric(-p2_[0], -p2_[1], -p2_[2]), I_3x3).finished()).topRows(2);

      }

      // return (p2_transformed - p1_).cross(u_);
      // return Vector1((p2_transformed[0] - p1_[0]) * u_[1] - (p2_transformed[1] - p1_[1]) * u_[0]);
      return (Vector3() << (p2_transformed[0] - p1_[0]) * u_[1] - (p2_transformed[1] - p1_[1]) * u_[0],
                           (p2_transformed - p1_).topRows(2)  * u_(2)).finished();
      // return Vector2((p2_transformed - p1_).topRows(2)  * u_(2));

    }

    virtual NonlinearFactor::shared_ptr clone() const
    {
      return boost::static_pointer_cast<NonlinearFactor>(
          NonlinearFactor::shared_ptr(new This(*this)));
    }

    virtual bool equals(const NonlinearFactor &expected, double tol = 1e-9) const
    {
      const This *e = dynamic_cast<const This *>(&expected);
      return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(p1_, e->p1_, tol) &&
             traits<Point3>::Equals(p2_, e->p2_, tol) && traits<Point3>::Equals(u_, e->u_, tol);
    }

    virtual void print(const std::string &s = "",
                       const KeyFormatter &keyFormatter = DefaultKeyFormatter) const
    {
      cout << s << ":\nLidarEdgeProjectedFactor2 on (" << keyFormatter(key1())
           << ", " << keyFormatter(key2()) << ")\n"
           << "  Edge Point: " << p1_.transpose() << "\n"
           << "  Edge Axis: " << u_.transpose() << "\n"
           << "  Match Point: " << p2_.transpose() << "\n";
      noiseModel_->print("  noise model: ");
    }

  private:
    Point3 p1_, p2_, u_;

  }; // class LidarEdgeProjectedFactor2

} // namespace gtsam


#ifndef OpticalElementInterface_hpp
#define OpticalElementInterface_hpp

/** @file OpticalElementInterface.hpp
 * @brief Definition of the optical element interface.
 * @author C.D. Clark III
 * @date 06/29/16
 */

#include <memory>

#include <Eigen/Dense>

#include "../Units.hpp"

/** @class OpticalElementInterface
 * @brief Abstract class that defines the interface an optical element must
 * implement.
 * @author C.D. Clark III
 *
 * An optical element tranforms a Gaussian beam. Before a beam enters the
 * element, it has a beam waist size and position. After it exists the element,
 * it has a new waist size and position. The theory of Gaussian beam propagation
 * based on the paraxial wave equation accounts for the action of an optical
 * element with Ray Transfer Matircies (RTM)
 * (https://en.wikipedia.org/wiki/Ray_transfer_matrix_analysis). These are the
 * same matricies that are used to transform optical rays when ray tracing
 * through an optical system and they are convienent because the action of
 * multiple elements can be accounted for my simply multiplying their RTM
 * together.
 *
 * This method is often referred to as the "ABCD" law for Gaussian beams.
 * Basically, the RTM is a \f$2\times2\f$ matrix that can be written as \f[
 * \left(
 * \begin{matrix}
 * A & B \\
 * C & D
 * \end{matrix}
 * \right)
 * \f]
 * The action of the element is then given by its affect on the complex beam
 * parameter (https://en.wikipedia.org/wiki/Complex_beam_parameter) \f[ q_f =
 * \frac{Aq_i + B}{Cq_i + D} \f]
 *
 * Where $q_i$ and $q_f$ are the complex beam parameters of the beam before and
 * after it passes through the lens. The complex beam parameter itself is
 * defined as \f$q = z + iz_R\f$ where \f$z\f$ is the distance from the beam
 * waist and \f$z_R\f$ is the Rayleigh range (\f$z_R = \frac{\pi
 * \omega_0^2}{\lambda}\f$, where \f$\omega_0\f$ is the beam waist radius
 * (\f$1/e^2\f$) and \f$\lambda\f$ is the wavelength.).
 *
 * One important point to make here is that the usual treatment of this method
 * assumes that the beam waist is always located at \f$z = 0\f$. In other words,
 * the origin of the coordinate system is always taken to be at the beam waist
 * which means that when the beam waist position changes (after it goes through
 * an optical element), the coordinate system changes.
 *
 * This library performs calculations in a fixed coordinate system. The beam
 * waist position is explicitly tracked, and when the beam passes through an
 * optical element, the beam waist position is updated. This introduces a few
 * subtle details that must be handled correctly. Some of these only affect the
 * GaussianBeam class. However, a few also affect optical elements.
 *
 * The optical element class is intended to encapsulate all of the information
 * necessary for a GaussianBeam instance to transform itself. In order to do
 * that, the beam will need the RTM representing the element. No surprise there.
 * However, it is possible for an element to separate two different media (more
 * specifically, two materials with different refractive indices. When this is
 * the case, the wavelength of the beam will change (since \f$\lambda =
 * \frac{\lambda_0}{n}\f$) and this change must be known in order for the beam
 * to update the beam waist size since the imaginary portion of the complex beam
 * parameter contains both the wavelength and beam waist radius. So, an optical
 * element must provide a method that the GaussianBeam instance can use to
 * determine the new wavelength.
 *
 * The second detail that arises from the uses of a fixed coordinate system is
 * the possibility that the z position of the complex beam parameter may shift
 * when it passes through the element. In other words, the z position of the
 * initial parameter may not coincide with the position of the final parameter.
 * When the coordinate system is always centered on the beam waist, this does
 * not matter. The real part of the complex beam parameter gives the distance to
 * the beam waist. However, for a fixed coordinate system, this possible shift
 * needs to be accounted for. The best example of this is a thick lens. The RTM
 * for a thick lens includes the refraction across the front surface,
 * propagation through the lens, and refraction across the back surface. When
 * this matrix is used to transform the complex beam parameter, the initial
 * parameter is evaluated at the front surface of the lens and the new parameter
 * actually corresponds to the position of the back surface of the lens.
 *
 * This shift needs to be known in order for the beam waist position to be
 * determined, so an optical element must provide a method to that the
 * GaussianBeam instance can use to determine the new beam waist position.
 *
 * @tparam LengthUnitType the length unit that will be used for calculating
 * elements of the RTM and position shifts.
 */

template<typename LengthUnitType>
class OpticalElementInterface
{
 public:
  virtual Eigen::Matrix<double, 2, 2> getRTMatrix()
      const = 0;  ///< return the Ray Transfer matrix for the element. NOTE:
                  ///< elements MUST be returned in units of LengthUnitType or
                  ///< inverse LengthUnitType.
  virtual boost::units::quantity<LengthUnitType> getPositionShift()
      const = 0;  ///< return the difference in the position (z coordinate) that
                  ///< the complex beam parameter corresponds to after it passes
                  ///< through the element.
  virtual double getPowerLoss()
      const = 0;  ///< return the power loss (fraction) through the element
  virtual double getWavelengthScaleFactor()
      const = 0;  ///< return the scaling factor through the element. i.e. if
                  ///< the wavelenght changes when light passes through the
                  ///< element.
};

template<typename T, typename U>
class OpticalElementAdapter : public OpticalElementInterface<U>
{
 private:
  T &t;

 public:
  OpticalElementAdapter(T &ref) : t(ref) {}
  Eigen::Matrix<double, 2, 2> getRTMatrix() const { return t.getRTMatrix(); }
  boost::units::quantity<U> getPositionShift() const { return t.getPositionShift(); }
  double      getPowerLoss() const { return t.getPowerLoss(); }
  double      getWavelengthScaleFactor() const
  {
    return t.getWavelengthScaleFactor();
  }
};

template<typename T>
using OpticalElement_ptr = std::shared_ptr<OpticalElementInterface<T> >;

#endif  // include protector


//			Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//	  (See accompanying file LICENSE_1_0.txt or copy at
//			http://www.boost.org/LICENSE_1_0.txt)

#ifndef INTEGRATION_MODIFIED_CHOLESKY_HPP
#define INTEGRATION_MODIFIED_CHOLESKY_HPP

#include <iostream>
#include <boost/noncopyable.hpp>
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <Eigen/Eigenvalues>

// #define DEBUG_MODIFIED_CHOLESKY 1

namespace metro {
	// Utility functions
	namespace {
		template< typename Real >
		Real max( Real const a, Real const b ) {
			return std::max( a, b ) ;
		}

		template< typename Real >
		Real max( Real const a, Real const b, Real const c ) {
			return std::max( a, std::max( b, c )) ;
		}
	}

	//
	// Implements the 'MC' algorithm from Gill & Murray, Practical Optimisation.
	// Also expressed as Algorithm 6.5, "Modified Cholesky algorithm" in
	// Nocedal & Wright, "Numerical Optimisation".
	// See also Fang & Leary 2006
	//
	// I have borrowed a little bit from Eigen's LDLT.h here to get the permutations and types right.
	//
	// This algorithm only touches the lower-diagonal of the matrix.
	// As described in the above references, we use the lower-diagonal of the matrix
	// to store the non-unity entries of L, and the diagonal to store the entries of D.
	// The auxiliary variables c_ij are stored in the lower diagonal too until they
	// are overwritten by entries of L and D.
	//
	// At step j the matrix looks like:
	//
	//	  0 . . j . .
	// 0  d
	// .  l d
	// .  l l d
	// j  c c c c
	// .  c c c a c
	// .  c c c a a c
	//
	// Where a refers to an entry of the original matrix (after the possible permutations) and l refers to an entry of
	// the computed matrix L, d refers to an entry of D, and c to one of the auxiliary c_ijs.
	//
	// At the jth step we update this to become
	// 1: d
	// .  l d
	// .  l l d
	// j: l l l d
	// .  c c c c c
	// .  c c c c a c
	//
	// i.e. we compute the jth row of L, the jth entry of D, and the c's in
	// the jth column and on the diagonal below j.
	template< typename Matrix >
	struct ModifiedCholesky {
	public:
		enum {
		  RowsAtCompileTime = Matrix::RowsAtCompileTime,
		  ColsAtCompileTime = Matrix::ColsAtCompileTime,
		  Options = Matrix::Options & ~Eigen::RowMajorBit, // these are the options for the TmpMatrixType, we need a ColMajor matrix here!
		  MaxRowsAtCompileTime = Matrix::MaxRowsAtCompileTime,
		  MaxColsAtCompileTime = Matrix::MaxColsAtCompileTime,
		  UpLo = Eigen::Lower
		} ;
		typedef typename Matrix::Scalar Scalar;
		typedef typename Eigen::NumTraits<typename Matrix::Scalar>::Real RealScalar;
		typedef typename Matrix::Index Index;

		typedef Eigen::Transpositions<RowsAtCompileTime, MaxRowsAtCompileTime> Transpositions;
		typedef Eigen::PermutationMatrix<RowsAtCompileTime, MaxRowsAtCompileTime> Permutations;
		typedef Eigen::TriangularView< Matrix const, Eigen::UnitLower > const MatrixL ;
		typedef Eigen::Diagonal< Matrix const > Diagonal ;

	public:
		ModifiedCholesky() {}

		ModifiedCholesky( ModifiedCholesky const& other ):
			m_matrix( other.m_matrix ),
			m_transpositions( other.m_transpositions )
		{}

		ModifiedCholesky& operator=( ModifiedCholesky const& other ) {
			m_matrix = other.m_matrix ;
			m_transpositions = other.m_transpositions ;
			return *this ;
		}

		ModifiedCholesky& compute( Matrix const& matrix ) {
			m_matrix = matrix ;
			compute_inplace( m_matrix ) ;
			return *this ;
		}

		MatrixL matrixL() const {
			return m_matrix.template triangularView< Eigen::UnitLower >() ;
		}

		Diagonal vectorD() const {
			return m_matrix.diagonal();
		}

		Permutations const matrixP() const {
			return Permutations( m_transpositions ) ;
		}

		Matrix solve( Matrix const& rhs ) const
		{
			Matrix result = rhs ;
			assert( result.rows() == m_matrix.rows() ) ;
			// result = P rhs
			result = m_transpositions * result ;
			// result = L^-1 (P rhs)
			matrixL().solveInPlace( result );
			// result = D^-1 (L^-1 P rhs)
			for( Index i = 0; i < m_matrix.rows(); ++i ) {
				result.row(i) /= m_matrix(i,i) ;
			}
			// result = L^-T (D^-1 L^-1 P rhs)
			matrixL().transpose().solveInPlace( result ) ;
			// result = P^-1 L^-T (D^-1 L^-1 P rhs)
			result = m_transpositions.transpose() * result ;
			return result ;
		}

		// "Half" solve the rhs
		// solve the rhs, i.e compute
		// S^-1 (rhs)
		// Where S is the 'square root' of the matrix, given by
		// S = P^-1 L sqrt(D)
		// such that
		// S S^t = P^-1
		Matrix halfSolve( Matrix const& rhs ) const
		{
			Matrix result = rhs ;
			assert( result.rows() == m_matrix.rows() ) ;
			// result = P rhs
			result = m_transpositions * result ;
			// result = L^-1 P rhs
			matrixL().solveInPlace( result );
			// result = sqrt(D^-1) L^-1 P rhs
			for( Index i = 0; i < m_matrix.rows(); ++i ) {
				result.row(i) /= std::sqrt( m_matrix(i,i) ) ;
			}
			return result ;
		}

	private:
		Matrix m_matrix ;
		Transpositions m_transpositions;

	private:

		void compute_inplace( Matrix& matrix ) {
			assert( matrix.rows() == matrix.cols() ) ;
			m_transpositions.resize( matrix.rows() ) ;
			Index const size = matrix.rows() ;

#if DEBUG_MODIFIED_CHOLESKY
			std::cerr << "ModifiedCholesky::compute_inplace(): computing with matrix:\n" << matrix << ".\n" ;
#endif

			Scalar biggestOnDiagonal ;
			RealScalar betaSquared ;
			RealScalar delta ;

			for( Index j = 0; j < size; ++j ) {
				// Find largest diagonal element
				Index indexOfBiggestOnDiagonal ;
				biggestOnDiagonal = matrix.diagonal().tail( size - j ).cwiseAbs().maxCoeff( &indexOfBiggestOnDiagonal ) ;
				indexOfBiggestOnDiagonal += j ;

				// Initialise beta and delta if we are starting.
				if( j == 0 ) {
					Scalar biggestOffDiagonal = 0.0 ;
					for( Index i = 0; i < size; ++i ) {
						for( Index j = i+1; j < size; ++j ) {
							biggestOffDiagonal = std::max( biggestOffDiagonal, std::abs( matrix( i, j )) ) ;
						}
					}
					// Had this before:
					//delta = std::numeric_limits< Scalar >::epsilon() * max( biggestOnDiagonal + biggestOffDiagonal, Scalar( 1 ) ) ;
					// ..but most authors e.g. (Fang & Leary 2006) say:
					delta = std::numeric_limits< Scalar >::epsilon() ;
					// Fang & Leary (2006), formula (5).
					betaSquared = max(
						std::numeric_limits< Scalar >::epsilon(),
						biggestOnDiagonal,
						biggestOffDiagonal / std::sqrt( ( size * size ) - 1 )
					) ;

#if DEBUG_MODIFIED_CHOLESKY
					std::cerr << "ModifiedCholesky::compute_inplace(): initialised with delta = "
						<< delta << ", beta^2 = " << betaSquared << ".\n" ;
#endif

					// Modified cholesky of A is cholesky of A+E for some diagonal matrix E
					// Bound on elements of E can be computed as in Fang & Leary (2006)
					//bound = (
					//	std::pow((biggestOffDiagonal/std::sqrt(betaSquared)) + (size-1)*std::sqrt(betaSquared), 2 )
					//	+ 2 * ( biggestOnDiagonal + (size-1)*betaSquared )
					//	+ std::numeric_limits< Scalar >::epsilon()
					//) ;
				}

#if DEBUG_MODIFIED_CHOLESKY
				std::cerr << "ModifiedCholesky::compute_inplace(): at iteration "
					<< j
						<< ": largest entry on diagonal = " << biggestOnDiagonal
					<< " at index " << indexOfBiggestOnDiagonal << ".\n" ;
#endif

				// Swap rows and columns corresponding to the jth and largest diagonal element.
				m_transpositions.coeffRef( j ) = indexOfBiggestOnDiagonal ;
				if( j != indexOfBiggestOnDiagonal ) {
					// indexOfbiggestOnDiagonal is always >= j by construction
					// we only touch the lower triangular part of the matrix.
					Index const tailSize = size - indexOfBiggestOnDiagonal - 1 ;
					matrix.row( j ).head( j ).swap( matrix.row( indexOfBiggestOnDiagonal ).head( j ) ) ;
					matrix.col( j ).tail( tailSize ).swap( matrix.col( indexOfBiggestOnDiagonal ).tail( tailSize ) ) ;
					std::swap( matrix.coeffRef(j,j), matrix.coeffRef( indexOfBiggestOnDiagonal, indexOfBiggestOnDiagonal ) );
					for( int i = j+1; i < indexOfBiggestOnDiagonal; ++i ) {
						std::swap( matrix.coeffRef( i, j ), matrix.coeffRef( indexOfBiggestOnDiagonal, i ) ) ;
					}
				}

				Index const tailSize = size - j - 1 ;
				// We first compute the jth row of L to produce:
				// 1: d
				// .  l d
				// .  l l d
				// j: l l l c
				// .  c c c a c
				// .  c c c a a c
				//
				// by formula: l_js = c_js / d_s for s = 0,...,j-1.
				if( j > 0 ) {
					matrix.row(j).head( j ).array() /= matrix.diagonal().head( j ).array() ;
				}
#if DEBUG_MODIFIED_CHOLESKY
				std::cerr << "ModifiedCholesky::compute_inplace(): after computing ls, matrix =\n"
					<< matrix << ".\n" ;
#endif
				// We next compute jth column of c_ijs to get:
				// 1: d
				// .  l d
				// .  l l d
				// j: l l l c
				// .  c c c c c
				// .  c c c c a c
				//
				// by formula c_ij = a_ij - sum_s l_ks c_is for s=j+1...n
				for( Index i = j+1; i < size; ++i ) {
					matrix(i,j) -= ( matrix.row( j ).head( j ) * matrix.row( i ).head( j ).transpose() ) ;
				}
				Scalar const theta = ( (j+1) == size ) ? 0.0 : ( matrix.col( j ).tail( tailSize ).maxCoeff() ) ;
#if DEBUG_MODIFIED_CHOLESKY
				std::cerr << "ModifiedCholesky::compute_inplace(): after computing cs, matrix =\n"
					<< matrix << ",\n"
					<< "theta = " << theta << ".\n" ;
#endif

				// compute d_jj to produce
				// 1: d
				// .  l d
				// .  l l d
				// j: l l l d
				// .  c c c c c
				// .  c c c c a c
				double new_dj = max(
					delta,
					std::abs( matrix(j,j) ),
					(theta*theta) / betaSquared
				) ;
				matrix(j,j) = new_dj ;
				//
				// Finally update the c_ii's
				matrix.diagonal().tail( tailSize ).array() -= ( matrix.col(j).tail( tailSize ).array().square() ) / matrix(j,j) ;

#if DEBUG_MODIFIED_CHOLESKY
				std::cerr << "ModifiedCholesky::compute_inplace(): after iteration " << j << ", matrix is:\n"
					<< matrix << ".\n" ;
#endif
			}
		}
	} ;
}

#endif


/* mexcpp_test.cpp -- Test mexcpp library
 *
 * Copyright (c) 2013-5 Kui Tang <kuitang@gmail.com>
 * License: MIT
 *
 * Inspired by Rcpp and nr3matlab.h
 *
 * Run this line in MATLAB:
 * [od, os, ocm, osm] = mexcpp_test(3, 'c', 'string', [1 2 3], single([1 2; 3 4]), {[1 2], [1 2; 3 4]}, struct('f0', [10 20; 30 40], 'f1', 52, 'f2', int32(99), 'f3', 'blahblah'))
 */

#include "mexcpp.h"
#include <string>

#ifdef HAVE_EIGEN
#include <Eigen/Dense>
#include <sstream>
#endif

enum {
  iDouble,
  iChar,
  iStr,
  iDoubleVector,
  iSingleMatrix,
  iCellMat,
  iStructMat,
  nI
};

enum {
  oDoubleMatrix,
  oString,
  oCellMat,
  oStructMat,
  nO
};

using namespace mexcpp;

// test pointer overloading
void printPointerArray(int n, double *p) {
  for (int i = 0; i < n; i++) {
    mexPrintf("printPointerArray: p[%d] = %g\n", i, p[i]);
  }
}

void mexFunction(int nOut, mxArray *pOut[], int nIn, const mxArray *pIn[]) {
  if (nIn != nI || nOut != nO) {
    mexErrMsgIdAndTxt("mexcpp_test:nIn", "Usage: mexcpp_test(double, char, doubleVec, singleMat, cellMat, structMat)", nI);
  }

  double d = scalar<double>(pIn[iDouble]);
  mexPrintf("Double scalar read; d = %g\n", d);

  char c = scalar<char>(pIn[iChar]);
  mexPrintf("Char scalar read; c = %c\n", c);

  std::string s = scalar<std::string>(pIn[iStr]);
  mexPrintf("String scalar read; s = %s\n", s.c_str());

  Mat<double> dv(pIn[iDoubleVector]);
  mexPrintf("Double vector read; N = %d, M = %d, end = %g\n", dv.N, dv.M, dv[dv.length - 1]);

  printPointerArray(dv.length, dv);

  Mat<float> m(pIn[iSingleMatrix]);
  mexPrintf("Single matrix read; N = %d, M = %d, end = %g\n", m.N, m.M, m(m.N - 1, m.M - 1));

#ifdef HAVE_EIGEN
  std::stringstream ss;
  const Mat<float>::EigenMap mEigenMap(m.asEigenMap());
  ss << "mEigenMap contents: \n" << mEigenMap << "\n";
  mexPrintf(ss.str().c_str());
  ss.str("");

  Mat<float>::EigenMatrix mEigenMat(m.asEigenMatrix());
  ss << "mEigenMatrix contents: \n" << mEigenMap << "\n";
  mexPrintf(ss.str().c_str());
  ss.str() = "";

#endif

  CellMat<Mat<double> > cm(pIn[iCellMat]);
  mexPrintf("Cell matrix read; N = %d, M = %d\n", cm.N, cm.M);

  for (int i = 0; i < cm.length; i++) {
    mexPrintf("Entry %d was %d x %d matrix\n", i, cm[i].N, cm[i].M);
  }

  StructMat sm(pIn[iStructMat]);
  mexPrintf("Structure matrix read; N = %d, M = %d, nFields = %d\n", sm.N, sm.M, sm.nFields);
  for (int i = 0; i < sm.length; i++) {
    Mat<double> f0 = sm[i].get<Mat<double> >("f0");
    double      f1 = sm[i].getS<double>("f1");
    int32_t     f2 = sm[i].getS<int32_t>("f2");
    std::string f3 = sm[i].getS<std::string>("f3");
    mexPrintf("Entry %d had f0 = [%d x %d] f1 = %g f2 = %d f3 = %s\n", i, f0.N, f0.M, f1, f2, f3.c_str());
  }

  pOut[oString] = scalar<std::string>("String output");

  // Making stuff
  Mat<double> om(2,2);
  om(0,0) = 1;
  om(0,1) = 2;
  om(1,0) = 3;
  om(1,1) = 4;
  pOut[oDoubleMatrix] = om;
  mexPrintf("Double matrix output created.\n");

#ifdef HAVE_EIGEN
  Mat<double>::EigenMap omEigenMap(om.asEigenMap());
  omEigenMap(0,0) += 0.1;
  mexPrintf("Created (nonconst) EigenMap on output matrix and manipulated it.");
#endif

  CellMat<Mat<double> > ocm(1,2);
  ocm.setS(0, 5.1);
  ocm.set(1, Mat<double>(3, 3));
  pOut[oCellMat] = ocm;
  mexPrintf("Cell matrix output created.\n");
  pOut[oCellMat] = ocm;

  // No C++11 features
  std::vector<std::string> fns;
  fns.push_back("bar");
  fns.push_back("quuz");
  StructMat osm(2, 1, fns);
  osm[0].set("bar", Mat<double>(3,3));
  osm[0].setS("quuz", 1);
  osm[1].set(0, Mat<double>(2,2));
  osm[1].setS(1, 2);
  pOut[oStructMat] = osm;
  mexPrintf("Struct matrix output created.\n");

  mexPrintf("All done!\n");
}


// Copyright (C) 2006  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/matrix.h>
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <vector>
#include "../stl_checked.h"
#include "../array.h"
#include "../rand.h"

#include "tester.h"
#include <dlib/memory_manager_stateless.h>
#include <dlib/array2d.h>

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.matrix2");

    dlib::rand rnd;

    void matrix_test (
    )
    /*!
        ensures
            - runs tests on the matrix stuff compliance with the specs
    !*/
    {
        typedef memory_manager_stateless<char>::kernel_2_2a MM;
        print_spinner();

        const double ident[] = {
            1, 0, 0, 0,
            0, 1, 0, 0,
            0, 0, 1, 0,
            0, 0, 0, 1 };

        const double uniform3[] = {
            3, 3, 3, 3,
            3, 3, 3, 3,
            3, 3, 3, 3,
            3, 3, 3, 3
        };

        const double uniform1[] = {
            1, 1, 1, 1,
            1, 1, 1, 1,
            1, 1, 1, 1,
            1, 1, 1, 1
        };

        const double uniform0[] = {
            0, 0, 0, 0,
            0, 0, 0, 0,
            0, 0, 0, 0,
            0, 0, 0, 0
        };

        const int array[] = {
            42, 58, 9, 1,
            9, 5, 8, 2,
            98, 28, 4, 77,
            9, 2, 44, 88 };

        const int array2[] = {
            1, 22, 3,
            4, 52, 6,
            7, 8, 9 };

        const int array2_r[] = {
            52, 6, 4,
            8, 9, 7,
            22, 3, 1
        };

        const double array_f[] = {
            -0.99,
            0.99};

        matrix<double,2,1,MM> fm(array_f);

        DLIB_TEST(fm.size() == 2);
        matrix<double> dfm(fm);
        DLIB_TEST(round(fm)(0) == -1);
        DLIB_TEST(round(fm)(1) == 1);
        DLIB_TEST(round(dfm)(0) == -1);
        DLIB_TEST(round(dfm)(1) == 1);
        DLIB_TEST(round(dfm).size() == dfm.size());

        const int array3[] = { 1, 2, 3, 4 };

        matrix<double,3,3,MM> m3(array2);
        matrix<double> dm3;
        DLIB_TEST(dm3.size() == 0);
        DLIB_TEST(dm3.nr() == 0);
        DLIB_TEST(dm3.nc() == 0);
        dm3.set_size(3,4);
        DLIB_TEST(dm3.nr() == 3);
        DLIB_TEST(dm3.nc() == 4);
        DLIB_TEST(dm3.size() == 3*4);
        dm3.set_size(3,3);
        DLIB_TEST(dm3.nr() == 3);
        DLIB_TEST(dm3.nc() == 3);
        dm3 = m3;
        dm3(0,0)++;
        DLIB_TEST( dm3 != m3);
        dm3 = m3;
        DLIB_TEST( dm3 == m3);
        DLIB_TEST( abs(sum(squared(normalize(dm3))) - 1.0) < 1e-10);

        matrix<double,3,4> mrc;
        mrc.set_size(3,4);

        set_all_elements(mrc,1);

        DLIB_TEST(diag(mrc) == uniform_matrix<double>(3,1,1));
        DLIB_TEST(diag(matrix<double>(mrc)) == uniform_matrix<double>(3,1,1));

        matrix<double,2,3> mrc2;
        set_all_elements(mrc2,1);
        DLIB_TEST((removerc<1,1>(mrc) == mrc2));
        DLIB_TEST((removerc(mrc,1,1) == mrc2));

        matrix<int,3,3> m4, m5, m6;
        set_all_elements(m4, 4);
        set_all_elements(m5, 4);
        set_all_elements(m6, 1);

        DLIB_TEST(squared(m4) == pointwise_multiply(m4,m4));
        DLIB_TEST(cubed(m4) == pointwise_multiply(m4,m4,m4));
        DLIB_TEST(pow(matrix_cast<double>(m4),2) == squared(matrix_cast<double>(m4)));
        DLIB_TEST(pow(matrix_cast<double>(m4),3) == cubed(matrix_cast<double>(m4)));

        matrix<int> dm4;
        matrix<int,0,0,memory_manager_stateless<char>::kernel_2_2a> dm5;
        dm4 = dm4;
        dm4 = dm5;
        DLIB_TEST(dm4.nr() == 0);
        dm4 = m4;
        dm5 = m5;
        DLIB_TEST(dm4 == dm5);

        DLIB_TEST(m4 == m5);
        DLIB_TEST(m6 != m5);
        m4.swap(m6);
        DLIB_TEST(m6 == m5);
        DLIB_TEST(m4 != m5);

        DLIB_TEST(m3.nr() == 3);
        DLIB_TEST(m3.nc() == 3);

        matrix<double,4,1> v(array3), v2;
        DLIB_TEST(v.nr() == 4);
        DLIB_TEST(v.nc() == 1);

        std::vector<double> stdv(4);
        std_vector_c<double> stdv_c(4);
        dlib::array<double> arr;
        arr.resize(4);
        for (long i = 0; i < 4; ++i)
            stdv[i] = stdv_c[i] = arr[i] = i+1;

        DLIB_TEST(mat(stdv)(0) == 1);
        DLIB_TEST(mat(stdv)(1) == 2);
        DLIB_TEST(mat(stdv)(2) == 3);
        DLIB_TEST(mat(stdv)(3) == 4);
        DLIB_TEST(mat(stdv).nr() == 4);
        DLIB_TEST(mat(stdv).nc() == 1);
        DLIB_TEST(mat(stdv).size() == 4);
        DLIB_TEST(equal(trans(mat(stdv))*mat(stdv), trans(v)*v));
        DLIB_TEST(equal(trans(mat(stdv))*mat(stdv), tmp(trans(v)*v)));

        DLIB_TEST(mat(stdv_c)(0) == 1);
        DLIB_TEST(mat(stdv_c)(1) == 2);
        DLIB_TEST(mat(stdv_c)(2) == 3);
        DLIB_TEST(mat(stdv_c)(3) == 4);
        DLIB_TEST(mat(stdv_c).nr() == 4);
        DLIB_TEST(mat(stdv_c).nc() == 1);
        DLIB_TEST(mat(stdv_c).size() == 4);
        DLIB_TEST(equal(trans(mat(stdv_c))*mat(stdv_c), trans(v)*v));

        DLIB_TEST(mat(arr)(0) == 1);
        DLIB_TEST(mat(arr)(1) == 2);
        DLIB_TEST(mat(arr)(2) == 3);
        DLIB_TEST(mat(arr)(3) == 4);
        DLIB_TEST(mat(arr).nr() == 4);
        DLIB_TEST(mat(arr).nc() == 1);
        DLIB_TEST(mat(arr).size() == 4);
        DLIB_TEST(equal(trans(mat(arr))*mat(arr), trans(v)*v));

        DLIB_TEST(v(0) == 1);
        DLIB_TEST(v(1) == 2);
        DLIB_TEST(v(2) == 3);
        DLIB_TEST(v(3) == 4);
        matrix<double> dv = v;
        DLIB_TEST((trans(v)*v).size() == 1);
        DLIB_TEST((trans(v)*v).nr() == 1);
        DLIB_TEST((trans(v)*dv).nr() == 1);
        DLIB_TEST((trans(dv)*dv).nr() == 1);
        DLIB_TEST((trans(dv)*v).nr() == 1);
        DLIB_TEST((trans(v)*v).nc() == 1);
        DLIB_TEST((trans(v)*dv).nc() == 1);
        DLIB_TEST((trans(dv)*dv).nc() == 1);
        DLIB_TEST((trans(dv)*v).nc() == 1);
        DLIB_TEST((trans(v)*v)(0) == 1*1 + 2*2 + 3*3 + 4*4);
        DLIB_TEST((trans(dv)*v)(0) == 1*1 + 2*2 + 3*3 + 4*4);
        DLIB_TEST((trans(dv)*dv)(0) == 1*1 + 2*2 + 3*3 + 4*4);
        DLIB_TEST((trans(v)*dv)(0) == 1*1 + 2*2 + 3*3 + 4*4);

        dv = trans(dv)*v;
        DLIB_TEST(dv.nr() == 1);
        DLIB_TEST(dv.nc() == 1);

        dm3 = m3;
        DLIB_TEST(floor(det(m3)+0.01) == -444);
        DLIB_TEST(floor(det(dm3)+0.01) == -444);
        DLIB_TEST(min(m3) == 1);
        DLIB_TEST(min(dm3) == 1);
        DLIB_TEST(max(m3) == 52);
        DLIB_TEST(max(dm3) == 52);
        DLIB_TEST(sum(m3) == 112);
        DLIB_TEST(sum(dm3) == 112);
        DLIB_TEST(prod(m3) == 41513472);
        DLIB_TEST(prod(dm3) == 41513472);
        DLIB_TEST(prod(diag(m3)) == 1*52*9);
        DLIB_TEST(prod(diag(dm3)) == 1*52*9);
        DLIB_TEST(sum(diag(m3)) == 1+52+9);
        DLIB_TEST(sum(diag(dm3)) == 1+52+9);
        DLIB_TEST(equal(round(10000*m3*inv(m3))/10000 , identity_matrix<double,3>()));
        DLIB_TEST(equal(round(10000*dm3*inv(m3))/10000 , identity_matrix<double,3>()));
        DLIB_TEST(equal(round(10000*dm3*inv(dm3))/10000 , identity_matrix<double,3>()));
        DLIB_TEST(equal(round(10000*m3*inv(dm3))/10000 , identity_matrix<double,3>()));
        DLIB_TEST(equal(round(10000*tmp(m3*inv(m3)))/10000 , identity_matrix<double,3>()));
        DLIB_TEST(equal(round(10000*tmp(dm3*inv(m3)))/10000 , identity_matrix<double,3>()));
        DLIB_TEST(equal(round(10000*tmp(dm3*inv(dm3)))/10000 , identity_matrix<double,3>()));
        DLIB_TEST(equal(round(10000*tmp(m3*inv(dm3)))/10000 , identity_matrix<double,3>()));
        DLIB_TEST(-1*m3 == -m3);
        DLIB_TEST(-1*dm3 == -m3);
        DLIB_TEST(-1*m3 == -dm3);
        DLIB_TEST(-1*dm3 == -dm3);

        DLIB_TEST(m3 == dm3);
        m3(1,1) = 99;
        DLIB_TEST(m3 != dm3);
        m3 = dm3;
        DLIB_TEST(m3 == dm3);

        matrix<double,4,4,MM> mident(ident);
        matrix<double,4,4> muniform0(uniform0);
        matrix<double,4,4> muniform1(uniform1);
        matrix<double,4,4> muniform3(uniform3);
        matrix<double,4,4> m1(array), m2;
        DLIB_TEST(m1.nr() == 4);
        DLIB_TEST(m1.nc() == 4);

        DLIB_TEST(muniform1 + muniform1 + muniform1 == muniform3);
        DLIB_TEST(muniform1*2 + muniform1 + muniform1 - muniform1 == muniform3);
        DLIB_TEST(2*muniform1 + muniform1 + muniform1 - muniform1 == muniform3);
        DLIB_TEST(muniform1 + muniform1 + muniform1 - muniform3 == muniform0);
        DLIB_TEST(equal(muniform3/3 , muniform1));
        DLIB_TEST(v != m1);
        DLIB_TEST(v == v);
        DLIB_TEST(m1 == m1);

        muniform0.swap(muniform1);
        DLIB_TEST((muniform1 == matrix_cast<double>(uniform_matrix<long,4,4,0>())));
        DLIB_TEST((muniform0 == matrix_cast<double>(uniform_matrix<long,4,4,1>())));
        DLIB_TEST((muniform1 == matrix_cast<double>(uniform_matrix<long>(4,4,0))));
        DLIB_TEST((muniform0 == matrix_cast<double>(uniform_matrix<long>(4,4,1))));
        swap(muniform0,muniform1);

        DLIB_TEST((mident == identity_matrix<double,4>()));
        DLIB_TEST((muniform0 == matrix_cast<double>(uniform_matrix<long,4,4,0>())));
        DLIB_TEST((muniform1 == matrix_cast<double>(uniform_matrix<long,4,4,1>())));
        DLIB_TEST((muniform3 == matrix_cast<double>(uniform_matrix<long,4,4,3>())));
        DLIB_TEST((muniform1*8 == matrix_cast<double>(uniform_matrix<long,4,4,8>())));

        set_all_elements(m2,7);
        DLIB_TEST(m2 == muniform1*7);
        m2 = array;
        DLIB_TEST(m2 == m1);

        const double m1inv[] = {
            -0.00946427624, 0.0593272941,  0.00970564379,  -0.00973323731,
            0.0249312057,   -0.0590122427, -0.00583102756, 0.00616002729,
            -0.00575431149, 0.110081189,   -0.00806792253, 0.00462297692,
            0.00327847478,  -0.0597669712, 0.00317386196,  0.00990759201
        };

        m2 = m1inv;
        DLIB_TEST((round(m2*m1) == identity_matrix<double,4>()));
        DLIB_TEST((round(tmp(m2*m1)) == identity_matrix<double,4>()));

        DLIB_TEST_MSG(round(m2*10000) == round(inv(m1)*10000),
                     round(m2*10000) - round(inv(m1)*10000)
                     << "\n\n" << round(m2*10000)
                     << "\n\n" << round(inv(m1)*10000)
                     << "\n\n" << m2
                     << "\n\n" << inv(m1)
                     );
        DLIB_TEST(m1 == abs(-1*m1));
        DLIB_TEST(abs(m2) == abs(-1*m2));

        DLIB_TEST_MSG(floor(det(m1)+0.01) == 3297875,"\nm1: \n" << m1 << "\ndet(m1): " << det(m1));

        ostringstream sout;
        m1 = m2;
        serialize(m1,sout);
        set_all_elements(m1,0);
        istringstream sin(sout.str());
        deserialize(m1,sin);
        DLIB_TEST_MSG(round(100000*m1) == round(100000*m2),"m1: \n" << m1 << endl << "m2: \n" << m2);

        set_all_elements(v,2);
        v2 =  pointwise_multiply(v, v*2);
        set_all_elements(v,8);
        DLIB_TEST(v == v2);
        DLIB_TEST(v == tmp(v2));
        DLIB_TEST((v == rotate<2,0>(v)));

        m4 = array2;
        m5 = array2_r;
        DLIB_TEST((m5 == rotate<1,1>(m4)));

        m5 = array2;
        DLIB_TEST((m5*2 == pointwise_multiply(m5,uniform_matrix<int,3,3,2>())));
        DLIB_TEST((tmp(m5*2) == tmp(pointwise_multiply(m5,uniform_matrix<int,3,3,2>()))));

        v = tmp(v);

        matrix<double> dm10(10,5);
        DLIB_TEST(dm10.nr() == 10);
        DLIB_TEST(dm10.nc() == 5);
        set_all_elements(dm10,4);
        DLIB_TEST(dm10.nr() == 10);
        DLIB_TEST(dm10.nc() == 5);
        matrix<double,10,5> m10;
        DLIB_TEST(m10.nr() == 10);
        DLIB_TEST(m10.nc() == 5);
        set_all_elements(m10,4);
        DLIB_TEST(dm10 == m10);
        DLIB_TEST((clamp<0,3>(dm10) == clamp<0,3>(m10)));
        DLIB_TEST((clamp<0,3>(dm10)(0,2) == 3));

        set_all_elements(dm10,1);
        set_all_elements(m10,4);
        DLIB_TEST(4*dm10 == m10);
        DLIB_TEST(5*dm10 - dm10 == m10);
        DLIB_TEST((16*dm10)/4 == m10);
        DLIB_TEST(dm10+dm10+2*dm10 == m10);
        DLIB_TEST(dm10+tmp(dm10+2*dm10) == m10);
        set_all_elements(dm10,4);
        DLIB_TEST(dm10 == m10);
        DLIB_TEST_MSG(sum(abs(sigmoid(dm10) -sigmoid(m10))) < 1e-10,sum(abs(sigmoid(dm10) -sigmoid(m10))) );

        {
            matrix<double,2,1> x, l, u, out;
            x = 3,4;

            l = 1,1;
            u = 2,2.2;

            out = 2, 2.2;
            DLIB_TEST(equal(clamp(x, l, u) , out));
            out = 3, 2.2;
            DLIB_TEST(!equal(clamp(x, l, u) , out));
            out = 2, 4.2;
            DLIB_TEST(!equal(clamp(x, l, u) , out));

            x = 1.5, 1.5;
            out = x;
            DLIB_TEST(equal(clamp(x, l, u) , out));

            x = 0.5, 1.5;
            out = 1, 1.5;
            DLIB_TEST(equal(clamp(x, l, u) , out));

            x = 1.5, 0.5;
            out = 1.5, 1.0;
            DLIB_TEST(equal(clamp(x, l, u) , out));

        }

        matrix<double, 7, 7,MM,column_major_layout> m7;
        matrix<double> dm7(7,7);
        dm7 = randm(7,7, rnd);
        m7 = dm7;

        DLIB_TEST_MSG(max(abs(dm7*inv(dm7) - identity_matrix<double>(7))) < 1e-12, max(abs(dm7*inv(dm7) - identity_matrix<double>(7))));
        DLIB_TEST(equal(inv(dm7),  inv(m7)));
        DLIB_TEST(abs(det(dm7) - det(m7)) < 1e-14);
        DLIB_TEST(abs(min(dm7) - min(m7)) < 1e-14);
        DLIB_TEST(abs(max(dm7) - max(m7)) < 1e-14);
        DLIB_TEST_MSG(abs(sum(dm7) - sum(m7)) < 1e-14,sum(dm7) - sum(m7));
        DLIB_TEST(abs(prod(dm7) -prod(m7)) < 1e-14);
        DLIB_TEST(equal(diag(dm7) , diag(m7)));
        DLIB_TEST(equal(trans(dm7) , trans(m7)));
        DLIB_TEST(equal(abs(dm7) , abs(m7)));
        DLIB_TEST(equal(round(dm7) , round(m7)));
        DLIB_TEST(matrix_cast<int>(dm7) == matrix_cast<int>(m7));
        DLIB_TEST((rotate<2,3>(dm7) == rotate<2,3>(m7)));
        DLIB_TEST((sum(pointwise_multiply(dm7,dm7) - pointwise_multiply(m7,m7))) < 1e-10);
        DLIB_TEST((sum(pointwise_multiply(dm7,dm7,dm7) - pointwise_multiply(m7,m7,m7))) < 1e-10);
        DLIB_TEST_MSG((sum(pointwise_multiply(dm7,dm7,dm7,dm7) - pointwise_multiply(m7,m7,m7,m7))) < 1e-10,
                     (sum(pointwise_multiply(dm7,dm7,dm7,dm7) - pointwise_multiply(m7,m7,m7,m7)))
        );

        matrix<double> temp(5,5);
        matrix<double> dsm(5,5);
        matrix<double,5,5,MM> sm;

        set_all_elements(dsm,1);
        set_all_elements(sm,1);
        set_all_elements(temp,1);

        dsm += dsm;
        sm += sm;
        DLIB_TEST(dsm == 2*temp);
        DLIB_TEST(sm == 2*temp);
        temp = dsm*sm + dsm;
        dsm += dsm*sm;
        DLIB_TEST_MSG(temp == dsm,temp - dsm);

        set_all_elements(dsm,1);
        set_all_elements(sm,1);
        set_all_elements(temp,1);

        dsm += dsm;
        sm += sm;
        DLIB_TEST(dsm == 2*temp);
        DLIB_TEST(sm == 2*temp);
        temp = dsm*sm + dsm;
        sm += dsm*sm;
        DLIB_TEST_MSG(temp == sm,temp - sm);

        set_all_elements(dsm,1);
        set_all_elements(sm,1);
        set_all_elements(temp,1);

        dsm += dsm;
        sm += sm;
        DLIB_TEST(dsm == 2*temp);
        DLIB_TEST(sm == 2*temp);
        temp = sm - dsm*sm ;
        sm -= dsm*sm;
        DLIB_TEST_MSG(temp == sm,temp - sm);

        set_all_elements(dsm,1);
        set_all_elements(sm,1);
        set_all_elements(temp,1);

        dsm += dsm;
        sm += sm;
        DLIB_TEST(dsm == 2*temp);
        DLIB_TEST(sm == 2*temp);
        temp = dsm - dsm*sm ;
        dsm -= dsm*sm;
        DLIB_TEST_MSG(temp == dsm,temp - dsm);

        set_all_elements(dsm,1);
        set_all_elements(sm,1);
        set_all_elements(temp,2);

        dsm *= 2;
        sm *= 2;
        DLIB_TEST(dsm == temp);
        DLIB_TEST(sm == temp);
        dsm /= 2;
        sm /= 2;
        DLIB_TEST(dsm == temp/2);
        DLIB_TEST(sm == temp/2);

        dsm += dsm;
        sm += sm;
        DLIB_TEST(dsm == temp);
        DLIB_TEST(sm == temp);
        dsm += sm;
        sm += dsm;
        DLIB_TEST(dsm == 2*temp);
        DLIB_TEST(sm == temp*3);
        dsm -= sm;
        sm -= dsm;
        DLIB_TEST(dsm == -temp);
        DLIB_TEST(sm == 4*temp);
        sm -= sm;
        dsm -= dsm;
        DLIB_TEST(dsm == 0*temp);
        DLIB_TEST(sm == 0*temp);

        set_all_elements(dsm,1);
        set_all_elements(sm,1);
        set_all_elements(temp,3);
        dsm += sm+sm;
        DLIB_TEST(dsm == temp);

        set_all_elements(dsm,1);
        set_all_elements(sm,1);
        set_all_elements(temp,-1);
        dsm -= sm+sm;
        DLIB_TEST(dsm == temp);

        set_all_elements(dsm,1);
        set_all_elements(sm,1);
        set_all_elements(temp,-1);
        sm -= dsm+dsm;
        DLIB_TEST(sm == temp);

        set_all_elements(dsm,1);
        set_all_elements(sm,1);
        set_all_elements(temp,3);
        sm += dsm+dsm;
        DLIB_TEST(sm == temp);

        // test the implicit conversion to bool stuff
        {
            matrix<float> bt1(3,1);
            matrix<float,3,1> bt2;
            set_all_elements(bt1,2);
            set_all_elements(bt2,3);

			float val = trans(bt1)*bt2;
            DLIB_TEST((float)(trans(bt1)*bt2) == 18);
            DLIB_TEST((float)(trans(bt1)*bt2) != 19);
			DLIB_TEST(val == 18);
        }
        {
            matrix<float,3,1> bt1;
            matrix<float> bt2(3,1);
            set_all_elements(bt1,2);
            set_all_elements(bt2,3);

			float val = trans(bt1)*bt2;
            DLIB_TEST((float)(trans(bt1)*bt2) == 18);
            DLIB_TEST((float)(trans(bt1)*bt2) != 19);
			DLIB_TEST(val == 18);
        }
        {
            matrix<float> bt1(3,1);
            matrix<float> bt2(3,1);
            set_all_elements(bt1,2);
            set_all_elements(bt2,3);

			float val = trans(bt1)*bt2;
            DLIB_TEST((float)(trans(bt1)*bt2) == 18);
            DLIB_TEST((float)(trans(bt1)*bt2) != 19);
			DLIB_TEST(val == 18);
        }
        {
            matrix<float,3,1> bt1;
            matrix<float,3,1> bt2;
            set_all_elements(bt1,2);
            set_all_elements(bt2,3);

			float val = trans(bt1)*bt2;
            DLIB_TEST((float)(trans(bt1)*bt2) == 18);
            DLIB_TEST((float)(trans(bt1)*bt2) != 19);
			DLIB_TEST(val == 18);
        }

        {
            srand(423452);
            const long M = 50;
            const long N = 40;

            matrix<double> a(M,N);
            for (long r = 0; r < a.nr(); ++r)
            {
                for (long c = 0; c < a.nc(); ++c)
                {
                    a(r,c) = 10*((double)::rand())/RAND_MAX;
                }
            }

            matrix<double> u, u2;
            matrix<double> q, q2;
            matrix<double> v, v2;

            matrix<double> a2;
            a2 = tmp(a/2);

            svd2(true,true,a2+a2,u,q,v);

            double err = max(abs(a - subm(u,get_rect(a2+a2))*diagm(q)*trans(v)));
            DLIB_TEST_MSG( err < 1e-11,"err: " << err);
            using dlib::equal;
            DLIB_TEST((equal(trans(u)*u , identity_matrix<double,M>(), 1e-10)));
            DLIB_TEST((equal(trans(v)*v , identity_matrix<double,N>(), 1e-10)));

            svd2(false,true,a2+a2,u,q,v2);
            svd2(true,false,a2+a2,u2,q,v);
            svd2(false,false,a2+a2,u,q2,v);

            err = max(abs(a - subm(u2,get_rect(a2+a2))*diagm(q2)*trans(v2)));
            DLIB_TEST_MSG( err < 1e-11,"err: " << err);
            DLIB_TEST((equal(trans(u2)*u2 , identity_matrix<double,M>(), 1e-10)));
            DLIB_TEST((equal(trans(v2)*v2 , identity_matrix<double,N>(), 1e-10)));

        }

        {
            srand(423452);
            const long M = 3;
            const long N = 3;

            matrix<double> a(M,N);
            for (long r = 0; r < a.nr(); ++r)
            {
                for (long c = 0; c < a.nc(); ++c)
                {
                    a(r,c) = 10*((double)::rand())/RAND_MAX;
                }
            }

            matrix<double,M,M> u, u2;
            matrix<double> q, q2;
            matrix<double,N,N> v, v2;

            matrix<double,M,N,MM> a2;
            a2 = tmp(a/2);

            svd2(true,true,a2+a2,u,q,v);

            double err = max(abs(a - subm(u,get_rect(a2+a2))*diagm(q)*trans(v)));
            DLIB_TEST_MSG( err < 1e-11,"err: " << err);
            using dlib::equal;
            DLIB_TEST((equal(trans(u)*u , identity_matrix<double,M>(), 1e-10)));
            DLIB_TEST((equal(trans(v)*v , identity_matrix<double,N>(), 1e-10)));

            svd2(false,true,a2+a2,u,q,v2);
            svd2(true,false,a2+a2,u2,q,v);
            svd2(false,false,a2+a2,u,q2,v);

            err = max(abs(a - subm(u2,get_rect(a2+a2))*diagm(q2)*trans(v2)));
            DLIB_TEST_MSG( err < 1e-11,"err: " << err);
            DLIB_TEST((equal(trans(u2)*u2 , identity_matrix<double,M>(), 1e-10)));
            DLIB_TEST((equal(trans(v2)*v2 , identity_matrix<double,N>(), 1e-10)));

        }

        {
            srand(423452);
            const long M = 3;
            const long N = 3;

            matrix<double,0,0,default_memory_manager, column_major_layout> a(M,N);
            for (long r = 0; r < a.nr(); ++r)
            {
                for (long c = 0; c < a.nc(); ++c)
                {
                    a(r,c) = 10*((double)::rand())/RAND_MAX;
                }
            }

            matrix<double,M,M,default_memory_manager, column_major_layout> u, u2;
            matrix<double,0,0,default_memory_manager, column_major_layout> q, q2;
            matrix<double,N,N,default_memory_manager, column_major_layout> v, v2;

            matrix<double,M,N,MM, column_major_layout> a2;
            a2 = tmp(a/2);

            svd2(true,true,a2+a2,u,q,v);

            double err = max(abs(a - subm(u,get_rect(a2+a2))*diagm(q)*trans(v)));
            DLIB_TEST_MSG( err < 1e-11,"err: " << err);
            using dlib::equal;
            DLIB_TEST((equal(trans(u)*u , identity_matrix<double,M>(), 1e-10)));
            DLIB_TEST((equal(trans(v)*v , identity_matrix<double,N>(), 1e-10)));

            svd2(false,true,a2+a2,u,q,v2);
            svd2(true,false,a2+a2,u2,q,v);
            svd2(false,false,a2+a2,u,q2,v);

            err = max(abs(a - subm(u2,get_rect(a2+a2))*diagm(q2)*trans(v2)));
            DLIB_TEST_MSG( err < 1e-11,"err: " << err);
            DLIB_TEST((equal(trans(u2)*u2 , identity_matrix<double,M>(), 1e-10)));
            DLIB_TEST((equal(trans(v2)*v2 , identity_matrix<double,N>(), 1e-10)));

        }

        {
            srand(423452);
            const long M = 10;
            const long N = 7;

            matrix<double> a(M,N);
            for (long r = 0; r < a.nr(); ++r)
            {
                for (long c = 0; c < a.nc(); ++c)
                {
                    a(r,c) = 10*((double)::rand())/RAND_MAX;
                }
            }

            matrix<double,M,M> u;
            matrix<double> q;
            matrix<double,N,N> v;

            matrix<double,M,N,MM> a2;
            a2 = tmp(a/2);

            svd2(true,true,a2+a2,u,q,v);

            double err = sum(round(1e10*(a - subm(u,get_rect(a2+a2))*diagm(q)*trans(v))));
            DLIB_TEST_MSG(  err == 0,"err: " << err);
            DLIB_TEST((round(1e10*trans(u)*u)  == 1e10*identity_matrix<double,M>()));
            DLIB_TEST((round(1e10*trans(v)*v)  == 1e10*identity_matrix<double,N>()));
        }

        {
            srand(423452);
            const long M = 10;
            const long N = 7;

            matrix<double> a(M,N);
            for (long r = 0; r < a.nr(); ++r)
            {
                for (long c = 0; c < a.nc(); ++c)
                {
                    a(r,c) = 10*((double)::rand())/RAND_MAX;
                }
            }

            matrix<double,M> u(M,N);
            matrix<double> w;
            matrix<double,N,N> v(N,N);

            matrix<double,M,N,MM> a2;
            a2 = tmp(a/2);

            svd(a2+a2,u,w,v);

            DLIB_TEST(  sum(round(1e10*(a - u*w*trans(v)))) == 0);
            DLIB_TEST((round(1e10*trans(u)*u)  == 1e10*identity_matrix<double,N>()));
            DLIB_TEST((round(1e10*trans(v)*v)  == 1e10*identity_matrix<double,N>()));
        }

        {
            srand(423452);
            const long M = 1;
            const long N = 1;

            matrix<double> a(M,N);
            for (long r = 0; r < a.nr(); ++r)
            {
                for (long c = 0; c < a.nc(); ++c)
                {
                    a(r,c) = 10*((double)::rand())/RAND_MAX;
                }
            }

            matrix<double,M,N> u;
            matrix<double> w;
            matrix<double,N,N> v;

            matrix<double,M,N> a2;
            a2 = 0;
            a2 = tmp(a/2);

            svd(a2+a2,u,w,v);

            DLIB_TEST(  sum(round(1e10*(a - u*w*trans(v)))) == 0);
            DLIB_TEST((round(1e10*trans(u)*u)  == 1e10*identity_matrix<double,N>()));
            DLIB_TEST((round(1e10*trans(v)*v)  == 1e10*identity_matrix<double,N>()));
        }

        {
            srand(53434);
            const long M = 5;
            const long N = 5;

            matrix<double> a(M,N);
            for (long r = 0; r < a.nr(); ++r)
            {
                for (long c = 0; c < a.nc(); ++c)
                {
                    a(r,c) = 10*((double)::rand())/RAND_MAX;
                }
            }

            matrix<double,0,N> u(M,N);
            matrix<double,N,N> w;
            matrix<double> v;

            svd(a,u,w,v);

            DLIB_TEST(  sum(round(1e10*(a - u*w*trans(v)))) == 0);
            DLIB_TEST((round(1e10*trans(u)*u)  == 1e10*identity_matrix<double,N>()));
            DLIB_TEST((round(1e10*trans(v)*v)  == 1e10*identity_matrix<double,N>()));
        }

        {
            srand(11234);
            const long M = 9;
            const long N = 4;

            matrix<double,0,0,MM> a(M,N);
            for (long r = 0; r < a.nr(); ++r)
            {
                for (long c = 0; c < a.nc(); ++c)
                {
                    a(r,c) = 10*((double)::rand())/RAND_MAX;
                }
            }

            matrix<double> u;
            matrix<double,0,0,MM> w;
            matrix<double> v;

            svd(a,u,w,v);

            DLIB_TEST(  sum(round(1e10*(a - u*w*trans(v)))) == 0);
            DLIB_TEST((round(1e10*trans(u)*u)  == 1e10*identity_matrix<double,N>()));
            DLIB_TEST((round(1e10*trans(v)*v)  == 1e10*identity_matrix<double,N>()));
        }

        {
            srand(53934);
            const long M = 2;
            const long N = 4;

            matrix<double> a(M,N);
            for (long r = 0; r < a.nr(); ++r)
            {
                for (long c = 0; c < a.nc(); ++c)
                {
                    a(r,c) = 10*((double)::rand())/RAND_MAX;
                }
            }

            matrix<double> u;
            matrix<double> w;
            matrix<double> v;

            svd(a,u,w,v);

            DLIB_TEST(  sum(round(1e10*(a - u*w*trans(v)))) == 0);
        }

        {
            srand(53234);
            const long M = 9;
            const long N = 40;

            matrix<double> a(M,N);
            for (long r = 0; r < a.nr(); ++r)
            {
                for (long c = 0; c < a.nc(); ++c)
                {
                    a(r,c) = 10*((double)::rand())/RAND_MAX;
                }
            }

            matrix<double> u;
            matrix<double> w;
            matrix<double> v;

            svd(a,u,w,v);

            DLIB_TEST(  sum(round(1e10*(a - u*w*trans(v)))) == 0);
        }

        {
            srand(53234);
            const long M = 9;
            const long N = 40;

            typedef matrix<double,0,0,default_memory_manager, column_major_layout> mat;
            mat a(M,N);
            for (long r = 0; r < a.nr(); ++r)
            {
                for (long c = 0; c < a.nc(); ++c)
                {
                    a(r,c) = 10*((double)::rand())/RAND_MAX;
                }
            }

            mat u;
            mat w;
            mat v;

            svd(a,u,w,v);

            DLIB_TEST(  sum(round(1e10*(a - u*w*trans(v)))) == 0);
        }

        {
            matrix<double> a(3,3);
            matrix<double,3,3> b;
            set_all_elements(a,0);

            a(0,0) = 1;
            a(1,1) = 2;
            a(2,2) = 3;
            b = a;

            DLIB_TEST(diag(a)(0) == 1);
            DLIB_TEST(diag(a)(1) == 2);
            DLIB_TEST(diag(a)(2) == 3);
            DLIB_TEST(diag(a).nr() == 3);
            DLIB_TEST(diag(a).nc() == 1);

            DLIB_TEST(diag(b)(0) == 1);
            DLIB_TEST(diag(b)(1) == 2);
            DLIB_TEST(diag(b)(2) == 3);
            DLIB_TEST(diag(b).nr() == 3);
            DLIB_TEST(diag(b).nc() == 1);

            DLIB_TEST(pointwise_multiply(a,b)(0,0) == 1);
            DLIB_TEST(pointwise_multiply(a,b)(1,1) == 4);
            DLIB_TEST(pointwise_multiply(a,b)(2,2) == 9);
            DLIB_TEST(pointwise_multiply(a,b)(1,0) == 0);
            DLIB_TEST(pointwise_multiply(a,b,a)(1,0) == 0);
            DLIB_TEST(pointwise_multiply(a,b,a,b)(1,0) == 0);

            DLIB_TEST(complex_matrix(a,b)(0,0) == std::complex<double>(1,1));
            DLIB_TEST(complex_matrix(a,b)(2,2) == std::complex<double>(3,3));
            DLIB_TEST(complex_matrix(a,b)(2,1) == std::complex<double>(0,0));
        }

        {
            matrix<complex<double> > m(2,2), m2(2,2);
            complex<double> val1(1,2), val2(1.0/complex<double>(1,2));
            m = val1;
            m2 = val2;

            DLIB_TEST(equal(reciprocal(m) , m2));
        }
        {
            matrix<complex<float> > m(2,2), m2(2,2);
            complex<float> val1(1,2), val2(1.0f/complex<float>(1,2));
            m = val1;
            m2 = val2;

            DLIB_TEST(equal(reciprocal(m) , m2));
        }

        {
            matrix<float,3,1> m1, m2;
            set_all_elements(m1,2.0);
            set_all_elements(m2,1.0/2.0);
            DLIB_TEST(reciprocal(m1) == m2);
            DLIB_TEST((reciprocal(uniform_matrix<float,3,1>(2.0)) == m2));
            DLIB_TEST((round_zeros(uniform_matrix<float,3,1>(1e-8f)) == uniform_matrix<float,3,1>(0)) );
            set_all_elements(m1,2.0);
            m2 = m1;
            m1(1,0) = static_cast<float>(1e-8);
            m2(1,0) = 0;
            DLIB_TEST(round_zeros(m1) == m2);
            m1 = round_zeros(m1);
            DLIB_TEST(m1 == m2);
        }

        {
            matrix<matrix<double,2,2> > m;
            m.set_size(3,3);
            set_all_elements(m,uniform_matrix<double,2,2>(1));
            DLIB_TEST((sum(m) == uniform_matrix<double,2,2>(9)));
            DLIB_TEST((round_zeros(sqrt(sum(m)) - uniform_matrix<double,2,2>(3)) == uniform_matrix<double,2,2>(0)));
        }

        {
            matrix<int,2,2> m1;
            matrix<int> m2;
            m2.set_size(2,2);

            set_all_elements(m1,2);
            m2 = uniform_matrix<int,2,2>(2);

            m1 = m1 + m2;
            DLIB_TEST((m1 == uniform_matrix<int,2,2>(4)));

            set_all_elements(m1,2);
            set_all_elements(m2,2);
            m1 = m1*m1;
            DLIB_TEST((m1 == uniform_matrix<int,2,2>(8)));

            m1(1,0) = 1;
            set_all_elements(m2,8);
            m2(0,1) = 1;
            m1 = trans(m1);
            DLIB_TEST(m1 == m2);
        }

        {
            matrix<double,2,3> m;
            matrix<double> m2(2,3);

            set_all_elements(m,1);
            DLIB_TEST(mean(m) == 1);
            set_all_elements(m,2);
            DLIB_TEST(mean(m) == 2);
            m(0,0) = 1;
            m(0,1) = 1;
            m(0,2) = 1;
            DLIB_TEST(abs(mean(m) - 1.5) < 1e-10);
            DLIB_TEST(abs(variance(m) - 0.3) < 1e-10);

            set_all_elements(m2,1);
            DLIB_TEST(mean(m2) == 1);
            set_all_elements(m2,2);
            DLIB_TEST(mean(m2) == 2);
            m2(0,0) = 1;
            m2(0,1) = 1;
            m2(0,2) = 1;
            DLIB_TEST(abs(mean(m2) - 1.5) < 1e-10);
            DLIB_TEST(abs(variance(m2) - 0.3) < 1e-10);

            set_all_elements(m,0);
            DLIB_TEST(abs(variance(m)) < 1e-10);
            set_all_elements(m,1);
            DLIB_TEST(abs(variance(m)) < 1e-10);
            set_all_elements(m,23.4);
            DLIB_TEST(abs(variance(m)) < 1e-10);
        }

        {
            matrix<matrix<double,3,1,MM>,2,2,MM> m;
            set_all_elements(m,uniform_matrix<double,3,1>(1));
            DLIB_TEST((round_zeros(variance(m)) == uniform_matrix<double,3,1>(0)));
            DLIB_TEST((round_zeros(mean(m)) == uniform_matrix<double,3,1>(1)));
            m(0,0) = uniform_matrix<double,3,1>(9);
            DLIB_TEST((round_zeros(variance(m)) == uniform_matrix<double,3,1>(16)));
            DLIB_TEST((round_zeros(mean(m)) == uniform_matrix<double,3,1>(3)));

            matrix<matrix<double> > m2(2,2);
            set_all_elements(m2,uniform_matrix<double,3,1>(1));
            DLIB_TEST((round_zeros(variance(m2)) == uniform_matrix<double,3,1>(0)));
            DLIB_TEST((round_zeros(mean(m2)) == uniform_matrix<double,3,1>(1)));
            m2(0,0) = uniform_matrix<double,3,1>(9);
            DLIB_TEST((round_zeros(variance(m2)) == uniform_matrix<double,3,1>(16)));
            DLIB_TEST((round_zeros(mean(m2)) == uniform_matrix<double,3,1>(3)));
        }

        {
            matrix<double> m(4,4), m2;
            m = 1,2,3,4,
                1,2,3,4,
                4,6,8,10,
                4,6,8,10;
            m2 = m;

            DLIB_TEST(colm(m,range(0,3)) == m);
            DLIB_TEST(rowm(m,range(0,3)) == m);
            DLIB_TEST(colm(m,range(0,0)) == colm(m,0));
            DLIB_TEST(rowm(m,range(0,0)) == rowm(m,0));
            DLIB_TEST(colm(m,range(1,1)) == colm(m,1));
            DLIB_TEST(rowm(m,range(1,1)) == rowm(m,1));

            DLIB_TEST(colm(m,range(2,2)) == colm(m,2));
            DLIB_TEST(rowm(m,range(2,2)) == rowm(m,2));

            DLIB_TEST(colm(m,range(1,2)) == subm(m,0,1,4,2));
            DLIB_TEST(rowm(m,range(1,2)) == subm(m,1,0,2,4));

            set_colm(m,range(1,2)) = 9;
            set_subm(m2,0,1,4,2) = 9;
            DLIB_TEST(m == m2);

            set_colm(m,range(1,2)) = 11;
            set_subm(m2,0,1,4,2) = 11;
            DLIB_TEST(m == m2);
        }

    }

    class matrix_tester : public tester
    {
    public:
        matrix_tester (
        ) :
            tester ("test_matrix2",
                    "Runs tests on the matrix component.")
        {}

        void perform_test (
        )
        {
            matrix_test();
        }
    } a;

}


#include <stiffness_checker/Material.h>
#include <stiffness_checker/StiffnessIO.h>
#include "test_common.h"
#include <catch2/catch.hpp>
#include <fstream>
#include <nlohmann/json.hpp>
#include <Eigen/Dense>

namespace
{
void assert_eq_PLA(const conmech::material::Material& m)
{
  using namespace conmech_testing;
  using namespace conmech::material;
  double eps = conmech::EPSILON;

  // these readings are expected to be exact
  // kN/cm2 to kN/m2
  double pressure_unit = 1e4;
  assert_near(m.youngs_modulus_, pressure_unit * 350, eps);
  assert_near(m.shear_modulus_, pressure_unit * 240, eps);
  assert_near(computePoissonRatio(m.youngs_modulus_, m.shear_modulus_), m.poisson_ratio_, eps);

  // kN/m3
  double density_unit = 1;
  assert_near(m.density_, density_unit * 12.2582, eps);

  // cm^2 -> m^2
  double area_unit = 1e-4;
  assert_near(m.cross_sec_area_, area_unit * 0.07068583470577035, eps);

  // cm^4 -> m^4
  double inertia_unit = 1e-8;
  assert_near(m.Jx_, inertia_unit * 0.0007952156404399163, eps);
  assert_near(m.Iy_, inertia_unit * 0.00039760782021995816, eps);
  assert_near(m.Iz_, inertia_unit * 0.00039760782021995816, eps);
}
}

TEST_CASE("material deserialed from a json file", "[io]")
{
  using namespace conmech_testing;
  using namespace conmech::material;
  std::string test_frame_path = conmech_testing::data_path("four-frame.json");

  std::ifstream is(test_frame_path);
  if (!is.is_open()) {
      throw std::runtime_error("Couldn't open frame: " + test_frame_path);
  }
  nlohmann::json config;
  is >> config;

  Material m;
  parseMaterialPropertiesJson(config["material_properties"], m);
  assert_eq_PLA(m);

  m.setFromJson(config["material_properties"]);
  assert_eq_PLA(m);
}

TEST_CASE("frame data deserialed from a json file", "[io]")
{
  using namespace conmech_testing;
  using namespace conmech::material;
  using namespace conmech::stiffness_checker;
  std::string test_frame_path = conmech_testing::data_path("four-frame.json");

  Eigen::MatrixXd V;
  Eigen::MatrixXi E;
  Eigen::MatrixXi Fixities;
  std::vector<Material> mats;
  double eps = conmech::EPSILON;

  parseFrameJson(test_frame_path, V, E, Fixities, mats);

  Eigen::MatrixXd realV(5,3);
  realV << 0, -20, -10,
           0, 20, -10,
           0, 20, 0,
           0, -20, 0,
           0, 0, 20;
  realV *= 0.001;
  assert_near_m(V, realV, eps);

  Eigen::MatrixXi realE(4,2);
  realE << 0, 3,
           1, 2,
           3, 4,
           2, 4;
  assert_near_m(E, realE, eps);

  Eigen::MatrixXi realFixities(2, 7);
  realFixities << 0, 1, 1, 1, 1, 1, 1,
                  1, 1, 1, 1, 1, 1, 1;
  assert_near_m(Fixities, realFixities, eps);

  for (const auto& m : mats)
  {
    assert_eq_PLA(m);
  }
}


//==============================================================================
//          Copyright 2015 - J.T. Lapreste
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_SIGNIFICANTS_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_SIGNIFICANTS_HPP_INCLUDED

#include <nt2/exponential/functions/significants.hpp>
#include <nt2/include/functions/simd/round.hpp>
#include <nt2/include/functions/simd/tenpower.hpp>
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/if_zero_else.hpp>
#include <nt2/include/functions/simd/is_eqz.hpp>
#include <nt2/include/functions/simd/log10.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/divides.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/functions/simd/iceil.hpp>
#include <boost/dispatch/attributes.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/assert.hpp>
#include <boost/simd/operator/functions/details/assert_utils.hpp>
#include <boost/mpl/equal_to.hpp>
#ifndef BOOST_SIMD_NO_INVALIDS
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/is_invalid.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT_IF( significants_, tag::cpu_
                             , (A0)(X)(A1)
                             , (boost::mpl::equal_to < boost::simd::meta::cardinal_of<A0>
                                                     , boost::simd::meta::cardinal_of<A1>
                                >)
                             , ((simd_< floating_<A0>, X>))
                               ((simd_< integer_<A1>, X>))
                             )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(2)
    {
      BOOST_ASSERT_MSG( assert_all(is_gtz(a1))
                      , "Number of significant digits must be positive"
                      );
      typedef typename boost::dispatch::meta::as_integer<A0>::type iA0;
      iA0 exp = a1 - iceil(log10(abs(a0)));
      A0 fac = tenpower(exp);
      A0 scaled = round(a0*fac);
#ifndef BOOST_SIMD_NO_INVALIDS
      A0 r = if_else(is_invalid(a0), a0, scaled/fac);
#else
      A0 r =  scaled/fac;
#endif
      return if_zero_else(is_eqz(a0), r);
    }
  };

} }

#endif


//rego: Automatic time series forecasting and missing value imputation.
//
//Copyright (C) Davide Altomare and David Loris <channelattribution.io>
//
//This source code is licensed under the MIT license found in the
//LICENSE file in the root directory of this source tree.

#include <iostream>
#include <vector>
#include <set>
#include <math.h>
#include <time.h>
#include <stdio.h>
#include <sstream>
#include <list>
#include <string>
#include <random>
#include <numeric>
#include <time.h>
#include <thread>
#include <map>
#include <algorithm>
#include <list>
#include <limits>

#include <armadillo>

#define uli unsigned long int

#include "functions.h"

using namespace std;
using namespace arma;

#define language_cpp

int main(void)
{

 mat Y;
 vec vnan;
 double max_lag,alpha;
 string direction;
 uli nsim;
 bool flg_print;
 str_output out;

 //USECASE 1

 Y.load("data/Data_air.csv", csv_ascii);
 Y.replace(0,datum::nan);

 max_lag=-1; //-1 means auto
 direction="<->";
 alpha=0.05;
 nsim=1000;
 flg_print=1;

 out=regpred_cpp(&Y, max_lag, alpha, nsim, flg_print, direction);

 out.predictions.print("Usecase 1 predictions");

 //USECASE 2

 Y.load("data/Data_sim_1000.csv", csv_ascii);
 Y.replace(0,datum::nan);

 max_lag=-1; //-1 means auto
 direction="<->";
 alpha=0.05;
 nsim=1000;
 flg_print=1;

 out=regpred_cpp(&Y, max_lag, alpha, nsim, flg_print, direction);

 out.predictions.print("Usecase 2 predictions");

 return 0;

}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_SINH_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_SINH_HPP_INCLUDED
#include <boost/simd/function/std.hpp>

#include <boost/simd/arch/common/detail/generic/sinh_kernel.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/log_2.hpp>
#include <boost/simd/detail/constant/maxlog.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/exp.hpp>
#include <boost/simd/function/fms.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( sinh_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_<bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 a0) const BOOST_NOEXCEPT
    {
      //////////////////////////////////////////////////////////////////////////////
      // if x = abs(a0) is less than 1 sinh is computed using a polynomial(float)
      // respectively rational(double) approx from cephes.
      // else according x < Threshold e =  exp(x) or exp(x/2) is respectively
      // computed
      // * in the first case sinh is (e-rec(e))/2
      // * in the second     sinh is (e/2)*e (avoiding undue overflow)
      // Threshold is Maxlog - Log_2 defined in Maxshlog
      //////////////////////////////////////////////////////////////////////////////
      A0 x = bs::abs(a0);
      if( x < One<A0>())
      {
       A0 x2 = sqr(x);
       return detail::sinh_kernel<A0>::compute(a0, x2);
      }
      else
      {
        A0 r;
        if (BOOST_UNLIKELY( x > Maxlog<A0>()-Log_2<A0>()))
        {
          A0 tmp = exp(Half<A0>()*x);
          r = (Half<A0>()*tmp)*tmp;
        }
        else
        {
          A0 tmp = exp(x);
          r =  fms(tmp, Half<A0>(), Half<A0>()*rec(tmp));
        }
        return bitwise_xor(r, bitofsign(a0));
      }
    }
  };
  BOOST_DISPATCH_OVERLOAD ( sinh_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                         )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &,  A0  a0) const BOOST_NOEXCEPT
    {
      return std::sinh(a0);
    }
  };
} } }

#endif


#pragma once

#ifndef AMM_H
#define AMM_H

#include <boost/multiprecision/gmp.hpp>
#include "./token.hpp"

// sample class describing a simple constant product AMM
// We use the convention that the first token is the sell token
// and the second token is the buy token.
class CP_AMM {
    public:
        const int amm_index;
        const int sell_token_index;
        const boost::multiprecision::mpz_int sell_reserve_amount;
        const int buy_token_index;
        const boost::multiprecision::mpz_int buy_reserve_amount;
        const boost::multiprecision::mpf_float fee_amount;
        const int cost_token_index;
        const boost::multiprecision::mpz_int cost_amount;
        const bool is_mandatory;

        // Constructor
        CP_AMM(int _amm_index, int _sell_token_index, boost::multiprecision::mpz_int _sell_reserve_amount, int _buy_token_index, boost::multiprecision::mpz_int _buy_reserve_amount, boost::multiprecision::mpf_float _fee_amount, int _cost_token_index, boost::multiprecision::mpz_int _cost_amount, bool _is_mandatory): amm_index(_amm_index), sell_token_index(_sell_token_index),  sell_reserve_amount(_sell_reserve_amount), buy_token_index(_buy_token_index), buy_reserve_amount(_buy_reserve_amount), fee_amount(_fee_amount), cost_token_index(_cost_token_index), cost_amount(_cost_amount), is_mandatory(_is_mandatory) {
        }

};

void print_cp_amm(CP_AMM &a, std::vector<Token> &_tokens);

#endif


/*
 * Copyright 2015 Christoph Jud (christoph.jud@unibas.ch)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include <iostream>
#include <memory>
#include <ctime>

#include <boost/random.hpp>

#include <Eigen/SVD>

#include "GaussianProcess.h"
#include "Kernel.h"

using namespace gpr;

typedef GaussianKernel<double>		KernelType;
typedef std::shared_ptr<KernelType> KernelTypePointer;
typedef GaussianProcess<double> GaussianProcessType;
typedef std::shared_ptr<GaussianProcessType> GaussianProcessTypePointer;

typedef GaussianProcessType::VectorType VectorType;
typedef GaussianProcessType::MatrixType MatrixType;

VectorType GetRandomVector(unsigned n){
    static boost::minstd_rand randgen(static_cast<unsigned>(time(0)));
    static boost::normal_distribution<> dist(0, 1);
    static boost::variate_generator<boost::minstd_rand, boost::normal_distribution<> > r(randgen, dist);

    VectorType v = VectorType::Zero(n);
    for (unsigned i=0; i < n; i++) {
        v(i) = r();
    }
    return v;
}

void Test1(){
    /*
     * Test 1: scalar valued GP
     * - try to learn sinus function
     */
    std::cout << "Test 1: sinus regression... " << std::flush;

    KernelTypePointer k(new KernelType(0.5));
    GaussianProcessTypePointer gp(new GaussianProcessType(k));
    gp->SetSigma(0.00001);

    unsigned number_of_samples = 20;

    // add training samples
    for(unsigned i=0; i<number_of_samples; i++){
        VectorType x(1);
        x(0) = i * 2*M_PI/number_of_samples;

        VectorType y(1);
        y(0) = std::sin(x(0));
        gp->AddSample(x,y);
    }
    gp->Initialize();

    // calculate credible interval
    unsigned number_of_tests = 50;
    for(unsigned i=0; i<number_of_tests; i++){
        VectorType x(1);
        x(0) = i * 2*M_PI/number_of_tests * 1.3;

        try{
            double c = 2*std::sqrt((*gp)(x,x)) - gp->GetCredibleInterval(x);
            if(c != 0){
                throw std::string("credible interval not correct.");
                return;
            }
        }
        catch(std::string& s){
            throw std::string("error in calculating credible interval.");
            return;
        }
    }
    std::cout << " [passed]" << std::endl;
}

void Test2(){
    /*
     * Test 2: sampling test
     * - try to sample from a posterior
     */
    std::cout << "Test 2: posterior sampling test... " << std::flush;

    KernelTypePointer k(new KernelType(1));
    GaussianProcessTypePointer gp(new GaussianProcessType(k));
    gp->SetSigma(0);

    // add some landmarks
    gp->AddSample(VectorType::Ones(1),  VectorType::Zero(1));
    gp->AddSample(VectorType::Ones(1)*2,VectorType::Ones(1));
    gp->AddSample(VectorType::Ones(1)*3,VectorType::Ones(1)*0.5);
    gp->AddSample(VectorType::Ones(1)*4,VectorType::Ones(1));
    gp->Initialize();

    // compute gp kernel matrix (interval 0-5)
    unsigned number_of_samples = 50;
    VectorType mean = VectorType::Zero(number_of_samples);
    MatrixType K = MatrixType::Zero(number_of_samples,number_of_samples);

    #pragma omp parallel for
    for(unsigned i=0; i<number_of_samples; i++){
        VectorType x1(1);
        x1(0) = i * 5.0/static_cast<double>(number_of_samples);
        mean[i] = gp->Predict(x1)(0);

        for(unsigned j=i; j<number_of_samples; j++){
            VectorType x2(1);
            x2(0) = j * 5.0/number_of_samples;

            K(i,j) = (*gp)(x1,x2);
            K(j,i) = K(i,j);
        }

    }

    // generate multivariate normal random numbers
    Eigen::SelfAdjointEigenSolver<MatrixType> eigenSolver(K);
    unsigned numComponentsToKeep = ((eigenSolver.eigenvalues().array() - 1e-10) > 0).count();
    MatrixType rot = eigenSolver.eigenvectors().rowwise().reverse().topLeftCorner(K.rows(), numComponentsToKeep);
    VectorType scl = eigenSolver.eigenvalues().reverse().topRows(numComponentsToKeep).array().sqrt();

    MatrixType Q = rot*scl.asDiagonal();
    double err = (Q*Q.transpose() - K).norm();
    if(err > 1e-8 || std::isnan(err)){
        std::stringstream ss; ss<<"eigen decomposition not accurate enough. (error: " << err << ")"; throw ss.str();
        return;
    }

    for(unsigned k=0; k<10; k++){
        VectorType r = rot * VectorType(GetRandomVector(number_of_samples).array() * scl.array()) + mean;

        // since the noise of the gp is zero, at the landmark points,
        // all samples have to match the mean
        if(std::fabs(r[10] - mean[10]) > 1e-9 ||
                std::fabs(r[20] - mean[20]) > 1e-9 ||
                std::fabs(r[30] - mean[30]) > 1e-9 ||
                std::fabs(r[40] - mean[40]) > 1e-9){
            throw std::string("samples do not corresponds to the landmarks.");
            return;
        }
    }

    std::cout << " [passed]" << std::endl;
}

int main (int argc, char *argv[]){
    std::cout << "Gaussian process posterior test: " << std::endl;
    try{
        Test1();
        Test2();
    }
    catch(std::string& s){
        std::cout << "[failed] Error: " << s << std::endl;
        return -1;
    }
    return 0;
}


#ifndef PCL_TRACKING_IMPL_DISTANCE_COHERENCE_H_
#define PCL_TRACKING_IMPL_DISTANCE_COHERENCE_H_

#include <Eigen/Dense>

namespace pcl {
namespace tracking {
template <typename PointInT>
double DistanceCoherence<PointInT>::computeCoherence(PointInT &source,
                                                     PointInT &target) {
    Eigen::Vector4f p = source.getVector4fMap();
    Eigen::Vector4f p_dash = target.getVector4fMap();
    double d = (p - p_dash).norm();
    return 1.0 / (1.0 + d * d * weight_);
}
} // namespace tracking
} // namespace pcl

#define PCL_INSTANTIATE_DistanceCoherence(T)                                   \
    template class PCL_EXPORTS pcl::tracking::DistanceCoherence<T>;

#endif