#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/lexical_cast.hpp>
#include <boost/mpl/list.hpp>

#include <orx/geometry/GeometryUtil.h>

#include "HelperFunctions.h"

using namespace ORUtils;
using namespace orx;

//#################### TESTS ####################

typedef boost::mpl::list<double,float> TS;

BOOST_AUTO_TEST_SUITE(test_GeometryUtil)

BOOST_AUTO_TEST_CASE_TEMPLATE(test_dual_quat_to_pose, T, TS)
{
  DualQuaternion<T> dq = DualQuaternion<T>::from_rotation(Vector3<T>(0,0,1), T(M_PI_2));

  SE3Pose pose = GeometryUtil::dual_quat_to_pose(dq);
  Vector3f t = pose.GetT();
  Vector3f r = GeometryUtil::to_rotation_vector(pose.GetR());

  BOOST_CHECK_SMALL(length(t), 1e-4f);
  BOOST_CHECK_SMALL(length(r - Vector3f(0,0,(float)M_PI_2)), 1e-4f);
  BOOST_CHECK(DualQuaternion<T>::close(GeometryUtil::pose_to_dual_quat<T>(pose), dq));
}

BOOST_AUTO_TEST_CASE(test_estimate_rigid_transform)
{
  Eigen::Matrix3f P;
  P(0,0) = 1; P(0,1) = 0; P(0,2) = 0;
  P(1,0) = 0; P(1,1) = 1; P(1,2) = 0;
  P(2,0) = 0; P(2,1) = 0; P(2,2) = 1;

  Eigen::Matrix3f Q;
  Q(0,0) = 1; Q(0,1) = 1; Q(0,2) = 0;
  Q(1,0) = 0; Q(1,1) = 1; Q(1,2) = 0;
  Q(2,0) = 1; Q(2,1) = 0; Q(2,2) = 0;

  Eigen::Matrix4f M = GeometryUtil::estimate_rigid_transform(P, Q);

  for(int i = 0; i < 3; ++i)
  {
    BOOST_CHECK_SMALL((M * P.col(i).homogeneous() - Q.col(i).homogeneous()).norm(), 1e-4f);
  }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_find_best_hypothesis, T, TS)
{
  // Generate increasingly-large clusters of rotated poses around the z axis at 0, PI/2, PI and 3*PI/2 radians.
  const double rotThreshold = 20 * M_PI / 180;
  const float transThreshold = 0.05f;
  const Vector3<T> up(0,0,1);

  std::map<std::string,SE3Pose> inputPoses;
  size_t id = 0;
  for(float i = 0.0f; i < 4.0f; ++i)
  {
    for(float j = -i; j <= i; ++j)
    {
      float angle = static_cast<float>(i * M_PI_2 + j * M_PI / 180);
      inputPoses.insert(std::make_pair(
        boost::lexical_cast<std::string>(id++),
        GeometryUtil::dual_quat_to_pose(DualQuaternion<T>::from_rotation(up, angle))
      ));
    }
  }

  // Find the best hypothesis from these poses, and check that it is one of the poses around 3*PI/2.
  std::vector<SE3Pose> inliersForBestHypothesis;
  int bestHypothesisID = boost::lexical_cast<int>(GeometryUtil::find_best_hypothesis(inputPoses, inliersForBestHypothesis, rotThreshold, transThreshold));
  BOOST_CHECK_GT(bestHypothesisID, 1 + 3 + 5);

  // Check that blending the inliers for the best hypothesis together gives the 3*PI/2 pose.
  SE3Pose refinedPose = GeometryUtil::blend_poses(inliersForBestHypothesis);
  BOOST_CHECK(DualQuaternion<T>::close(GeometryUtil::pose_to_dual_quat<T>(refinedPose), DualQuaternion<T>::from_rotation(up, T(3 * M_PI_2))));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_pose_to_dual_quat, T, TS)
{
  Vector3<T> r(0,T(M_PI_4),0);
  Vector3<T> t(3,4,5);
  SE3Pose pose;
  pose.SetT(t.toFloat());
  pose.SetR(GeometryUtil::to_rotation_matrix<T,float>(r));

  DualQuaternion<T> dq = GeometryUtil::pose_to_dual_quat<T>(pose);

  BOOST_CHECK_SMALL(length(dq.get_rotation() - r), T(1e-4f));
  BOOST_CHECK_SMALL(length(dq.get_translation() - t), T(1e-4f));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_poses_are_similar, T, TS)
{
  const double rotThreshold = 20 * M_PI / 180;
  const float transThreshold = 0.05f;

  DualQuaternion<T> r1 = DualQuaternion<T>::from_rotation(Vector3<T>(0,0,1), 0.0f);
  DualQuaternion<T> r2 = DualQuaternion<T>::from_rotation(Vector3<T>(0,0,1), 19 * (float)M_PI / 180);
  DualQuaternion<T> r3 = DualQuaternion<T>::from_rotation(Vector3<T>(0,0,1), 21 * (float)M_PI / 180);

  BOOST_CHECK(GeometryUtil::poses_are_similar(GeometryUtil::dual_quat_to_pose(r1), GeometryUtil::dual_quat_to_pose(r2), rotThreshold, transThreshold));
  BOOST_CHECK(!GeometryUtil::poses_are_similar(GeometryUtil::dual_quat_to_pose(r1), GeometryUtil::dual_quat_to_pose(r3), rotThreshold, transThreshold));
  BOOST_CHECK(GeometryUtil::poses_are_similar(GeometryUtil::dual_quat_to_pose(r2), GeometryUtil::dual_quat_to_pose(r3), rotThreshold, transThreshold));

  DualQuaternion<T> t1 = DualQuaternion<T>::from_translation(Vector3<T>(0,0,0));
  DualQuaternion<T> t2 = DualQuaternion<T>::from_translation(Vector3<T>(0.04f,0,0));
  DualQuaternion<T> t3 = DualQuaternion<T>::from_translation(Vector3<T>(0.06f,0,0));

  BOOST_CHECK(GeometryUtil::poses_are_similar(GeometryUtil::dual_quat_to_pose(t1), GeometryUtil::dual_quat_to_pose(t2), rotThreshold, transThreshold));
  BOOST_CHECK(!GeometryUtil::poses_are_similar(GeometryUtil::dual_quat_to_pose(t1), GeometryUtil::dual_quat_to_pose(t3), rotThreshold, transThreshold));
  BOOST_CHECK(GeometryUtil::poses_are_similar(GeometryUtil::dual_quat_to_pose(t2), GeometryUtil::dual_quat_to_pose(t3), rotThreshold, transThreshold));

  DualQuaternion<T> t1r1 = t1 * r1, t1r3 = t1 * r3, t2r2 = t2 * r2, t3r1 = t3 * r1, t3r3 = t3 * r3;

  BOOST_CHECK(GeometryUtil::poses_are_similar(GeometryUtil::dual_quat_to_pose(t1r1), GeometryUtil::dual_quat_to_pose(t2r2), rotThreshold, transThreshold));
  BOOST_CHECK(!GeometryUtil::poses_are_similar(GeometryUtil::dual_quat_to_pose(t1r1), GeometryUtil::dual_quat_to_pose(t1r3), rotThreshold, transThreshold));
  BOOST_CHECK(!GeometryUtil::poses_are_similar(GeometryUtil::dual_quat_to_pose(t1r1), GeometryUtil::dual_quat_to_pose(t3r1), rotThreshold, transThreshold));
  BOOST_CHECK(!GeometryUtil::poses_are_similar(GeometryUtil::dual_quat_to_pose(t1r1), GeometryUtil::dual_quat_to_pose(t3r3), rotThreshold, transThreshold));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_to_rotation_matrix, T, TS)
{
  const T TOL = static_cast<T>(1e-4);

  Matrix3<T> I;
  I.setIdentity();
  check_close(GeometryUtil::to_rotation_matrix(Vector3<T>(T(0))), I, TOL);

  Vector3<T> r(static_cast<T>(M_PI) / 4.0f, 0.0f, 0.0f);
  const T root2inv = static_cast<T>(1.0 / sqrt(2.0));
  Matrix3<T> R(T(1), T(0), T(0), T(0), root2inv, root2inv, T(0), -root2inv, root2inv);
  check_close(GeometryUtil::to_rotation_matrix(r), R, TOL);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_to_rotation_vector, T, TS)
{
  const T TOL = static_cast<T>(1e-4);

  Matrix3<T> I;
  I.setIdentity();
  check_close(GeometryUtil::to_rotation_vector(I), Vector3<T>(T(0)), TOL);

  const T root2inv = static_cast<T>(1.0 / sqrt(2.0));
  Matrix3<T> R(T(1), T(0), T(0), T(0), root2inv, root2inv, T(0), -root2inv, root2inv);
  Vector3<T> r(static_cast<T>(M_PI) / 4.0f, 0.0f, 0.0f);
  check_close(GeometryUtil::to_rotation_vector(R), r, TOL);
}

BOOST_AUTO_TEST_SUITE_END()


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_SIMD_FUNCTION_LOG1P_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_LOG1P_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/simd/divides.hpp>
#include <boost/simd/function/simd/is_nez.hpp>
#include <boost/simd/function/simd/log.hpp>
#include <boost/simd/function/simd/minus.hpp>
#include <boost/simd/function/simd/minusone.hpp>
#include <boost/simd/function/simd/oneplus.hpp>
#include <boost/simd/function/simd/seladd.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/is_equal.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;

  BOOST_DISPATCH_OVERLOAD ( log1p_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator()( const A0& a0) BOOST_NOEXCEPT
    {
      A0 u = oneplus(a0);
      A0 r = seladd(is_nez(u),
                    log(u),
                    (a0-minusone(u))/u); // cancels errors with IEEE arithmetic
#ifndef BOOST_SIMD_NO_INFINITIES
      r = if_else(is_equal(u, Inf<A0>()),u, r);
#endif
      return r;
    }
  };
} } }

#endif


//#define _DEBUG
#define _U_AMOEBA_

#ifdef _U_AMOEBA_
#include "amoeba.hpp"

#ifdef _ALGEBRA_TOOLS_
#else
#include "algebraTools.hpp"
#endif

#ifdef _WEIGHTED_MAT_
#else
#include "weightedMat.hpp"
#endif

#ifdef _LIE_ALGEBRA_
#else
#include "lieAlgebra.cpp"
#endif

#include <iostream>
#include <cmath>
#include <cstdlib>
#include <complex>
#include <armadillo>

using namespace algebraTools;
using std::vector;
using std::ofstream;
using std::complex;

using namespace arma;

//dimension
class UAmoeba: public Amoeba<vec, cx_mat>
{
public:
    UAmoeba(long maxIters,
            int nGridPoints,
            double precision,
            int matSize,
            int lieDimension,
            vector<weightedMat> *lieBasis)
    : Amoeba<vec, cx_mat>( maxIters,  nGridPoints,  precision)
    {

        //int matSize = number of rows in matrix
        //int lieDimension = number of basis vectors in lie algebra
        //int nGridPoints = number of guess points for the amoeba routine

        /*
          to do replace with parameter object
        */
        this->matSize = matSize;
        this->basis = lieBasis;
        this->gridSize = 1000;
        this->gridSizeOld = 1000;
        this->halfGridSize = 500;
        this->h = (1 / static_cast<double>(gridSize));
        this->idMat = eye<cx_mat>(matSize,matSize);
        this->lieDimension = lieDimension;
        this->globalEnergyOld = 10000000;

    }

    ~UAmoeba();

    //Pauli pauliBasis;
    void curvePrint(vec bestVector);
    void curvePrint();

    void newBoundary(cx_mat& newBound);
    void curveSeeder(vector<vec> &newGuess, int nGridPoints, cx_mat sU);
    double getEnergy();
    virtual cx_mat curveFunc(vec kVector);
    virtual double objectFunc(cx_mat A);

protected:
    vector<weightedMat> *basis;

    int matSize;
    int gridSize, halfGridSize, gridSizeOld;
    int lieDimension;

    cx_double imagI = cx_double(0.0, 1.0);

    double globalEnergyOld;
    double globalEnergy;
    double h;

    cx_mat idMat;

    ofstream kFile;
    //ofstream eFile;
    ofstream xFile;

    //function describing the geodesic equations
    virtual void amoebaEnergy();
    virtual void amoebaRestart();

    cx_mat matrixExp(vec &K, double scalar);

    double cost(int index);
    double invCost(int index);

    double energyExtra(cx_mat A, cx_mat B);
    void lieFunction(vec &kVector);
    void curveCorrect(cx_mat &A, vec &v);
};

#include "UAmoeba.cpp"
#endif


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

#ifndef BOOST_MATH_CCMATH_ROUND_HPP
#define BOOST_MATH_CCMATH_ROUND_HPP

#include <cmath>
#include <type_traits>
#include <stdexcept>
#include <boost/math/tools/is_constant_evaluated.hpp>
#include <boost/math/ccmath/abs.hpp>
#include <boost/math/ccmath/isinf.hpp>
#include <boost/math/ccmath/isnan.hpp>
#include <boost/math/ccmath/modf.hpp>

namespace boost::math::ccmath {

namespace detail {

// Computes the nearest integer value to arg (in floating-point format),
// rounding halfway cases away from zero, regardless of the current rounding mode.
template <typename T>
inline constexpr T round_impl(T arg) noexcept
{
    T iptr = 0;
    const T x = boost::math::ccmath::modf(arg, &iptr);
    constexpr T half = T(1)/2;

    if(x >= half && iptr > 0)
    {
        return iptr + 1;
    }
    else if(boost::math::ccmath::abs(x) >= half && iptr < 0)
    {
        return iptr - 1;
    }
    else
    {
        return iptr;
    }
}

template <typename ReturnType, typename T>
inline constexpr ReturnType int_round_impl(T arg)
{
    const T rounded_arg = round_impl(arg);

    if(rounded_arg > static_cast<T>((std::numeric_limits<ReturnType>::max)()))
    {
        if constexpr (std::is_same_v<ReturnType, long long>)
        {
            throw std::domain_error("Rounded value cannot be represented by a long long type without overflow");
        }
        else
        {
            throw std::domain_error("Rounded value cannot be represented by a long type without overflow");
        }
    }
    else
    {
        return static_cast<ReturnType>(rounded_arg);
    }
}

} // Namespace detail

template <typename Real, std::enable_if_t<!std::is_integral_v<Real>, bool> = true>
inline constexpr Real round(Real arg) noexcept
{
    if(BOOST_MATH_IS_CONSTANT_EVALUATED(arg))
    {
        return boost::math::ccmath::abs(arg) == Real(0) ? arg :
               boost::math::ccmath::isinf(arg) ? arg :
               boost::math::ccmath::isnan(arg) ? arg :
               boost::math::ccmath::detail::round_impl(arg);
    }
    else
    {
        using std::round;
        return round(arg);
    }
}

template <typename Z, std::enable_if_t<std::is_integral_v<Z>, bool> = true>
inline constexpr double round(Z arg) noexcept
{
    return boost::math::ccmath::round(static_cast<double>(arg));
}

inline constexpr float roundf(float arg) noexcept
{
    return boost::math::ccmath::round(arg);
}

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
inline constexpr long double roundl(long double arg) noexcept
{
    return boost::math::ccmath::round(arg);
}
#endif

template <typename Real, std::enable_if_t<!std::is_integral_v<Real>, bool> = true>
inline constexpr long lround(Real arg)
{
    if(BOOST_MATH_IS_CONSTANT_EVALUATED(arg))
    {
        return boost::math::ccmath::abs(arg) == Real(0) ? 0l :
               boost::math::ccmath::isinf(arg) ? 0l :
               boost::math::ccmath::isnan(arg) ? 0l :
               boost::math::ccmath::detail::int_round_impl<long>(arg);
    }
    else
    {
        using std::lround;
        return lround(arg);
    }
}

template <typename Z, std::enable_if_t<std::is_integral_v<Z>, bool> = true>
inline constexpr long lround(Z arg)
{
    return boost::math::ccmath::lround(static_cast<double>(arg));
}

inline constexpr long lroundf(float arg)
{
    return boost::math::ccmath::lround(arg);
}

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
inline constexpr long lroundl(long double arg)
{
    return boost::math::ccmath::lround(arg);
}
#endif

template <typename Real, std::enable_if_t<!std::is_integral_v<Real>, bool> = true>
inline constexpr long long llround(Real arg)
{
    if(BOOST_MATH_IS_CONSTANT_EVALUATED(arg))
    {
        return boost::math::ccmath::abs(arg) == Real(0) ? 0ll :
               boost::math::ccmath::isinf(arg) ? 0ll :
               boost::math::ccmath::isnan(arg) ? 0ll :
               boost::math::ccmath::detail::int_round_impl<long long>(arg);
    }
    else
    {
        using std::llround;
        return llround(arg);
    }
}

template <typename Z, std::enable_if_t<std::is_integral_v<Z>, bool> = true>
inline constexpr long llround(Z arg)
{
    return boost::math::ccmath::llround(static_cast<double>(arg));
}

inline constexpr long long llroundf(float arg)
{
    return boost::math::ccmath::llround(arg);
}

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
inline constexpr long long llroundl(long double arg)
{
    return boost::math::ccmath::llround(arg);
}
#endif

} // Namespaces

#endif // BOOST_MATH_CCMATH_ROUND_HPP


//
// Copyright Chong Peng 2017
//

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

#include "type_traits.h"
#include "std_vector_interface.h"
#include "symm_davidson_diag.h"
#include "nonsymm_davidson_diag.h"

using namespace code_example;

int main() {

  std::cout.precision(12);

  std::cout << "Nonsymmetric Davidson Diagnolization Using std::vector \n";

  /// typedef the Array type to use
  using Array = std::vector<double>;

  // variables
  const std::size_t n = 200;
  const double sparse = 0.1;
  const std::size_t n_roots = 5;   // number of roots to solve in davidson
  const double converge = 1.0e-10; // convergence threshold in davidson
  const std::size_t max_iter = 100; // max iteration in davidson

  // initialize symmetric matrix
  ColMatrix<double> A = ColMatrix<double>::Zero(n, n);
  for (auto i = 0; i < n; i++) {
    A(i, i) = i + 1;
  }
  A = A + sparse * ColMatrix<double>::Random(n, n);

  EigenVector<double> A_diagonal = A.diagonal();
  // eigen solve
  Eigen::EigenSolver<ColMatrix<double>> es(A);

  EigenVector<double> e = es.eigenvalues().real();
  std::sort(e.data(), e.data()+e.size());
  e = e.segment(0,n_roots);

  std::cout << "Reference Result from EigenSolve: " << std::endl
            << e << std::endl;

  /// construct the SymmDavidsonDiag  object
  NonSymmDavidsonDiag<Array> dvd(n_roots);

  /// make the initial guess use unit vector
  std::vector<Array> guess(n_roots);
  {
    for (std::size_t i = 0; i < n_roots; i++) {
      guess[i] = Array(n, 0.0);
      guess[i][i] = 1;
    }
  }

  /// make the preconditioner

  auto pred = [&A_diagonal](const EigenVector<double> &e,
                            std::vector<Array> &guess) {

    for (std::size_t i = 0; i < guess.size(); i++) {
      const auto ei = e[i];
      auto &guess_i = guess[i];
      const auto n_r = guess_i.size();
      for (std::size_t i = 0; i < n_r; i++) {
        guess_i[i] = guess_i[i] / (ei - A_diagonal[i]);
      }
    }

  };

  /// make the operator
  auto op = [&A,n](const std::vector<Array> &vec) {
    const std::size_t n_vec = vec.size();

    std::vector<Array> HC(n_vec);

    const char trans = 'N';
    const int32_t rows = A.rows();
    const int32_t cols = A.cols();
    const double alpha = 1.0;
    const double beta = 0.0;
    const int32_t inc = 1;
    for (std::size_t i = 0; i < n_vec; i++) {
      HC[i] = Array(vec[i].size(), 0.0);
      dgemv_(&trans, &rows, &cols, &alpha, A.data(), &rows, vec[i].data(), &inc,
             &beta, HC[i].data(), &inc);
    }

    return HC;
  };

  /// solve

  auto eig = dvd.solve(guess, op, pred, converge, max_iter);

  std::cout << "NonSymmDavidsonDiag result: " << std::endl;
  std::cout << eig << std::endl;

  return 0;
}


//
// Copyright 2012 Chung-Lin Wen, Davide Anastasia
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
#ifndef BOOST_GIL_EXTENSION_TOOLBOX_COLOR_SPACES_XYZ_HPP
#define BOOST_GIL_EXTENSION_TOOLBOX_COLOR_SPACES_XYZ_HPP

#include <boost/gil/color_convert.hpp>
#include <boost/gil/detail/mp11.hpp>
#include <boost/gil/typedefs.hpp>

namespace boost {
namespace gil {

/// \addtogroup ColorNameModel
/// \{
namespace xyz_color_space {
/// \brief x Color Component
struct x_t {};
/// \brief y Color Component
struct y_t {};
/// \brief z Color Component
struct z_t {};
} // namespace xyz_color_space
/// \}

/// \ingroup ColorSpaceModel
using xyz_t = mp11::mp_list<xyz_color_space::x_t, xyz_color_space::y_t,
                            xyz_color_space::z_t>;

/// \ingroup LayoutModel
using xyz_layout_t = layout<xyz_t>;

BOOST_GIL_DEFINE_ALL_TYPEDEFS(32f, float32_t, xyz)

/// \ingroup ColorConvert
/// \brief RGB to XYZ
/// <a
/// href="http://www.brucelindbloom.com/index.html?Eqn_RGB_XYZ_Matrix.html">Link</a>
/// \note rgb_t is assumed to be sRGB D65
template <> struct default_color_converter_impl<rgb_t, xyz_t> {
private:
  BOOST_FORCEINLINE
  float32_t inverse_companding(float32_t sample) const {
    if (sample > 0.04045f) {
      return powf(((sample + 0.055f) / 1.055f), 2.4f);
    } else {
      return (sample / 12.92f);
    }
  }

public:
  template <typename P1, typename P2>
  void operator()(const P1 &src, P2 &dst) const {
    using namespace xyz_color_space;

    float32_t red(inverse_companding(
        channel_convert<float32_t>(get_color(src, red_t()))));
    float32_t green(inverse_companding(
        channel_convert<float32_t>(get_color(src, green_t()))));
    float32_t blue(inverse_companding(
        channel_convert<float32_t>(get_color(src, blue_t()))));

    get_color(dst, x_t()) =
        red * 0.4124564f + green * 0.3575761f + blue * 0.1804375f;
    get_color(dst, y_t()) =
        red * 0.2126729f + green * 0.7151522f + blue * 0.0721750f;
    get_color(dst, z_t()) =
        red * 0.0193339f + green * 0.1191920f + blue * 0.9503041f;
  }
};

/// \ingroup ColorConvert
/// \brief XYZ to RGB
template <> struct default_color_converter_impl<xyz_t, rgb_t> {
private:
  BOOST_FORCEINLINE
  float32_t companding(float32_t sample) const {
    if (sample > 0.0031308f) {
      return (1.055f * powf(sample, 1.f / 2.4f) - 0.055f);
    } else {
      return (12.92f * sample);
    }
  }

public:
  template <typename P1, typename P2>
  void operator()(const P1 &src, P2 &dst) const {
    using namespace xyz_color_space;

    // Note: ideally channel_convert should be compiled out, because xyz_t
    // is float32_t natively only
    float32_t x(channel_convert<float32_t>(get_color(src, x_t())));
    float32_t y(channel_convert<float32_t>(get_color(src, y_t())));
    float32_t z(channel_convert<float32_t>(get_color(src, z_t())));

    get_color(dst, red_t()) =
        channel_convert<typename color_element_type<P2, red_t>::type>(
            companding(x * 3.2404542f + y * -1.5371385f + z * -0.4985314f));
    get_color(dst, green_t()) =
        channel_convert<typename color_element_type<P2, green_t>::type>(
            companding(x * -0.9692660f + y * 1.8760108f + z * 0.0415560f));
    get_color(dst, blue_t()) =
        channel_convert<typename color_element_type<P2, blue_t>::type>(
            companding(x * 0.0556434f + y * -0.2040259f + z * 1.0572252f));
  }
};

} // namespace gil
} // namespace boost

#endif // BOOST_GIL_EXTENSION_TOOLBOX_COLOR_SPACES_XYZ_HPP


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__IMPL__SE3_HPP_
#define SMOOTH__IMPL__SE3_HPP_

#include <Eigen/Core>

#include "common.hpp"
#include "so3.hpp"

namespace smooth {

/**
 * @brief SE(3) Lie Group represented as S^3 ⋉ R3
 *
 * Memory layout
 * -------------
 * Group:    x y z qx qy qz qw
 * Tangent:  vx vy vz Ωx Ωy Ωz
 *
 * Lie group Matrix form
 * ---------------------
 * [ R T ]
 * [ 0 1 ]
 *
 * where R ∈ SO(3) and T = [x y z] ∈ R3
 *
 * Lie algebra Matrix form
 * -----------------------
 * [  0 -Ωz  Ωy vx]
 * [  Ωz  0 -Ωx vy]
 * [ -Ωy Ωx   0 vz]
 * [   0  0   0  0]
 *
 * Constraints
 * -----------
 * Group:   qx * qx + qy * qy + qz * qz + qw * qw = 1
 * Tangent: -pi < Ωx Ωy Ωz <= pi
 */
template<typename _Scalar>
class SE3Impl
{
public:
  using Scalar = _Scalar;

  static constexpr Eigen::Index RepSize = 7;
  static constexpr Eigen::Index Dim     = 4;
  static constexpr Eigen::Index Dof     = 6;

  SMOOTH_DEFINE_REFS;

  static void setIdentity(GRefOut g_out)
  {
    g_out.template head<6>().setZero();
    g_out(6) = Scalar(1);
  }

  static void setRandom(GRefOut g_out)
  {
    g_out.template head<3>().setRandom();
    SO3Impl<Scalar>::setRandom(g_out.template tail<4>());
  }

  static void matrix(GRefIn g_in, MRefOut m_out)
  {
    m_out.setIdentity();
    SO3Impl<Scalar>::matrix(g_in.template tail<4>(), m_out.template topLeftCorner<3, 3>());
    m_out.template topRightCorner<3, 1>() = g_in.template head<3>();
  }

  static void composition(GRefIn g_in1, GRefIn g_in2, GRefOut g_out)
  {
    SO3Impl<Scalar>::composition(
      g_in1.template tail<4>(), g_in2.template tail<4>(), g_out.template tail<4>());
    Eigen::Matrix<Scalar, 3, 3> R1;
    SO3Impl<Scalar>::matrix(g_in1.template tail<4>(), R1);
    g_out.template head<3>() = R1 * g_in2.template head<3>() + g_in1.template head<3>();
  }

  static void inverse(GRefIn g_in, GRefOut g_out)
  {
    Eigen::Matrix<Scalar, 4, 1> so3inv;
    SO3Impl<Scalar>::inverse(g_in.template tail<4>(), so3inv);

    Eigen::Matrix<Scalar, 3, 3> Rinv;
    SO3Impl<Scalar>::matrix(so3inv, Rinv);

    g_out.template head<3>() = -Rinv * g_in.template head<3>();
    g_out.template tail<4>() = so3inv;
  }

  static void log(GRefIn g_in, TRefOut a_out)
  {
    using SO3TangentMap = Eigen::Matrix<Scalar, 3, 3>;

    SO3Impl<Scalar>::log(g_in.template tail<4>(), a_out.template tail<3>());

    SO3TangentMap M_dr_expinv, M_ad;
    SO3Impl<Scalar>::dr_expinv(a_out.template tail<3>(), M_dr_expinv);
    SO3Impl<Scalar>::ad(a_out.template tail<3>(), M_ad);
    a_out.template head<3>() = (-M_ad + M_dr_expinv) * g_in.template head<3>();
  }

  static void Ad(GRefIn g_in, TMapRefOut A_out)
  {

    SO3Impl<Scalar>::matrix(g_in.template tail<4>(), A_out.template topLeftCorner<3, 3>());
    SO3Impl<Scalar>::hat(g_in.template head<3>(), A_out.template topRightCorner<3, 3>());
    A_out.template topRightCorner<3, 3>() *= A_out.template topLeftCorner<3, 3>();
    A_out.template bottomRightCorner<3, 3>() = A_out.template topLeftCorner<3, 3>();
    A_out.template bottomLeftCorner<3, 3>().setZero();
  }

  static void exp(TRefIn a_in, GRefOut g_out)
  {
    using SO3TangentMap = Eigen::Matrix<Scalar, 3, 3>;

    SO3Impl<Scalar>::exp(a_in.template tail<3>(), g_out.template tail<4>());

    SO3TangentMap M_dr_exp, M_Ad;
    SO3Impl<Scalar>::dr_exp(a_in.template tail<3>(), M_dr_exp);
    SO3Impl<Scalar>::Ad(g_out.template tail<4>(), M_Ad);

    g_out.template head<3>() = M_Ad * M_dr_exp * a_in.template head<3>();
  }

  static void hat(TRefIn a_in, MRefOut A_out)
  {
    A_out.setZero();
    SO3Impl<Scalar>::hat(a_in.template tail<3>(), A_out.template topLeftCorner<3, 3>());
    A_out.template topRightCorner<3, 1>() = a_in.template head<3>();
  }

  static void vee(MRefIn A_in, TRefOut a_out)
  {
    SO3Impl<Scalar>::vee(A_in.template topLeftCorner<3, 3>(), a_out.template tail<3>());
    a_out.template head<3>() = A_in.template topRightCorner<3, 1>();
  }

  static void ad(TRefIn a_in, TMapRefOut A_out)
  {
    SO3Impl<Scalar>::hat(a_in.template tail<3>(), A_out.template topLeftCorner<3, 3>());
    SO3Impl<Scalar>::hat(a_in.template head<3>(), A_out.template topRightCorner<3, 3>());
    A_out.template bottomRightCorner<3, 3>() = A_out.template topLeftCorner<3, 3>();
    A_out.template bottomLeftCorner<3, 3>().setZero();
  }

  static Eigen::Matrix<Scalar, 3, 3> calculate_q(TRefIn a)
  {
    using std::abs, std::sqrt, std::cos, std::sin;

    const Scalar th2 = a.template tail<3>().squaredNorm();

    Scalar A, B, C;
    if (th2 < Scalar(eps2)) {
      // https://www.wolframalpha.com/input/?i=series+%28x+-+sin+x%29+%2F+x%5E3+at+x%3D0
      A = Scalar(1) / Scalar(6) - th2 / Scalar(120);
      // https://www.wolframalpha.com/input/?i=series+%28cos+x+-+1+%2B+x%5E2%2F2%29+%2F+x%5E4+at+x%3D0
      B = Scalar(1) / Scalar(24) - th2 / Scalar(720);
      // https://www.wolframalpha.com/input/?i=series+%28x+-+sin+x+-+x%5E3%2F6%29+%2F+x%5E5+at+x%3D0
      C = -Scalar(1) / Scalar(120) + th2 / Scalar(5040);
    } else {
      const Scalar th = sqrt(th2), th_4 = th2 * th2, cTh = cos(th), sTh = sin(th);
      A = (th - sTh) / (th * th2);
      B = (cTh - Scalar(1) + th2 / Scalar(2)) / th_4;
      C = (th - sTh - th * th2 / Scalar(6)) / (th_4 * th);
    }

    Eigen::Matrix<Scalar, 3, 3> V, W;
    SO3Impl<Scalar>::hat(a.template head<3>(), V);
    SO3Impl<Scalar>::hat(a.template tail<3>(), W);

    const Scalar vdw                     = a.template tail<3>().dot(a.template head<3>());
    const Eigen::Matrix<Scalar, 3, 3> WV = W * V, VW = V * W, WW = W * W;

    return Scalar(0.5) * V + A * (WV + VW - vdw * W)
         + B * (W * WV + VW * W + vdw * (Scalar(3) * W - WW)) - C * Scalar(3) * vdw * WW;
  }

  static void dr_exp(TRefIn a_in, TMapRefOut A_out)
  {
    SO3Impl<Scalar>::dr_exp(a_in.template tail<3>(), A_out.template topLeftCorner<3, 3>());
    A_out.template topRightCorner<3, 3>()    = calculate_q(-a_in);
    A_out.template bottomRightCorner<3, 3>() = A_out.template topLeftCorner<3, 3>();
    A_out.template bottomLeftCorner<3, 3>().setZero();
  }

  static void dr_expinv(TRefIn a_in, TMapRefOut A_out)
  {
    SO3Impl<Scalar>::dr_expinv(a_in.template tail<3>(), A_out.template topLeftCorner<3, 3>());
    A_out.template topRightCorner<3, 3>() = -A_out.template topLeftCorner<3, 3>()
                                          * calculate_q(-a_in)
                                          * A_out.template topLeftCorner<3, 3>();
    A_out.template bottomRightCorner<3, 3>() = A_out.template topLeftCorner<3, 3>();
    A_out.template bottomLeftCorner<3, 3>().setZero();
  }
};

}  // namespace smooth

#endif  // SMOOTH__IMPL__SE3_HPP_


#include <Eigen/Dense>
#include <vector>
#include <iostream>
#include <fstream>
#include "tsai.hpp"

#ifdef USEVISP
#include <visp/vpCalibration.h>
#include <visp/vpMath.h>
#include <visp/vpPose.h>
#include <visp/vpPixelMeterConversion.h>
#endif
//using LTD = SE3dist<float>;
using LTD = float;

#ifdef USEVISP

void assign(Eigen::Matrix4f & a, const 	vpHomogeneousMatrix & b)
{
	vpQuaternionVector q;
	vpTranslationVector t;
	b.extract(q);
	b.extract(t);
	Eigen::Vector3f et(t[0],t[1],t[2]);
	Eigen::Quaternionf eq(q.w(),q.x(),q.y(),q.z());
	a.setIdentity();
	a.block<3,3>(0,0) = Eigen::Matrix3f(eq);
	a.block<3,1>(0,3) = et;
}

void assign(vpHomogeneousMatrix & b,const Eigen::Matrix4f & a)
{
	Eigen::Quaternionf eq(Eigen::Matrix3f(a.block<3,3>(0,0)));
	Eigen::Vector3f et = a.block<3,1>(0,3);

	vpQuaternionVector q(eq.x(),eq.y(),eq.z(),eq.w());
	vpTranslationVector t(et.x(),et.y(),et.z());
	b.buildFrom(t,q);
}

void calibrationTsai(std::vector<vpHomogeneousMatrix>& cMo,
						std::vector<vpHomogeneousMatrix>& rMe,
						vpHomogeneousMatrix &eMc){

  vpColVector x ;
  unsigned int nbPose = (unsigned int)cMo.size();
  if(cMo.size()!=rMe.size()) throw vpCalibrationException(vpCalibrationException::dimensionError,"cMo and rMe have different sizes");
  {
    vpMatrix A ;
    vpColVector B ;
    unsigned int k = 0 ;
    // for all couples ij
    for (unsigned int i=0 ; i < nbPose ; i++)
    {
      vpRotationMatrix rRei, ciRo ;
      rMe[i].extract(rRei) ;
      cMo[i].extract(ciRo) ;
      //std::cout << "rMei: " << std::endl << rMe[i] << std::endl;

      for (unsigned int j=i+1; j < nbPose ; j++)
      {
        {
          vpRotationMatrix rRej, cjRo ;
          rMe[j].extract(rRej) ;
          cMo[j].extract(cjRo) ;
	  //std::cout << "rMej: " << std::endl << rMe[j] << std::endl;

          vpRotationMatrix rReij = rRej.t() * rRei;

          vpRotationMatrix cijRo = cjRo * ciRo.t();

          vpThetaUVector rPeij(rReij);

          double theta = sqrt(rPeij[0]*rPeij[0] + rPeij[1]*rPeij[1]
                              + rPeij[2]*rPeij[2]);

          for (unsigned int m=0;m<3;m++) rPeij[m] = rPeij[m] * vpMath::sinc(theta/2);

          vpThetaUVector cijPo(cijRo) ;
          theta = sqrt(cijPo[0]*cijPo[0] + cijPo[1]*cijPo[1]
                       + cijPo[2]*cijPo[2]);
          for (unsigned int m=0;m<3;m++) cijPo[m] = cijPo[m] * vpMath::sinc(theta/2);

          vpMatrix As;
          vpColVector b(3) ;

          As = vpColVector::skew(vpColVector(rPeij) + vpColVector(cijPo)) ;

          b =  (vpColVector)cijPo - (vpColVector)rPeij ;           // A.40

          if (k==0)
          {
            A = As ;
            B = b ;
#if 0
            auto w = rReij.t();
            std::cout << "rPeij0 R\n";
            std::cout << w.getCol(0) << "\n" << w.getCol(1) << "\n" << w.getCol(2) << std::endl;
            std::cout << "rPeij0 is " << rPeij[0] << " " << rPeij[1] << " " << rPeij[2] << std::endl;
            std::cout << "cijPo0 is " << cijPo[0] << " " << cijPo[1] << " " << cijPo[2] << std::endl;
            std::cout << "As0 is \n ";
            As.csvPrint (std::cout);
            std::cout<< "b0 " << b[0] << " " << b[1] << " " << b[2] << std::endl;
#endif
          }
          else
          {
            A = vpMatrix::stack(A,As) ;
            B = vpColVector::stack(B,b) ;
          }
          k++ ;
        }
      }
    }

    // the linear system is defined
    // x = AtA^-1AtB is solved
    vpMatrix AtA = A.AtA() ;

    vpMatrix Ap ;
    AtA.pseudoInverse(Ap, 1e-6) ; // rank 3
    x = Ap*A.t()*B ;

//     {
//       // Residual
//       vpColVector residual;
//       residual = A*x-B;
//       std::cout << "Residual: " << std::endl << residual << std::endl;

//       double res = 0;
//       for (int i=0; i < residual.getRows(); i++)
// 	res += residual[i]*residual[i];
//       res = sqrt(res/residual.getRows());
//       printf("Mean residual = %lf\n",res);
//     }

    // extraction of theta and U
    double theta ;
    double   d=x.sumSquare() ;
    for (unsigned int i=0 ; i < 3 ; i++) x[i] = 2*x[i]/sqrt(1+d) ;
    theta = sqrt(x.sumSquare())/2 ;
    theta = 2*asin(theta) ;
    //if (theta !=0)
    if (std::fabs(theta) > std::numeric_limits<double>::epsilon())
    {
      for (unsigned int i=0 ; i < 3 ; i++) x[i] *= theta/(2*sin(theta/2)) ;
    }
    else
      x = 0 ;
  }

  // Building of the rotation matrix eRc
  vpThetaUVector xP(x[0],x[1],x[2]);
  vpRotationMatrix eRc(xP);

  {
    vpMatrix A ;
    vpColVector B ;
    // Building of the system for the translation estimation
    // for all couples ij
    vpRotationMatrix I3 ;
    I3.eye() ;
    int k = 0 ;
    for (unsigned int i=0 ; i < nbPose ; i++)
    {
      vpRotationMatrix rRei, ciRo ;
      vpTranslationVector rTei, ciTo ;
      rMe[i].extract(rRei) ;
      cMo[i].extract(ciRo) ;
      rMe[i].extract(rTei) ;
      cMo[i].extract(ciTo) ;

      for (unsigned int j=i+1 ; j < nbPose ; j++)
      {
        {

          vpRotationMatrix rRej, cjRo ;
          rMe[j].extract(rRej) ;
          cMo[j].extract(cjRo) ;

          vpTranslationVector rTej, cjTo ;
          rMe[j].extract(rTej) ;
          cMo[j].extract(cjTo) ;

          vpRotationMatrix rReij = rRej.t() * rRei ;

          vpTranslationVector rTeij = rTej+ (-rTei);

          rTeij = rRej.t()*rTeij ;

          vpMatrix a = vpMatrix(rReij) - vpMatrix(I3);

          vpTranslationVector b ;
          b = eRc*cjTo - rReij*eRc*ciTo + rTeij ;

          if (k==0)
          {
            A = a ;
            B = b ;
          }
          else
          {
            A = vpMatrix::stack(A,a) ;
            B = vpColVector::stack(B,b) ;
          }
          k++ ;

        }
      }
    }

    // the linear system is solved
    // x = AtA^-1AtB is solved
    vpMatrix AtA = A.AtA() ;
    vpMatrix Ap ;
    vpColVector AeTc ;
    AtA.pseudoInverse(Ap, 1e-6) ;
    AeTc = Ap*A.t()*B ;

//     {
//       // residual
//       vpColVector residual;
//       residual = A*AeTc-B;
//       std::cout << "Residual: " << std::endl << residual << std::endl;
//       double res = 0;
//       for (int i=0; i < residual.getRows(); i++)
// 	res += residual[i]*residual[i];
//       res = sqrt(res/residual.getRows());
//       printf("mean residual = %lf\n",res);
//     }

    vpTranslationVector eTc(AeTc[0],AeTc[1],AeTc[2]);

    eMc.insert(eTc) ;
    eMc.insert(eRc) ;
  }
}

#endif
void out(std::ostream & onf, const Eigen::Matrix4f  & q)
{
	for(int i = 0; i < 16; i++)
		onf << q.data()[i] << ' ';
}

Eigen::Matrix4f makerandom(const LTD &ld)
{
  return Eigen::Matrix4f::Identity(); //ld.sample().asMatrix();
}

Eigen::Matrix<float,6,1> distance(Eigen::Matrix4f a, Eigen::Matrix4f b)
{
  return Eigen::Matrix<float,6,1> ::Zero();
/*	SE3group<float> ag(a);
	SE3group<float> bg(b);
	return ag.distance(bg).get();*/
}

// pos and ang
Eigen::Matrix<float,2,1> distance2(Eigen::Matrix4f a, Eigen::Matrix4f b)
{
    //auto q = SE3group<float>(a).distance(SE3group<float>(b)).get();
    return Eigen::Matrix<float,2,1>::Zero(); //{q.segment<3>(0).norm(),q.segment<3>(3).norm()};
}

// pos and ang
template <class T>
T distanceT(Eigen::Matrix<T,4,4> a, Eigen::Matrix<T,4,4> b)
{
    return (a*b.inverse()).template block<3,1>(0,3).norm();
}

template <class T>
Vector3<T> normalize(Vector3<T>  q)
{
	return q / q.norm();
}

int main(int argc, char const *argv[])
{
#if 0
	Eigen::AngleAxisf aa(0.001*M_PI,normalize(Eigen::Vector3f(0.2,0.3,0.4)));
	Eigen::Quaternionf q(aa);
	Eigen::Vector3f p = quat2paratsai(q);
	Eigen::Quaternionf Q = paratsai2quat(p);
	Eigen::Matrix3f R = paratsai2rot(p);
	Eigen::Quaternionf QR(R);
	Eigen::Vector3f pp = paratsai2paratsaiprime(p);
	Eigen::Vector3f ppp = paratsaiprime2paratsai(pp);

	float pangle = paratsai2theta(p);
	std::cout << "angle original  " << aa.angle() << std::endl;
	std::cout << "angle from para " << pangle << std::endl;
	std::cout << "angle from quat " << Eigen::AngleAxisf(Q).angle() << std::endl;
	std::cout << "angle from rot  " << Eigen::AngleAxisf(R).angle() << std::endl;
	std::cout << q << std::endl;
	std::cout << Q << std::endl;
	std::cout << QR << std::endl;
	std::cout << ppp.transpose() << std::endl;
	std::cout << p.transpose() << std::endl;
#endif

    if(argc < 2)
        return -1;
    using FT = double;
    std::ifstream inf(argv[1],std::ios::binary);
    std::vector<Eigen::Matrix<FT,4,4> > cMm,rMe;
    Eigen::Matrix<FT,4,4> m1fp,m2fp,m1f,m2f;
    std::cout << "assuming file of pairs of matrices: cMm and cMe" << std::endl;
    while(inf)
    {
        Eigen::Matrix<double,4,4,Eigen::RowMajor> m1,m2;
        inf.read((char*)m1.data(),16*sizeof(double));
        if(!inf)
            break;
        inf.read((char*)m2.data(),16*sizeof(double));
        if(!inf)
            break;
        // data is stored rowmajor
        m1f = m1.cast<FT>();
        m2f = m2.cast<FT>();
        cMm.push_back(m1f);
        rMe.push_back(m2f);
        if(cMm.size() == 1)
        {
            std::cout << "m1[0] is\n" << m1f << std::endl;
            std::cout << "m2[0] is\n" << m2f << std::endl;
        }
        else
        {
            // previous is valid

            // use 2D (pos,rot) or 1D (pos) distance
            //std::cout << "Diff m1: " << distance2(m1f,m1fp).transpose() << " Diff m2: " << distance2(m2f,m2fp).transpose() << std::endl;
            std::cout << "Diff m1: " << distanceT(m1f,m1fp) << " Diff m2: " << distanceT(m2f,m2fp) << std::endl;
        }
        m1fp = m1f;
        m2fp = m2f;

    }
    Eigen::Matrix<FT,4,4> r;

        FT res = calibrationTsai(cMm,rMe,r);

        std::cout << "Output:   \n" << r << std::endl;
        std::cout << "Residual:   \n" << res << std::endl;
        /*std::cout << "Expected: \n" << ec << std::endl;
	std::cout << "THIS diff:  " << distance(r,ec).transpose() << std::endl;
	std::cout << "!THIS error: " << distance(r,ec).transpose().norm() << std::endl;
    */
#ifdef USEVISP
	vpCalibration c;
	std::vector<vpHomogeneousMatrix> vcMo(cMm.size());
	std::vector<vpHomogeneousMatrix> vrMe(cMm.size());
	vpHomogeneousMatrix veMc;
    Eigen::Matrix4f eMc;
	for(int i = 0; i < cMm.size(); i++)
	{
		assign(vcMo[i],cMm[i].cast<float>());
		assign(vrMe[i],rMe[i].cast<float>());
	}
         calibrationTsai(vcMo,vrMe,veMc);
	assign(eMc,veMc);

        std::cout << "MY output res:\n" << res << std::endl;
        std::cout << "VISP output:\n" << eMc << std::endl;
  std::ofstream onf("tsaiout.bin",std::ios::binary);
  Matrix4<double> out(eMc.cast<FT>());
  onf.write((char*)out.data(),sizeof(double)*16);
/*	std::cout << "Expected:   \n" << ec << std::endl;
	std::cout << "VISP diff:  " << distance(eMc,ec).transpose() << std::endl;
	std::cout << "!VISP error:" << distance(eMc,ec).transpose().norm() << std::endl;
*/
#endif
#if 0
	if(argc == 2)
	{
		std::ofstream onf(argv[1]);
		out(onf,ec);		onf << std::endl;
		out(onf,r);		onf << std::endl;
		onf << cMm.size() << std::endl;
		for(int i = 0; i < cMm.size(); i++)
		{
			out(onf,cMm[i]);
			onf << std::endl;
		}
		for(int i = 0; i < rMe.size(); i++)
		{
			out(onf,rMe[i]);
			onf << std::endl;
		}
	}
#endif
	return 0;
}

/*
 * 0.000  1.000  0.000  0.038
-1.000 -0.000  0.000  0.012
0.000  0.000  1.000  0.015
0.000  0.000  0.000  1.000
*/


#include "Util.h"
#include "Matrix.h"

#include <random>
#include <time.h>
#include <cmath>
#include <fstream>
#include <float.h>

#include <opencv2/opencv.hpp>
#include <boost/tokenizer.hpp>

using namespace std;

int seed = time(0);
std::default_random_engine random_engine(seed);

namespace fns{
	double relu(double x){
		if(x > 0) return x;
		else return (double) 0;
	}
	double sigmoid(double x){
		return (1.0/(1.0 + exp(-x)));
	}
	double tan(double x){
		return tanh(x);
	}
	double relu_gradient(double x){
		if(x > 0) return (double) 1;
		else return (double) 0.2;
	}
	double sigmoid_gradient(double x){
		return (x*(1-x));
	}
	double tan_gradient(double x){
		return (1-(x*x));
	}
	double softmax(double x){
		if(isnan(x)) return 0;
		return exp(x);
	}
}

namespace pre_process{
	int process_mnist_images(const char* path, std::vector<std::unique_ptr<Matrix> > &Xtrain,
		std::vector<std::unique_ptr<std::vector<double> > > &Ytrain, unsigned int nr_images){
		std::string str(path);	// convert char* to string
		const int width = 28;
		const int height = 28;
		const int LABELS = 10;

		for(unsigned int i=0; i < LABELS; i++){
			std::vector<cv::String> files;	// vector of strings to store file names
			cv::glob(path + std::to_string(i), files, true);
				// true means recursively read from path
			for(unsigned int k=0; k < (nr_images/LABELS); k++){
				cv::Mat img = cv::imread(files[k]);
				if(img.empty()) continue;	//only proceed further if the file is not empty
				std::unique_ptr<Matrix> image = std::make_unique<Matrix>(width, height, true);
				for(unsigned int h=0; h<height; h++){
					for(unsigned int w=0; w<width; w++){
						image->set(h,w,(double)(img.at<uchar>(h,w)/255.0));
					}
				}
				Xtrain.emplace_back(std::move(image));
				std::unique_ptr<std::vector<double> > vr = std::make_unique<std::vector<double> >(LABELS, 0);
				(*vr)[i] = 1.0;
				Ytrain.emplace_back(std::move(vr));
			}
		}
		return 0;
	}

	int process_mnist_csv(const char* filename, std::vector<std::vector<double> > &Xtrain,
		std::vector<std::vector<double> > &Ytrain){
		std::string data(filename);
		ifstream in(data.c_str());

		if(!in.is_open()) return 1;

		typedef boost::tokenizer< boost::escaped_list_separator<char> > Tokenizer;
		std::vector<std::string> svec;
		std::string line;

		while(getline(in, line)){
			Tokenizer tok(line);
			auto it = tok.begin();
			int label = std::stoi(*it);
			std::vector<double> labels(10, 0.0);
			labels[label] = 1.0;

			svec.assign(std::next(it, 1), tok.end());

			std::vector<double> dvec(svec.size());
			std::transform(svec.begin(), svec.end(), dvec.begin(), [](const std::string& val)
			{
				return (std::stod(val)/255); // divide by 255 for normalization, since each pixel is 8 bit
			});

			Xtrain.push_back(dvec);
			Ytrain.push_back(labels);
		}
		cout << "processed the input file" << endl;
		return 0;
	}

	void process_image(const char* filename){
		std::vector<double> image;
		cv::Mat img = cv::imread(filename);
		if(img.empty()){
			std::cout << "No Image" << std::endl;
		}
		else{
			if(img.isContinuous()){
				image.assign(img.datastart, img.dataend);
				for(unsigned int j=0; j < image.size(); j++){
					cout << image[j] << " " ;
				}
				cout << endl << image.size();
			}
			else{
				std::cout << "Not Continous !" << std::endl;
			}
		}
	}
}


//    Copyright 2017 Rainer Gemulla
//
//    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.
/** \file
 *
 * Illustrates matrix factorization with Lee's GKL method. We first creates factors and then a data matrix
 * from these factors. THis process ensures that we know the best factorization of the input.
 * We then try to reconstruct the factors.
 */
#include <iostream>

#include <boost/math/distributions/normal.hpp>
#include <boost/random/uniform_real.hpp>

#include <util/evaluation.h>
#include <util/io.h>

#include <mf/mf.h>

using namespace std;
using namespace mf;
using namespace rg;
using namespace boost::numeric::ublas;

log4cxx::LoggerPtr logger(log4cxx::Logger::getLogger("main"));

int main(int argc, char* argv[]) {
#ifndef NDEBUG
	LOG4CXX_WARN(logger, "Warning: Debug mode activated (runtimes may be slow).");
#endif

	// parameters for the factorization
	mf_size_type size1 = 10000;
	mf_size_type size2 = 10000;
	mf_size_type nnz = 5000000;
	mf_size_type rank = 10;

	// parameters for Lee01
	unsigned epochs = 20;

	// generate original factors by sampling from a uniform[0,1] distribution
	Random32 random; // note: this takes a default seed (not randomized!)
	DenseMatrix wIn(size1, rank);
	DenseMatrixCM hIn(rank, size2);
	generateRandom(wIn, random,  boost::uniform_real<>(0, 1));
	generateRandom(hIn, random, boost::uniform_real<>(0, 1));
	// div2(wIn, sums2(wIn));
	// div1(hIn, sums1(hIn));

	// generate a sparse matrix by selecting random entries from the generated factors
	// and sample from a Poisson with mean equal to the entry
	// TODO: this generation process does not match the factorization model since we sample
	//       from the Poisson only at some entries of wh
	SparseMatrix v;
	generateRandom(v, nnz, wIn, hIn, random);
	applyPoisson(v, random);
	LOG4CXX_INFO(logger, "Data matrix: "
		<< v.size1() << " x " << v.size2() << ", " << v.nnz() << " nonzeros");
	LOG4CXX_INFO(logger, "Loss with original factors: " << gkl(v, wIn, hIn));

	// generate initial factors by sampling from a uniform[0,1] distribution
	DenseMatrix w(size1, rank);
	DenseMatrixCM h(rank, size2);
	generateRandom(w, random, boost::uniform_real<>(0, 1));
	generateRandom(h, random, boost::uniform_real<>(0, 1));
	div2(w, sums2(w));
	div1(h, sums1(h));
	double scaleFactor = sqrt(sum(v));
	mult(w, scaleFactor);
	mult(h, scaleFactor);

	// perform the factorization
	FactorizationData<> data(v, w, h);
	Trace trace;
	lee01Gkl(data, epochs, trace);

	// write the trace
	LOG4CXX_INFO(logger, "Writing trace to " << "/tmp/lee01-gkl-trace.R");
	trace.toRfile("/tmp/lee01-gkl-trace.R", "lee01.gkl");

	return 0;
}


#include <boost/test/unit_test.hpp>
#include "algorithms/data_structures/array/pascals_triangle.hpp"

BOOST_AUTO_TEST_SUITE(TestPascalTriangles)

BOOST_AUTO_TEST_CASE(invalid_row_num)
{
    const std::vector<std::vector<int>> expected;
    BOOST_CHECK(expected == Algo::DS::Array::PascalsTriangle::Generate(-1));
    BOOST_CHECK(expected == Algo::DS::Array::PascalsTriangle::Generate(0));
}

BOOST_AUTO_TEST_CASE(valid_row_num)
{
    {
        const std::vector<std::vector<int>> expected = {{1}};
        BOOST_CHECK(expected == Algo::DS::Array::PascalsTriangle::Generate(1));
    }

    {
        const std::vector<std::vector<int>> expected = {
            {1},
            {1, 1}
        };

        BOOST_CHECK(expected == Algo::DS::Array::PascalsTriangle::Generate(2));
    }

    {
        const std::vector<std::vector<int>> expected = {
            {1},
            {1, 1},
            {1, 2, 1}
        };

        BOOST_CHECK(expected == Algo::DS::Array::PascalsTriangle::Generate(3));
    }

    {
        const std::vector<std::vector<int>> expected = {
            {1},
            {1, 1},
            {1, 2, 1},
            {1, 3, 3, 1}
        };

        BOOST_CHECK(expected == Algo::DS::Array::PascalsTriangle::Generate(4));
    }

    {
        const std::vector<std::vector<int>> expected = {
            {1},
            {1, 1},
            {1, 2, 1},
            {1, 3, 3, 1},
            {1, 4, 6, 4, 1}
        };

        BOOST_CHECK(expected == Algo::DS::Array::PascalsTriangle::Generate(5));
    }
}

BOOST_AUTO_TEST_CASE(rowgen_invalid_row_num) {
    const std::vector<int> expected;
    BOOST_CHECK(expected == Algo::DS::Array::PascalsTriangle::GenerateRow(-1));
    BOOST_CHECK(expected == Algo::DS::Array::PascalsTriangle::GenerateRow(-85));
}

BOOST_AUTO_TEST_CASE(rowgen_valid_row_num) {
    {
        const std::vector<int> expected = {1};
        BOOST_CHECK(expected ==
                    Algo::DS::Array::PascalsTriangle::GenerateRow(0));
    }

    {
        const std::vector<int> expected = {1, 1};
        BOOST_CHECK(expected ==
                    Algo::DS::Array::PascalsTriangle::GenerateRow(1));
    }

    {
        const std::vector<int> expected = {1, 2, 1};
        BOOST_CHECK(expected ==
                    Algo::DS::Array::PascalsTriangle::GenerateRow(2));
    }

    {
        const std::vector<int> expected = {1, 4, 6, 4, 1};
        BOOST_CHECK(expected ==
                    Algo::DS::Array::PascalsTriangle::GenerateRow(4));
    }
}

BOOST_AUTO_TEST_SUITE_END()


#include <stan/math/rev/scal.hpp>
#include <gtest/gtest.h>
#include <math/rev/scal/fun/nan_util.hpp>
#include <math/rev/scal/util.hpp>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/zeta.hpp>

TEST(AgradRev, digamma) {
  AVAR a = 0.5;
  AVAR f = digamma(a);
  EXPECT_FLOAT_EQ(boost::math::digamma(0.5), f.val());

  AVEC x = createAVEC(a);
  VEC grad_f;
  f.grad(x, grad_f);
  EXPECT_FLOAT_EQ(4.9348022005446793094, grad_f[0]);
}

namespace {
struct digamma_fun {
  template <typename T0>
  inline T0 operator()(const T0& arg1) const {
    return digamma(arg1);
  }
};
}  // namespace

TEST(AgradRev, digamma_NaN) {
  digamma_fun digamma_;
  test_nan(digamma_, false, true);
}

TEST(AgradRev, check_varis_on_stack_10) {
  AVAR a = 0.5;
  test::check_varis_on_stack(stan::math::digamma(a));
}


/*
 * Copyright 2020 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
 */

/*
 * File:   CartesianToGeodeticConversionTest.hpp
 * Author: jordan
 */

#ifndef CARTESIANTOGEODETICCONVERSIONTEST_HPP
#define CARTESIANTOGEODETICCONVERSIONTEST_HPP

#include "catch.hpp"
#include <Eigen/Dense>
#include "../src/Position.hpp"
#include "../src/math/CartesianToGeodeticFukushima.hpp"
#include "../src/math/CoordinateTransform.hpp"
#include "../src/utils/Constants.hpp"

TEST_CASE("Center of the Earth Degenerate Cases of Cartesian To Geodetic Conversion Test") {
    double latitudeEps = 1e-9;
    double longitudeEps = 1e-9;
    double altitudeEps = 1e-6;

    unsigned int numberOfIterations = 1;
    CartesianToGeodeticFukushima cart2geo(numberOfIterations);

    // Begin center of earth test
    Position centerOfEarth(0, 0.0, 0.0, 0.0);

    Eigen::Vector3d centerOfEarthTRF;
    CoordinateTransform::getPositionECEF(centerOfEarthTRF, centerOfEarth);

    Position testCenterOfEarthPosition(0,0.0,0.0,0.0);
    cart2geo.ecefToLongitudeLatitudeElevation(centerOfEarthTRF, testCenterOfEarthPosition);

    REQUIRE(std::abs(testCenterOfEarthPosition.getLatitude() - centerOfEarth.getLatitude()) < latitudeEps);
    REQUIRE(std::abs(testCenterOfEarthPosition.getLongitude() - centerOfEarth.getLongitude()) < longitudeEps);
    REQUIRE(std::abs(testCenterOfEarthPosition.getEllipsoidalHeight() - centerOfEarth.getEllipsoidalHeight()) < altitudeEps);
}

TEST_CASE("Equator Degenerate Cases of Cartesian To Geodetic Conversion Test") {

    double longitudeEpsilon = 1e-12;
    double latitudeEpsilon = 1e-12;

    int longitudeIncrement = 1;

    bool testFail = false;

    Position p(0,0.0,0.0,0.0);
    Eigen::Vector3d pTRF;
    Position testPosition(0,0.0,0.0,0.0); // must be equal to p in order for test to pass

    unsigned int numberOfIterations = 1;
    CartesianToGeodeticFukushima cart2geo(numberOfIterations);

    for (int longitude = -180; longitude <= 180; longitude += longitudeIncrement) {
        if (testFail) {
            break;
        }

        p.setLatitude(0.0);
        p.setLongitude(longitude);
        p.setEllipsoidalHeight(0);

        CoordinateTransform::getPositionECEF(pTRF, p);

        cart2geo.ecefToLongitudeLatitudeElevation(pTRF, testPosition);

        if (std::abs(longitude - testPosition.getLongitude()) > longitudeEpsilon) {
            testFail = true;
        }

        if (std::abs(0.0 - testPosition.getLatitude()) > latitudeEpsilon) {
            testFail = true;
        }
    }

    if (testFail) {
        std::cout << "Equator degenerate case for Cartesian To Geodetic conversion test:" << std::endl;
        std::cout << "What is expected:" << std::endl;
        std::cout << p << std::endl;
        std::cout << "What is obtained:" << std::endl;
        std::cout << testPosition << std::endl;
    }

}

TEST_CASE("North and South Pole Degenerate Cases of Cartesian To Geodetic Conversion Test") {

    double latitudeEps = 1e-9;
    double longitudeEps = 1e-9;
    double altitudeEps = 1e-6;

    unsigned int numberOfIterations = 1;
    CartesianToGeodeticFukushima cart2geo(numberOfIterations);

    // Begin north pole test
    Position northPole(0, 90.0, 0.0, 0);

    Eigen::Vector3d northPoleTRF;
    CoordinateTransform::getPositionECEF(northPoleTRF, northPole);

    Position testNorthPolePosition(0,0.0,0.0,0.0);
    cart2geo.ecefToLongitudeLatitudeElevation(northPoleTRF, testNorthPolePosition);

    REQUIRE(std::abs(testNorthPolePosition.getLatitude() - northPole.getLatitude()) < latitudeEps);
    REQUIRE(std::abs(testNorthPolePosition.getLongitude() - northPole.getLongitude()) < longitudeEps);
    REQUIRE(std::abs(testNorthPolePosition.getEllipsoidalHeight() - northPole.getEllipsoidalHeight()) < altitudeEps);

    // Begin south pole test
    Position southPole(0, -90.0, 0.0, 0);

    Eigen::Vector3d southPoleTRF;
    CoordinateTransform::getPositionECEF(southPoleTRF, southPole);

    Position testSouthPolePosition(0,0.0,0.0,0.0);
    cart2geo.ecefToLongitudeLatitudeElevation(southPoleTRF, testSouthPolePosition);

    REQUIRE(std::abs(testSouthPolePosition.getLatitude() - southPole.getLatitude()) < latitudeEps);
    REQUIRE(std::abs(testSouthPolePosition.getLongitude() - southPole.getLongitude()) < longitudeEps);
    REQUIRE(std::abs(testSouthPolePosition.getEllipsoidalHeight() - southPole.getEllipsoidalHeight()) < altitudeEps);

}

TEST_CASE("Cartesian To Geodetic Conversion Test") {

    double longitudeEpsilon = 1e-13;
    double latitudeEpsilon = 1e-13;
    double heightEpsilon = 1e-7;

    std::cout << std::setprecision(15) << std::endl;

    unsigned int numberOfIterations = 2;
    CartesianToGeodeticFukushima cart2geo(numberOfIterations);

    int longitudeIncrement = 2;
    int latitudeIncrement = 2;
    int heightIncrement = 50;

    double a = M_PI_2;

    if (a > M_PI_2) {
        std::cout << "a > PI/2" << std::endl;
    }
    if (-a < -M_PI_2) {
        std::cout << "-a < -PI/2" << std::endl;
    }

    bool testFail = false;

    Position p(0,0.0,0.0,0.0);
    Eigen::Vector3d pTRF;
    Position testPosition(0,0.0,0.0,0.0);

    for (int longitude = -180; longitude <= 180; longitude += longitudeIncrement) {
        if (testFail) {
            break;
        }

        for (int latitude = -89; latitude <= 89; latitude += latitudeIncrement) {
            if (testFail) {
                break;
            }

            for (int height = -1000; height <= 1000; height += heightIncrement) {
                if (testFail) {
                    break;
                }

                p.setLatitude(latitude);
                p.setLongitude(longitude);
                p.setEllipsoidalHeight(height);

                CoordinateTransform::getPositionECEF(pTRF, p);

                cart2geo.ecefToLongitudeLatitudeElevation(pTRF, testPosition);

                if (std::abs(testPosition.getLatitude() - latitude) > latitudeEpsilon) {
                    std::cout << "Latitude mismatch for cartesian to geodetic conversion" << std::endl;
                    std::cout << testPosition.getLatitude() << " should be " << latitude << std::endl;
                    testFail = true;
                }

                if (std::abs(testPosition.getLongitude() - longitude) > longitudeEpsilon) {
                    std::cout << "Longitude mismatch for cartesian to geodetic conversion" << std::endl;
                    std::cout << testPosition.getLongitude() << " should be " << longitude << std::endl;
                    testFail = true;
                }

                if (std::abs(testPosition.getEllipsoidalHeight() - height) > heightEpsilon) {
                    std::cout << "Height mismatch for cartesian to geodetic conversion" << std::endl;
                    std::cout << testPosition.getEllipsoidalHeight() << " should be " << height << std::endl;
                    testFail = true;
                }
            } // height
        } // latitude
    } // longitude

    if (testFail) {
        std::cout << "Cartesian To Geodetic Conversion Test Failure:" << std::endl;
        std::cout << "Expected position:" << std::endl;
        std::cout << p << std::endl;
        std::cout << "What is obtained:" << std::endl;
        std::cout << testPosition << std::endl;
    }

    REQUIRE(!testFail);
}

TEST_CASE("test the ecef conversion to longitude latitude elevation")
{
    Position result(0,0.0,0.0,0.0);
    Eigen::Vector3d ecefPosition(0.0,0.0,0.0);
    CartesianToGeodeticFukushima convertTest(0);
    convertTest.ecefToLongitudeLatitudeElevation(ecefPosition,result);
    REQUIRE(abs(result.getLatitude()-(0.0*R2D))<1e-10);
    REQUIRE(abs(result.getLongitude()-(0.0*R2D))<1e-10);
    REQUIRE(abs(result.getEllipsoidalHeight()-(0.0))<1e-10);
    ecefPosition(0) = 0.0;
    ecefPosition(1) = 0.0;
    ecefPosition(2) = 1.0;
    convertTest.ecefToLongitudeLatitudeElevation(ecefPosition,result);
    REQUIRE(abs(result.getLatitude()-(M_PI_2*R2D))<1e-10);
    REQUIRE(abs(result.getLongitude()-(0.0*R2D))<1e-10);
    REQUIRE(abs(result.getEllipsoidalHeight()-(1-(a_wgs84*(std::sqrt(1-e2_wgs84)))))<1e-10);
    ecefPosition(0) = 0.0;
    ecefPosition(1) = 0.0;
    ecefPosition(2) = -1.0;
    convertTest.ecefToLongitudeLatitudeElevation(ecefPosition,result);
    REQUIRE(abs(result.getLatitude()-(-M_PI_2*R2D))<1e-10);
    REQUIRE(abs(result.getLongitude()-(0.0*R2D))<1e-10);
    REQUIRE(abs(result.getEllipsoidalHeight()-(1-(a_wgs84*(std::sqrt(1-e2_wgs84)))))<1e-10);
}

#endif /* CARTESIANTOGEODETICCONVERSIONTEST_HPP */


//==================================================================================================
/*!
  @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_FUNCTION_COSPI_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_COSPI_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-trigonometric
    Function object implementing cospi capabilities

    cosine of angle in \f$\pi\f$ multiples: \f$\cos(\pi x)\f$.

    @par Semantic:

   The semantics of the function are similar to @ref cos ones.
    see @ref cos for further details

    @par Note

    However as it conveys a peculiar meaning,  unlike the orher cosine, cospi is defined
    for integral types and the result of cospi(n) coincides with \f$(-1)^n\f$.

    Take care that large floating entries are always integral and even !

    @see sincospi, cos, cosd

  **/
  Value cospi(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/cospi.hpp>
#include <boost/simd/function/simd/cospi.hpp>

#endif


#include <crave/SystemC.hpp>
#include <crave/ConstrainedRandom.hpp>
#include <systemc.h>
#include <boost/timer.hpp>

using crave::rand_obj;
using crave::randv;
using sc_dt::sc_bv;
using sc_dt::sc_uint;

struct ALU12 : public rand_obj {
  randv< sc_bv<2> >  op ;
  randv< sc_uint<12> > a, b ;

  ALU12()
  : op(this), a(this), b(this)
  {
    constraint ( (op() != 0x0) || ( 4095 >= a() + b() ) );
    constraint ( (op() != 0x1) || ((4095 >= a() - b()) && (b() <= a()) ) );
    constraint ( (op() != 0x2) || ( 4095 >= a() * b() ) );
    constraint ( (op() != 0x3) || ( b() != 0        ) );
  }

  friend std::ostream & operator<< (std::ostream & o, ALU12 const & alu)
  {
    o << alu.op
      << ' ' << alu.a
      << ' ' << alu.b
      ;
    return o;
  }
};

int sc_main (int argc, char** argv)
{
  boost::timer timer;
  ALU12 c;
  c.next();
  std::cout << "first: " << timer.elapsed() << "\n";
  for (int i=0; i<1000; ++i) {
    c.next();
  }
  std::cout << "complete: " << timer.elapsed() << "\n";
  return 0;
}


#pragma once
#include <Eigen/Geometry>
#include "mtao/types.hpp"

namespace mtao::geometry {
    template <bool OutputEdges=false, typename T=double, int D=3>
        auto  bounding_box_mesh(const Eigen::AlignedBox<T,D>& bb) {
            mtao::ColVectors<T,D> V(D,1<<D);
            if constexpr(D == 2) {
                mtao::ColVectors<int,2> E(2,4);
                V << bb.corner(Eigen::AlignedBox<T,D>::CornerType::BottomLeft),
                  bb.corner(Eigen::AlignedBox<T,D>::CornerType::BottomRight),
                  bb.corner(Eigen::AlignedBox<T,D>::CornerType::TopLeft),
                  bb.corner(Eigen::AlignedBox<T,D>::CornerType::TopRight);
                E << 0,0,3,2,
                     1,2,1,3;
                return std::make_tuple(V,E);
            } else {//D == 3
                mtao::ColVectors<int,3> F(3,12);
                V <<
                    bb.corner(Eigen::AlignedBox<T,D>::CornerType::BottomLeftFloor),
                    bb.corner(Eigen::AlignedBox<T,D>::CornerType::BottomRightFloor),
                    bb.corner(Eigen::AlignedBox<T,D>::CornerType::TopLeftFloor),
                    bb.corner(Eigen::AlignedBox<T,D>::CornerType::TopRightFloor),
                    bb.corner(Eigen::AlignedBox<T,D>::CornerType::BottomLeftCeil),
                    bb.corner(Eigen::AlignedBox<T,D>::CornerType::BottomRightCeil),
                    bb.corner(Eigen::AlignedBox<T,D>::CornerType::TopLeftCeil),
                    bb.corner(Eigen::AlignedBox<T,D>::CornerType::TopRightCeil);
                //0:000
                //1:010
                //2:100
                //3:110
                //4:001
                //5:011
                //6:101
                //7:111
                F <<
                    6,2,3,1,7,3,6,7,4,5,5,7,
                    0,0,0,0,2,2,4,4,0,0,1,1,
                    4,6,2,3,6,7,7,5,5,1,7,3;

                if constexpr(OutputEdges) {
                    mtao::ColVectors<int,2> E(2,12);
                    E << 0,0,0,1,1,2,2,3,4,4,5,6,
                      1,2,4,3,5,3,6,7,5,6,7,7;
                    return std::make_tuple(V,F,E);
                } else {
                    return std::make_tuple(V,F);
                }
            }
        }
}


// Maximum Subarray
#include <iostream>
#include <string>
#include <array>
#include <vector>
#include <deque>
#include <list>
#include <set>
#include <map>
#include <unordered_set>
#include <unordered_map>
#include <stack>
#include <queue>
#include <algorithm>
#include <numeric>
#include <cmath>
#include <cassert>
#define BOOST_TEST_MODULE Maximum Subarray
#include <boost/test/included/unit_test.hpp>

using namespace std;

class Solution {
public:
    int maxSubArray(vector<int>& nums) {
        int max_ending_here = nums[0];
        int max_so_far = nums[0];
        for (auto it=next(nums.cbegin()); it!=nums.cend(); ++it) {
            max_ending_here = max(*it, max_ending_here + *it);
            max_so_far = max(max_ending_here, max_so_far);
        }
        return max_so_far;

    }
};

BOOST_AUTO_TEST_CASE( my_test )
{
    Solution s;
    vector<int> coll = {-2,1,-3,4,-1,2,1,-5,4};
    BOOST_TEST( 6 == s.maxSubArray( coll ));
}


#include "ros/ros.h"
#include "std_msgs/Float64.h"
#include <iostream>
#include <Eigen/Dense>

Eigen::MatrixXd m(3,3);
Eigen::Vector3d r;
Eigen::Vector3d v;

ros::Subscriber yaw_sub;
ros::Subscriber pitch_sub;
ros::Subscriber roll_sub;
ros::Publisher motor1_pub;
ros::Publisher motor2_pub;
ros::Publisher motor3_pub;
double yaw_control_effort, pitch_control_effort, roll_control_effort;
/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */

void matrixInit(void)
{

  m << -2,-1.732,1,
-2,0,-2,
-2,1.732,1;
}

void yawCallback(const std_msgs::Float64& control_effort_input)
{
     yaw_control_effort = control_effort_input.data;

}

void pitchCallback(const std_msgs::Float64& control_effort_input)
{
     pitch_control_effort = control_effort_input.data;

}

void rollCallback(const std_msgs::Float64& control_effort_input)
{
     roll_control_effort = control_effort_input.data;

}

void controlCallback(const ros::TimerEvent& event)
{
     v(0) = yaw_control_effort;
     v(1) = pitch_control_effort;
     v(2) = roll_control_effort;
     r = m*v;

     std_msgs::Float64 motor1_control_effort,motor2_control_effort,motor3_control_effort;
     motor1_control_effort.data = r(0);
     motor2_control_effort.data = r(1);
     motor3_control_effort.data = r(2);
     motor1_pub.publish(motor1_control_effort);
     motor2_pub.publish(motor2_control_effort);
     motor3_pub.publish(motor3_control_effort);
     //ROS_INFO("published rpy control_effort: roll=%f pitch=%f yaw=%f", r(0), r(1), r(2));

}

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

  ros::init(argc, argv, "velo_transformer");

  ros::NodeHandle n;

  ros::Timer timer1 = n.createTimer(ros::Duration(0.01), controlCallback);
  yaw_sub = n.subscribe("/ballbot_yaw/control_effort", 1000, yawCallback);
  pitch_sub = n.subscribe("/ballbot_pitch/control_effort", 1000, pitchCallback);
  roll_sub = n.subscribe("/ballbot_roll/control_effort", 1000, rollCallback);
  motor1_pub = n.advertise<std_msgs::Float64>("/ballbot/joint0_vel_cmd", 1000);
  motor2_pub = n.advertise<std_msgs::Float64>("/ballbot/joint1_vel_cmd", 1000);
  motor3_pub = n.advertise<std_msgs::Float64>("/ballbot/joint2_vel_cmd", 1000);
  ros::spin();

  return 0;
}


#include <NTL/ZZ.h>
#include <NTL/BasicThreadPool.h>
#include "FHE.h"
#include "timing.h"
#include "EncryptedArray.h"
#include <NTL/lzz_pXFactoring.h>

#include <cassert>
#include <cstdio>
#include <iostream>
using namespace std;

/*  下面两个函数的作用是把数字或者数组转为可用于运算操作的vector
 *  因为当加密和解密过后，最后连续的n个0会被隐藏
 *  例如[2 2 0 0]→[2 2], [0 0 0 0]→[]
 *  []转为long输出时会出错，所以在末尾加上1使得0可以正确地被提取出来
 */

// 数字转为可用于运算操作的vector
Vec<ZZ> num2validVec(long num)
{
	Vec<ZZ> v;
	v.SetLength(2);
	v[0] = num;
	v[1] = 1;
	return v;
}

// 数组转为可用于运算操作的vector
Vec<ZZ> arr2validVec(long* num, int arrLen)
{
	Vec<ZZ> v;
	v.SetLength(arrLen+1);
	for (int i=0; i<arrLen; i++)
		v[i] = num[i];
	v[arrLen] = 1;
	return v;
}

// 四则运算全同态解密（只实现了数字作为运算符的解密，数组作为运算符的有待修缮）
long FHE_ptDec(ZZX ptxt, long p)
{
	long ptDec;
	conv(ptDec, ptxt[0]);
	if (ptDec > p/2)
		ptDec -= p;
	return ptDec;
}

// 全同态加法
Ctxt FHE_Add(Ctxt Ea, Ctxt Eb)
{
	Ctxt ctSum = Ea;
	ctSum += Eb;
	return ctSum;
}

// 全同态乘法
Ctxt FHE_Mul(Ctxt Ea, Ctxt Eb, long p, const FHESecKey& secretKey)
{
	ZZX ptEa, ptEb;
	secretKey.Decrypt(ptEa, Ea);
	secretKey.Decrypt(ptEb, Eb);
	// 解密判断乘数中是否有0，如果是，则返回0的密文
	if (FHE_ptDec(ptEa, p) == 0)
		return Ea;
	else if (FHE_ptDec(ptEb, p) == 0)
		return Eb;
	else
	{
		Ctxt ctMul = Ea;
		ctMul *= Eb;
		return ctMul;
	}
}

// 全同态减法
Ctxt FHE_Sub(Ctxt Ea, Ctxt Eb, const FHEPubKey& publicKey)
{
	// sub = op2*(-1)+op1
	Ctxt minus1(publicKey);
	Vec<ZZ> m1 = num2validVec(-1);
	publicKey.Encrypt(minus1, to_ZZX(m1));
	Ctxt ctSub = Eb;
	ctSub *= minus1;
	ctSub += Ea;
	return ctSub;
}

// 全同态除法
Ctxt FHE_Div(Ctxt Ea, Ctxt Eb, long p,
			 const FHEPubKey& publicKey, const FHESecKey& secretKey)
{
	int quotient = 0;  // 初始化商quotient为0
	ZZX ptMul, ptMul2, ptSub, ptSum, ptEa, ptEb;

	/* 判断思路（设被除数和除数为op1和op2）：
	 * 1. op2=0时，输出"Error: Invalid Denominator."，否则继续
	 * 2. op1=0时，返回0的密文，否则继续
	 * 3. 判断op1和op2是否同号,可用op1·op2是否为正来判断
	 *    3-1. 同号: 当op1=0时跳出循环，否则
	 *               sub=op1-op2，如果sub和op1同号(op1和op2同为负数的时候sub也是负数)则quotient递增且op1=sub，否则跳出循环
	 *    3-2. 异号: 当op1=0时跳出循环，否则
	 *               sum=op1+op2(因为是异号)，如果sum和op1同号则quotient递减(异号相除结果为负数)且op1=sum，否则跳出循环
	 */
	bool positive = true;
	secretKey.Decrypt(ptEa, Ea);
	long EaDec = FHE_ptDec(ptEa, p);
	if(EaDec < 0) positive = false;
	secretKey.Decrypt(ptEb, Eb);
	long EbDec = FHE_ptDec(ptEb, p);
	if (EbDec == 0)
	{
		cout << "Error: Invalid Denominator." << endl;
		Ctxt ctDiv(publicKey);
		Vec<ZZ> q = num2validVec(quotient);
		publicKey.Encrypt(ctDiv, to_ZZX(q));
		return ctDiv;
	}
	else if (EaDec == 0)
	{
		Ctxt ctDiv(publicKey);
		Vec<ZZ> q = num2validVec(quotient);
		publicKey.Encrypt(ctDiv, to_ZZX(q));
		return ctDiv;
	}
	else {
		secretKey.Decrypt(ptMul, FHE_Mul(Ea, Eb, p, secretKey));
		// 两操作数同号
		if(FHE_ptDec(ptMul, p) >= 0)
		{
			while (1)
			{
				secretKey.Decrypt(ptEa, Ea);
				long EaDec = FHE_ptDec(ptEa, p);
				if (EaDec == 0) break;
				else
				{
					Ctxt ctSub = FHE_Sub(Ea, Eb, publicKey);
					secretKey.Decrypt(ptSub, ctSub);
					long sub = FHE_ptDec(ptSub, p);
					if (sub >= 0 && positive || sub <= 0 && !positive)
					{
						Ea = ctSub;
						quotient ++;
					}
					else break;
				}
			}
		}
		// 两操作数异号
		else
		{
			while (1)
			{
				secretKey.Decrypt(ptEa, Ea);
				long EaDec = FHE_ptDec(ptEa, p);
				if (EaDec == 0) break;
				else
				{
					Ctxt ctSum = FHE_Add(Ea, Eb);
					secretKey.Decrypt(ptMul2, FHE_Mul(ctSum, Ea, p, secretKey));
					long temp = FHE_ptDec(ptMul2, p);
					if (temp >= 0)
					{
						Ea = ctSum;
						quotient --;
					}
					else break;
				}
			}
		}
		Ctxt ctDiv(publicKey);
		Vec<ZZ> q = num2validVec(quotient);
		publicKey.Encrypt(ctDiv, to_ZZX(q));
		return ctDiv;
	}
}

int main()
{
	long m = 0;    // 确定系数
	long p = 1021; // 模量，定义超过p/2的数为负数，负数的真值x=D[E[x]]-p
	long r = 1;
	long L = 16;
	long c = 3;
	long w = 64;
	long d = 0;
	long k = 128;
	long s = 0;

	m = FindM(k, L, c, p, d, s, 0);

	FHEcontext context(m, p, r);
	buildModChain(context, L, c);

	ZZX G = context.alMod.getFactorsOverZZ()[0];

	// 生成公钥
	FHESecKey secretKey(context);
	const FHEPubKey& publicKey = secretKey;
	secretKey.GenSecKey(w);

	// 初始化密文
	Ctxt Ea(publicKey);
	Ctxt Eb(publicKey);

	/* Test: 输入两个不全为0的数，输出四则运算结果，输入两个0可退出	*/
	long op1, op2;
	while (!(op1==0 && op2==0))
	{
		cin >> op1 >> op2;
		Vec<ZZ> h1 = num2validVec(op1);
		Vec<ZZ> h2 = num2validVec(op2);

		publicKey.Encrypt(Ea, to_ZZX(h1));
		publicKey.Encrypt(Eb, to_ZZX(h2));

		cout << "Operator 1 : " << op1 << " , Operator 2 : " << op2 << endl;

		ZZX ptSum;
		secretKey.Decrypt(ptSum, FHE_Add(Ea, Eb));
		cout << "ptSum : " << FHE_ptDec(ptSum, p) << endl;

		ZZX ptMul;
		secretKey.Decrypt(ptMul, FHE_Mul(Ea, Eb, p, secretKey));
		cout << "ptMul : " << FHE_ptDec(ptMul, p) << endl;

		ZZX ptSub;
		secretKey.Decrypt(ptSub, FHE_Sub(Ea, Eb, publicKey));
		cout << "ptSub : " << FHE_ptDec(ptSub, p) << endl;

		ZZX ptDiv;
		secretKey.Decrypt(ptDiv, FHE_Div(Ea, Eb, p, publicKey, secretKey));
		cout << "ptDiv : " << FHE_ptDec(ptDiv, p) << endl;
	}

	return 0;
}


//
// Copyright 2019 Mateusz Loskot <mateusz at loskot dot net>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
#include <boost/gil.hpp>
#include <boost/gil/extension/numeric/convolve.hpp>

#include <tuple>
#include <type_traits>

#define BOOST_TEST_MODULE test_ext_numeric_colvolve_2d
#include "unit_test.hpp"
#include "unit_test_utility.hpp"
#include "test_fixture.hpp"
#include "core/image/test_fixture.hpp"

namespace gil = boost::gil;
namespace fixture = boost::gil::test::fixture;

BOOST_AUTO_TEST_SUITE(convolve_1d)

BOOST_AUTO_TEST_CASE_TEMPLATE(image_1x1_kernel_1x1_identity, Image, fixture::image_types)
{
    auto const img = fixture::create_image<Image>(1, 1, 7);
    Image img_out(img);

    using pixel_t = typename Image::value_type;
    using channel_t = typename gil::channel_type<pixel_t>::type;
    auto const kernel = fixture::create_kernel<channel_t>({1});
    gil::detail::convolve_1d<pixel_t>(const_view(img_out), kernel, view(img_out));

    // 1x1 kernel reduces convolution to multiplication
    BOOST_TEST(gil::const_view(img).front() == gil::const_view(img_out).front());
}

BOOST_AUTO_TEST_CASE_TEMPLATE(image_1x1_kernel_3x3_identity, Image, fixture::image_types)
{
    auto const img = fixture::create_image<Image>(1, 1, 7);
    Image img_out(img);

    using pixel_t = typename Image::value_type;
    using channel_t = typename gil::channel_type<pixel_t>::type;
    auto const kernel = fixture::create_kernel<channel_t>({0, 0, 0, 0, 1, 0, 0, 0, 0});
    gil::detail::convolve_1d<pixel_t>(const_view(img_out), kernel, view(img_out));

    BOOST_TEST(gil::const_view(img).front() == gil::const_view(img_out).front());
}

BOOST_AUTO_TEST_CASE_TEMPLATE(image_3x3_kernel_3x3_identity, Image, fixture::image_types)
{
    using pixel_t = typename Image::value_type;
    using channel_t = typename gil::channel_type<pixel_t>::type;
    auto const img = fixture::generate_image<Image>(3, 3, fixture::random_value<channel_t>{});
    Image img_out(img);

    auto const kernel = fixture::create_kernel<channel_t>({0, 0, 0, 0, 1, 0, 0, 0, 0});
    gil::detail::convolve_1d<pixel_t>(const_view(img_out), kernel, view(img_out));

    BOOST_TEST(gil::equal_pixels(gil::const_view(img), gil::const_view(img_out)));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(image_5x5_kernel_3x3_identity, Image, fixture::image_types)
{
    using pixel_t = typename Image::value_type;
    using channel_t = typename gil::channel_type<pixel_t>::type;
    auto const img = fixture::generate_image<Image>(5, 5, fixture::random_value<channel_t>{});
    Image img_out(img);

    auto const kernel = fixture::create_kernel<channel_t>({0, 0, 0, 0, 1, 0, 0, 0, 0});
    gil::detail::convolve_1d<pixel_t>(const_view(img_out), kernel, view(img_out));
    // TODO: Test different boundary options

    BOOST_TEST(gil::equal_pixels(gil::const_view(img), gil::const_view(img_out)));
}

BOOST_AUTO_TEST_SUITE_END()


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

#include "main.h"
#include <Eigen/Geometry>

template<typename Scalar,int Size> void homogeneous(void)
{
  /* this test covers the following files:
     Homogeneous.h
  */

  typedef Matrix<Scalar,Size,Size> MatrixType;
  typedef Matrix<Scalar,Size,1, ColMajor> VectorType;

  typedef Matrix<Scalar,Size+1,Size> HMatrixType;
  typedef Matrix<Scalar,Size+1,1> HVectorType;

  typedef Matrix<Scalar,Size,Size+1>   T1MatrixType;
  typedef Matrix<Scalar,Size+1,Size+1> T2MatrixType;
  typedef Matrix<Scalar,Size+1,Size> T3MatrixType;

  VectorType v0 = VectorType::Random(),
             ones = VectorType::Ones();

  HVectorType hv0 = HVectorType::Random();

  MatrixType m0 = MatrixType::Random();

  HMatrixType hm0 = HMatrixType::Random();

  hv0 << v0, 1;
  VERIFY_IS_APPROX(v0.homogeneous(), hv0);
  VERIFY_IS_APPROX(v0, hv0.hnormalized());

  hm0 << m0, ones.transpose();
  VERIFY_IS_APPROX(m0.colwise().homogeneous(), hm0);
  VERIFY_IS_APPROX(m0, hm0.colwise().hnormalized());
  hm0.row(Size-1).setRandom();
  for(int j=0; j<Size; ++j)
    m0.col(j) = hm0.col(j).head(Size) / hm0(Size,j);
  VERIFY_IS_APPROX(m0, hm0.colwise().hnormalized());

  T1MatrixType t1 = T1MatrixType::Random();
  VERIFY_IS_APPROX(t1 * (v0.homogeneous().eval()), t1 * v0.homogeneous());
  VERIFY_IS_APPROX(t1 * (m0.colwise().homogeneous().eval()), t1 * m0.colwise().homogeneous());

  T2MatrixType t2 = T2MatrixType::Random();
  VERIFY_IS_APPROX(t2 * (v0.homogeneous().eval()), t2 * v0.homogeneous());
  VERIFY_IS_APPROX(t2 * (m0.colwise().homogeneous().eval()), t2 * m0.colwise().homogeneous());

  VERIFY_IS_APPROX((v0.transpose().rowwise().homogeneous().eval()) * t2,
                    v0.transpose().rowwise().homogeneous() * t2);
                    m0.transpose().rowwise().homogeneous().eval();
  VERIFY_IS_APPROX((m0.transpose().rowwise().homogeneous().eval()) * t2,
                    m0.transpose().rowwise().homogeneous() * t2);

  T3MatrixType t3 = T3MatrixType::Random();
  VERIFY_IS_APPROX((v0.transpose().rowwise().homogeneous().eval()) * t3,
                    v0.transpose().rowwise().homogeneous() * t3);
  VERIFY_IS_APPROX((m0.transpose().rowwise().homogeneous().eval()) * t3,
                    m0.transpose().rowwise().homogeneous() * t3);

  // test product with a Transform object
  Transform<Scalar, Size, Affine> aff;
  Transform<Scalar, Size, AffineCompact> caff;
  Transform<Scalar, Size, Projective> proj;
  Matrix<Scalar, Size, Dynamic>   pts;
  Matrix<Scalar, Size+1, Dynamic> pts1, pts2;

  aff.affine().setRandom();
  proj = caff = aff;
  pts.setRandom(Size,internal::random<int>(1,20));

  pts1 = pts.colwise().homogeneous();
  VERIFY_IS_APPROX(aff  * pts.colwise().homogeneous(), (aff  * pts1).colwise().hnormalized());
  VERIFY_IS_APPROX(caff * pts.colwise().homogeneous(), (caff * pts1).colwise().hnormalized());
  VERIFY_IS_APPROX(proj * pts.colwise().homogeneous(), (proj * pts1));

  VERIFY_IS_APPROX((aff  * pts1).colwise().hnormalized(),  aff  * pts);
  VERIFY_IS_APPROX((caff * pts1).colwise().hnormalized(), caff * pts);

  pts2 = pts1;
  pts2.row(Size).setRandom();
  VERIFY_IS_APPROX((aff  * pts2).colwise().hnormalized(), aff  * pts2.colwise().hnormalized());
  VERIFY_IS_APPROX((caff * pts2).colwise().hnormalized(), caff * pts2.colwise().hnormalized());
  VERIFY_IS_APPROX((proj * pts2).colwise().hnormalized(), (proj * pts2.colwise().hnormalized().colwise().homogeneous()).colwise().hnormalized());
}

void test_geo_homogeneous()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1(( homogeneous<float,1>() ));
    CALL_SUBTEST_2(( homogeneous<double,3>() ));
    CALL_SUBTEST_3(( homogeneous<double,8>() ));
  }
}


/**
 * $Id$
 *
 * Copyright (C)
 * 2015 - $Date$
 *     Martin Wolf <ndhist@martin-wolf.org>
 *
 * This file is distributed under the BSD 2-Clause Open Source License
 * (See LICENSE file).
 *
 */
#ifndef NDHIST_STATS_MEAN_HPP_INCLUDED
#define NDHIST_STATS_MEAN_HPP_INCLUDED 1

#include <boost/python.hpp>

#include <ndhist/ndhist.hpp>
#include <ndhist/stats/expectation.hpp>

namespace ndhist {
namespace stats {

namespace detail {

template <typename AxisValueType, typename WeightValueType>
AxisValueType
calc_axis_mean_impl(
    ndhist const & h
  , intptr_t const axis
)
{
    return calc_axis_expectation_impl<AxisValueType, WeightValueType>(h, 1, axis);
}

}// namespace detail

namespace py {

/**
 * @brief Calculates the mean value along the given axis of the given ndhist
 *     object.
 *     Since the mean is equal to the first order expectation, this function
 *     just calls the expectation function to calculate the first order
 *     expectation value.
 *     If None is given as axis, the mean value for all axes of the ndhist
 *     object will be calculated and returned as a tuple. But if the
 *     dimensionality of the ndhist object is 1, a scalar value is returned.
 *
 * @note This function is only defined for ndhist objects with POD type axis
 *     values AND POD type weight values.
 */
boost::python::object
mean(
    ndhist const & h
  , boost::python::object const & axis = boost::python::object()
);

}// namespace py
}// namespace stats
}// namespace ndhist

#endif // !NDHIST_STATS_MEAN_HPP_INCLUDED