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

#include "pinocchio/spatial/explog.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

#include "utils/macros.hpp"

using namespace pinocchio;

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE(exp)
{
  SE3 M(SE3::Random());
  Motion v(Motion::Random()); v.linear().setZero();

  SE3::Matrix3 R = exp3(v.angular());
  BOOST_CHECK(R.isApprox(Eigen::AngleAxis<double>(v.angular().norm(), v.angular().normalized()).matrix()));

  SE3::Matrix3 R0 = exp3(SE3::Vector3::Zero());
  BOOST_CHECK(R0.isIdentity());

  // check the NAN case
  #ifdef NDEBUG
    Motion::Vector3 vec3_nan(Motion::Vector3::Constant(NAN));
    SE3::Matrix3 R_nan = exp3(vec3_nan);
    BOOST_CHECK(R_nan != R_nan);
  #endif

  M = exp6(v);

  BOOST_CHECK(R.isApprox(M.rotation()));

  // check the NAN case
  #ifdef NDEBUG
    Motion::Vector6 vec6_nan(Motion::Vector6::Constant(NAN));
    SE3 M_nan = exp6(vec6_nan);
    BOOST_CHECK(M_nan != M_nan);
  #endif

  R = exp3(SE3::Vector3::Zero());
  BOOST_CHECK(R.isIdentity());

  // Quaternion
  Eigen::Quaterniond quat;
  quaternion::exp3(v.angular(),quat);
  BOOST_CHECK(quat.toRotationMatrix().isApprox(M.rotation()));

  quaternion::exp3(SE3::Vector3::Zero(),quat);
  BOOST_CHECK(quat.toRotationMatrix().isIdentity());
  BOOST_CHECK(quat.vec().isZero() && quat.coeffs().tail<1>().isOnes());

  // Check QuaternionMap
  Eigen::Vector4d vec4;
  Eigen::QuaternionMapd quat_map(vec4.data());
  quaternion::exp3(v.angular(),quat_map);
  BOOST_CHECK(quat_map.toRotationMatrix().isApprox(M.rotation()));
}

BOOST_AUTO_TEST_CASE(log)
{
  SE3 M(SE3::Identity());
  Motion v(Motion::Random()); v.linear().setZero();

  SE3::Vector3 omega = log3(M.rotation());
  BOOST_CHECK(omega.isZero());

  // check the NAN case
#ifdef NDEBUG
  SE3::Matrix3 mat3_nan(SE3::Matrix3::Constant(NAN));
  SE3::Vector3 omega_nan = log3(mat3_nan);
  BOOST_CHECK(omega_nan != omega_nan);
#endif

  M.setRandom();
  M.translation().setZero();

  v = log6(M);
  omega = log3(M.rotation());
  BOOST_CHECK(omega.isApprox(v.angular()));

  // check the NAN case
  #ifdef NDEBUG
    SE3::Matrix4 mat4_nan(SE3::Matrix4::Constant(NAN));
    Motion::Vector6 v_nan = log6(mat4_nan);
    BOOST_CHECK(v_nan != v_nan);
  #endif

  // Quaternion
  Eigen::Quaterniond quat(SE3::Matrix3::Identity());
  omega = quaternion::log3(quat);
  BOOST_CHECK(omega.isZero());

  for(int k = 0; k < 1e3; ++k)
  {
    SE3::Vector3 w; w.setRandom();
    quaternion::exp3(w,quat);
    SE3::Matrix3 rot = exp3(w);

    BOOST_CHECK(quat.toRotationMatrix().isApprox(rot));
    double theta;
    omega = quaternion::log3(quat,theta);
    const double PI_value = PI<double>();
    BOOST_CHECK(omega.norm() <= PI_value);
    double theta_ref;
    SE3::Vector3 omega_ref = log3(quat.toRotationMatrix(),theta_ref);

    BOOST_CHECK(omega.isApprox(omega_ref));
  }

  // Check QuaternionMap
  Eigen::Vector4d vec4;
  Eigen::QuaternionMapd quat_map(vec4.data());
  quat_map = SE3::Random().rotation();
  BOOST_CHECK(quaternion::log3(quat_map).isApprox(log3(quat_map.toRotationMatrix())));
}

BOOST_AUTO_TEST_CASE(explog3)
{
  SE3 M(SE3::Random());
  SE3::Matrix3 M_res = exp3(log3(M.rotation()));
  BOOST_CHECK(M_res.isApprox(M.rotation()));

  Motion::Vector3 v; v.setRandom();
  Motion::Vector3 v_res = log3(exp3(v));
  BOOST_CHECK(v_res.isApprox(v));
}

BOOST_AUTO_TEST_CASE(explog3_quaternion)
{
  SE3 M(SE3::Random());
  Eigen::Quaterniond quat;
  quat = M.rotation();
  Eigen::Quaterniond quat_res;
  quaternion::exp3(quaternion::log3(quat),quat_res);
  BOOST_CHECK(quat_res.isApprox(quat) || quat_res.coeffs().isApprox(-quat.coeffs()));

  Motion::Vector3 v; v.setRandom();
  quaternion::exp3(v,quat);
  BOOST_CHECK(quaternion::log3(quat).isApprox(v));

  SE3::Matrix3 R_next = M.rotation() * exp3(v);
  Motion::Vector3 v_est = log3(M.rotation().transpose() * R_next);
  BOOST_CHECK(v_est.isApprox(v));

  SE3::Quaternion quat_v;
  quaternion::exp3(v,quat_v);
  SE3::Quaternion quat_next = quat * quat_v;
  v_est = quaternion::log3(quat.conjugate() * quat_next);
  BOOST_CHECK(v_est.isApprox(v));
}

BOOST_AUTO_TEST_CASE(Jlog3_fd)
{
  SE3 M(SE3::Random());
  SE3::Matrix3 R (M.rotation());

  SE3::Matrix3 Jfd, Jlog;
  Jlog3 (R, Jlog);
  Jfd.setZero();

  Motion::Vector3 dR; dR.setZero();
  const double eps = 1e-8;
  for (int i = 0; i < 3; ++i)
  {
    dR[i] = eps;
    SE3::Matrix3 R_dR = R * exp3(dR);
    Jfd.col(i) = (log3(R_dR) - log3(R)) / eps;
    dR[i] = 0;
  }
  BOOST_CHECK(Jfd.isApprox(Jlog, std::sqrt(eps)));
}

BOOST_AUTO_TEST_CASE(Jexp3_fd)
{
  SE3 M(SE3::Random());
  SE3::Matrix3 R (M.rotation());

  Motion::Vector3 v = log3(R);

  SE3::Matrix3 Jexp_fd, Jexp;

  Jexp3(Motion::Vector3::Zero(), Jexp);
  BOOST_CHECK(Jexp.isIdentity());

  Jexp3(v, Jexp);

  Motion::Vector3 dv; dv.setZero();
  const double eps = 1e-8;
  for (int i = 0; i < 3; ++i)
  {
    dv[i] = eps;
    SE3::Matrix3 R_next = exp3(v+dv);
    Jexp_fd.col(i) = log3(R.transpose()*R_next) / eps;
    dv[i] = 0;
  }
  BOOST_CHECK(Jexp_fd.isApprox(Jexp, std::sqrt(eps)));
}

template<typename QuaternionLike, typename Matrix43Like>
void Jexp3QuatLocal(const Eigen::QuaternionBase<QuaternionLike> & quat,
                    const Eigen::MatrixBase<Matrix43Like> & Jexp)
{
  Matrix43Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix43Like,Jexp);

  skew(0.5 * quat.vec(),Jout.template topRows<3>());
  Jout.template topRows<3>().diagonal().array() += 0.5 * quat.w();
  Jout.template bottomRows<1>() = -0.5 * quat.vec().transpose();
}

BOOST_AUTO_TEST_CASE(Jexp3_quat_fd)
{
  typedef double Scalar;
  SE3::Vector3 w; w.setRandom();
  SE3::Quaternion quat; quaternion::exp3(w,quat);

  typedef Eigen::Matrix<Scalar,4,3> Matrix43;
  Matrix43 Jexp3, Jexp3_fd;
  quaternion::Jexp3CoeffWise(w,Jexp3);
  SE3::Vector3 dw; dw.setZero();
  const double eps = 1e-8;

  for(int i = 0; i < 3; ++i)
  {
    dw[i] = eps;
    SE3::Quaternion quat_plus; quaternion::exp3(w + dw,quat_plus);
    Jexp3_fd.col(i) = (quat_plus.coeffs() - quat.coeffs()) / eps;
    dw[i] = 0;
  }
  BOOST_CHECK(Jexp3.isApprox(Jexp3_fd,sqrt(eps)));

  SE3::Matrix3 Jlog;
  pinocchio::Jlog3(quat.toRotationMatrix(),Jlog);

  Matrix43 Jexp_quat_local;
  Jexp3QuatLocal(quat,Jexp_quat_local);

  Matrix43 Jcompositon = Jexp3 * Jlog;
  BOOST_CHECK(Jcompositon.isApprox(Jexp_quat_local));
//  std::cout << "Jcompositon\n" << Jcompositon << std::endl;
//  std::cout << "Jexp_quat_local\n" << Jexp_quat_local << std::endl;

  // Arount zero
  w.setZero();
  w.fill(1e-6);
  quaternion::exp3(w,quat);
  quaternion::Jexp3CoeffWise(w,Jexp3);
  for(int i = 0; i < 3; ++i)
  {
    dw[i] = eps;
    SE3::Quaternion quat_plus; quaternion::exp3(w + dw,quat_plus);
    Jexp3_fd.col(i) = (quat_plus.coeffs() - quat.coeffs()) / eps;
    dw[i] = 0;
  }
  BOOST_CHECK(Jexp3.isApprox(Jexp3_fd,sqrt(eps)));

}

BOOST_AUTO_TEST_CASE(Jexp3_quat)
{
  SE3 M(SE3::Random());
  SE3::Quaternion quat(M.rotation());

  Motion dv(Motion::Zero());
  const double eps = 1e-8;

  typedef Eigen::Matrix<double,7,6> Matrix76;
  Matrix76 Jexp6_fd, Jexp6_quat; Jexp6_quat.setZero();
  typedef Eigen::Matrix<double,4,3> Matrix43;
  Matrix43 Jexp3_quat; Jexp3QuatLocal(quat,Jexp3_quat);
  SE3 M_next;

  Jexp6_quat.middleRows<3>(Motion::LINEAR).middleCols<3>(Motion::LINEAR) = M.rotation();
  Jexp6_quat.middleRows<4>(Motion::ANGULAR).middleCols<3>(Motion::ANGULAR) = Jexp3_quat;// * Jlog6_SE3.middleRows<3>(Motion::ANGULAR);
  for(int i = 0; i < 6; ++i)
  {
    dv.toVector()[i] = eps;
    M_next = M * exp6(dv);
    const SE3::Quaternion quat_next(M_next.rotation());
    Jexp6_fd.middleRows<3>(Motion::LINEAR).col(i) = (M_next.translation() - M.translation())/eps;
    Jexp6_fd.middleRows<4>(Motion::ANGULAR).col(i) = (quat_next.coeffs() - quat.coeffs())/eps;
    dv.toVector()[i] = 0.;
  }

  BOOST_CHECK(Jexp6_quat.isApprox(Jexp6_fd,sqrt(eps)));
}

BOOST_AUTO_TEST_CASE(Jexplog3)
{
  Motion v(Motion::Random());

  Eigen::Matrix3d R (exp3(v.angular())),
    Jexp, Jlog;
  Jexp3 (v.angular(), Jexp);
  Jlog3 (R          , Jlog);

  BOOST_CHECK((Jlog * Jexp).isIdentity());

  SE3 M(SE3::Random());
  R = M.rotation();
  v.angular() = log3(R);
  Jlog3 (R          , Jlog);
  Jexp3 (v.angular(), Jexp);

  BOOST_CHECK((Jexp * Jlog).isIdentity());
}

BOOST_AUTO_TEST_CASE(Jlog3_quat)
{
  SE3::Vector3 w; w.setRandom();
  SE3::Quaternion quat; quaternion::exp3(w,quat);

  SE3::Matrix3 R(quat.toRotationMatrix());

  SE3::Matrix3 res, res_ref;
  quaternion::Jlog3(quat,res);
  Jlog3(R,res_ref);

  BOOST_CHECK(res.isApprox(res_ref));
}

BOOST_AUTO_TEST_CASE(explog6)
{
  SE3 M(SE3::Random());
  SE3 M_res = exp6(log6(M));
  BOOST_CHECK(M_res.isApprox(M));

  Motion v(Motion::Random());
  Motion v_res = log6(exp6(v));
  BOOST_CHECK(v_res.toVector().isApprox(v.toVector()));
}

BOOST_AUTO_TEST_CASE(Jlog6_fd)
{
  SE3 M(SE3::Random());

  SE3::Matrix6 Jfd, Jlog;
  Jlog6 (M, Jlog);
  Jfd.setZero();

  Motion dM; dM.setZero();
  double step = 1e-8;
  for (int i = 0; i < 6; ++i)
  {
    dM.toVector()[i] = step;
    SE3 M_dM = M * exp6(dM);
    Jfd.col(i) = (log6(M_dM).toVector() - log6(M).toVector()) / step;
    dM.toVector()[i] = 0;
  }

  BOOST_CHECK(Jfd.isApprox(Jlog, sqrt(step)));
}

BOOST_AUTO_TEST_CASE(Jexplog6)
{
  Motion v(Motion::Random());

  SE3 M (exp6(v));
  {
    Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Jexp, Jlog;
    Jexp6 (v, Jexp);
    Jlog6 (M, Jlog);

    BOOST_CHECK((Jlog * Jexp).isIdentity());
  }

  M.setRandom();

  v = log6(M);
  {
    Eigen::Matrix<double, 6, 6> Jexp, Jlog;
    Jlog6 (M, Jlog);
    Jexp6 (v, Jexp);

    BOOST_CHECK((Jexp * Jlog).isIdentity());
  }
}

BOOST_AUTO_TEST_CASE (test_basic)
{
  typedef pinocchio::SE3::Vector3 Vector3;
  typedef pinocchio::SE3::Matrix3 Matrix3;
  typedef Eigen::Matrix4d Matrix4;
  typedef pinocchio::Motion::Vector6 Vector6;

  const double EPSILON = 1e-12;

  // exp3 and log3.
  Vector3 v3(Vector3::Random());
  Matrix3 R(pinocchio::exp3(v3));
  BOOST_CHECK(R.transpose().isApprox(R.inverse(), EPSILON));
  BOOST_CHECK_SMALL(R.determinant() - 1.0, EPSILON);
  Vector3 v3FromLog(pinocchio::log3(R));
  BOOST_CHECK(v3.isApprox(v3FromLog, EPSILON));

  // exp6 and log6.
  pinocchio::Motion nu = pinocchio::Motion::Random();
  pinocchio::SE3 m = pinocchio::exp6(nu);
  BOOST_CHECK(m.rotation().transpose().isApprox(m.rotation().inverse(),
                                                EPSILON));
  BOOST_CHECK_SMALL(m.rotation().determinant() - 1.0, EPSILON);
  pinocchio::Motion nuFromLog(pinocchio::log6(m));
  BOOST_CHECK(nu.linear().isApprox(nuFromLog.linear(), EPSILON));
  BOOST_CHECK(nu.angular().isApprox(nuFromLog.angular(), EPSILON));

  Vector6 v6(Vector6::Random());
  pinocchio::SE3 m2(pinocchio::exp6(v6));
  BOOST_CHECK(m2.rotation().transpose().isApprox(m2.rotation().inverse(),
                                                 EPSILON));
  BOOST_CHECK_SMALL(m2.rotation().determinant() - 1.0, EPSILON);
  Matrix4 M = m2.toHomogeneousMatrix();
  pinocchio::Motion nu2FromLog(pinocchio::log6(M));
  Vector6 v6FromLog(nu2FromLog.toVector());
  BOOST_CHECK(v6.isApprox(v6FromLog, EPSILON));
}

BOOST_AUTO_TEST_CASE(test_SE3_interpolate)
{
  SE3 A = SE3::Random();
  SE3 B = SE3::Random();

  SE3 A_bis = SE3::Interpolate(A,B,0.);
  BOOST_CHECK(A_bis.isApprox(A));
  SE3 B_bis = SE3::Interpolate(A,B,1.);
  BOOST_CHECK(B_bis.isApprox(B));

  A_bis = SE3::Interpolate(A,A,1.);
  BOOST_CHECK(A_bis.isApprox(A));

  B_bis = SE3::Interpolate(B,B,1.);
  BOOST_CHECK(B_bis.isApprox(B));

  SE3 C = SE3::Interpolate(A,B,0.5);
  SE3 D = SE3::Interpolate(B,A,0.5);
  BOOST_CHECK(D.isApprox(C));
}

BOOST_AUTO_TEST_SUITE_END()


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/AlgorithmUtils.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <Eigen/SVD>

namespace fluid {
namespace algorithm {

class NNDSVD
{

public:
  using MatrixXd = Eigen::MatrixXd;

  index process(RealMatrixView X, RealMatrixView W, RealMatrixView H,
                index minRank = 0, index maxRank = 200, double amount = 0.8,
                index method = 0) // 0 - NMF-SVD, 1 NNDSVDar, 2 NNDSVDa 3 NNDSVD
  {
    using namespace _impl;
    using namespace Eigen;
    MatrixXd XT = asEigen<Matrix>(X).transpose();
    MatrixXd WT = asEigen<Matrix>(W).transpose();
    MatrixXd HT = asEigen<Matrix>(H).transpose();

    assert(amount > 0 || minRank > 0);

    BDCSVD<MatrixXd> svd(XT, ComputeThinV | ComputeThinU);

    MatrixXd U = svd.matrixU();
    MatrixXd V = svd.matrixV().transpose();
    VectorXd s = svd.singularValues();
    MatrixXd S = svd.singularValues().asDiagonal();
    index    k = 0;
    if (amount == 0)
      k = minRank;
    else
    {
      double current = 0;
      double total = s.sum();
      while ((current / total) < amount) current += s[k++];
    }
    if (k < minRank) k = minRank;
    if (k > maxRank) k = maxRank;

    if (method == 0)
    {
      WT.block(0, 0, WT.rows(), k) = U.block(0, 0, U.rows(), k).array().abs();
      HT.block(0, 0, k, HT.cols()) =
          (S.block(0, 0, k, S.cols()) * V).array().abs();
    }
    else
    {
      // avoid scaling for NMF with normalized W
      WT.col(0) = U.col(0).array().abs();
      HT.row(0) = sqrt(s(0)) * V.row(0).array().abs();

      for (index j = 1; j < k; j++)
      {
        VectorXd x = U.col(j);
        VectorXd y = V.row(j);
        VectorXd xP = x.array().max(0.0);
        VectorXd yP = y.array().max(0.0);
        VectorXd xN = x.array().min(0.0).abs();
        VectorXd yN = y.array().min(0.0).abs();
        double   xPNorm = xP.norm();
        double   yPNorm = yP.norm();
        double   xNNorm = xN.norm();
        double   yNNorm = xN.norm();
        double   mP = xPNorm * yPNorm;
        double   mN = xNNorm * yNNorm;
        ArrayXd  u;
        ArrayXd  v;
        double   sigma;
        if (mP > mN)
        {
          u = xP / xPNorm;
          v = yP / yPNorm;
          sigma = mP;
        }
        else
        {
          u = xN / xNNorm;
          v = yN / yNNorm;
          sigma = mN;
        }
        auto lbd = std::sqrt(s[j] * sigma);
        WT.col(j) = u; // avoid scaling for NMF with normalized W
        HT.row(j) = lbd * v;
      }
      WT = WT.array().max(epsilon);
      HT = HT.array().max(epsilon);
      if (method == 1)
      {
        auto Wrand =
            MatrixXd::Random(WT.rows(), WT.cols()).array().abs() / 100.0;
        auto Hrand =
            MatrixXd::Random(HT.rows(), HT.cols()).array().abs() / 100.0;
        WT = (WT.array() < epsilon).select(Wrand, WT);
        HT = (HT.array() < epsilon).select(Hrand, HT);
      }
      else if (method == 2)
      {
        double mean = XT.mean();
        WT = (WT.array() < epsilon)
                 .select(MatrixXd::Constant(WT.rows(), WT.cols(), mean), WT);
        HT = (HT.array() < epsilon)
                 .select(MatrixXd::Constant(HT.rows(), HT.cols(), mean), HT);
      }
    }
    MatrixXd W1 = WT.transpose();
    W <<= asFluid(W1);
    MatrixXd H1 = HT.transpose();
    H <<= asFluid(H1);
    return k;
  }
};
} // namespace algorithm
} // namespace fluid


//------------------------------------------------------------------------------
/// \file FunctionT_tests.cpp
/// \ref Vandevoorde, Josuttis, Gregor. C++ Templates: The Complete Guide. 2nd
/// Ed. Addison-Wesley Professional. 2017.
//------------------------------------------------------------------------------

#include <boost/test/unit_test.hpp>
#include <cmath>
#include <complex>
#include <string>

using std::cos;
using std::sin;
using std::sqrt;

BOOST_AUTO_TEST_SUITE(Cpp)
BOOST_AUTO_TEST_SUITE(Templates)
BOOST_AUTO_TEST_SUITE(FunctionTemplates)
BOOST_AUTO_TEST_SUITE(FunctionT_tests)

template <typename X, typename FX, FX ObjectMap(X)>
FX object_map(const X& x)
{
  return ObjectMap(x);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(MapAsTemplateParameter)
{
  const auto result = object_map<double, double, &sin>(M_PI_4);
  BOOST_TEST(result == 1.0 / sqrt(2.0));
}

BOOST_AUTO_TEST_SUITE_END() // FunctionT_tests

BOOST_AUTO_TEST_SUITE_END() // FunctionTemplates
BOOST_AUTO_TEST_SUITE_END() // Templates
BOOST_AUTO_TEST_SUITE_END() // Cpp


#include <gtest/gtest.h>

#include <Eigen/Dense>
#include <cmath>
#include <memory>

#include "fbstab/fbstab_dense.h"
#include "tools/utilities.h"

namespace fbstab {
namespace test {

using MatrixXd = Eigen::MatrixXd;
using VectorXd = Eigen::VectorXd;
/**
 * Tests FBstab with
 *
 * H = [3 1]  f = [10]
 *     [1 1]      [5 ]
 *
 * A = [-1 0] b = [0]
 *     [0  1]     [0]
 *
 * This QP can be solved analytically
 * and has the unique primal(z) - dual(v) solution
 * z = [0 -5],  v = [5 0]
 */
GTEST_TEST(FBstabDense, FeasibleQP) {
  int n = 2;
  int m = 0;
  int q = 2;

  FBstabDense::Variable x0(n, m, q);
  FBstabDense::ProblemData data(n, m, q);
  data.H << 3, 1, 1, 1;
  data.f << 10, 5;
  data.A << -1, 0, 0, 1;
  data.b << 0, 0;

  FBstabDense solver(n, m, q);
  FBstabDense::Options opts = FBstabDense::DefaultOptions();
  opts.abs_tol = 1e-8;
  opts.display_level = Display::OFF;
  solver.UpdateOptions(opts);

  SolverOut out = solver.Solve(data, &x0);

  ASSERT_EQ(out.eflag, ExitFlag::SUCCESS);

  VectorXd zopt(2);
  VectorXd vopt(2);
  zopt << 0, -5;
  vopt << 5, 0;
  for (int i = 0; i < n; i++) {
    EXPECT_NEAR(x0.z(i), zopt(i), 1e-8);
  }

  for (int i = 0; i < q; i++) {
    EXPECT_NEAR(x0.v(i), vopt(i), 1e-8);
  }
}

/**
 * Tests FBstab with
 *
 * H = [4 1]  f = [1]
 *     [1 2]      [1]
 *
 * G = [1 1]  h = [1]
 *
 * A = [-1 0] b = [0]
 *     [0 -1]     [0]
 *
 */
GTEST_TEST(FBstabDense, FeasibleQPwithEQ) {
  int n = 2;
  int m = 1;
  int q = 2;

  FBstabDense::Variable x0(n, m, q);
  FBstabDense::ProblemData data(n, m, q);
  data.H << 4, 1, 1, 2;
  data.f << 1, 1;
  data.G << 1, 1;
  data.h << 1;
  data.A << -1, 0, 0, -1;
  data.b << 0, 0;

  FBstabDense solver(n, m, q);
  FBstabDense::Options opts = FBstabDense::DefaultOptions();
  opts.abs_tol = 1e-8;
  opts.display_level = Display::OFF;
  solver.UpdateOptions(opts);

  SolverOut out = solver.Solve(data, &x0);

  ASSERT_EQ(out.eflag, ExitFlag::SUCCESS);

  VectorXd zopt(n);
  zopt << 2.5e-1, 7.5e-1;
  for (int i = 0; i < n; i++) {
    EXPECT_NEAR(x0.z(i), zopt(i), 1e-8);
  }
}

/**
 * Tests FBstab with
 *
 * H = [1 0]  f = [1]
 *     [0 0]      [0]
 *
 * A = [0  0] b = [0 ]
 *     [1  0]     [3 ]
 *     [0  1]     [3 ]
 *     [-1 0]     [-1]
 *     [0 -1]     [-1]
 *
 * This QP is degenerate with a primal solution set
 * [1] x [1,3]
 */
GTEST_TEST(FBstabDense, DegenerateQP) {
  constexpr int n = 2;
  constexpr int m = 0;
  constexpr int q = 5;

  // Using a ref variable
  std::unique_ptr<double[]> zmem(new double[n]);
  std::unique_ptr<double[]> lmem(new double[m]);
  std::unique_ptr<double[]> vmem(new double[q]);
  std::unique_ptr<double[]> ymem(new double[q]);

  Eigen::Map<VectorXd> z(zmem.get(), n);
  Eigen::Map<VectorXd> l(lmem.get(), m);
  Eigen::Map<VectorXd> v(vmem.get(), q);
  Eigen::Map<VectorXd> y(ymem.get(), q);

  FBstabDense::VariableRef x0(&z, &l, &v, &y);
  x0.fill(0.0);

  std::unique_ptr<double[]> Hmem(new double[n * n]);
  std::unique_ptr<double[]> fmem(new double[n]);
  std::unique_ptr<double[]> Gmem(new double[m * n]);
  std::unique_ptr<double[]> hmem(new double[m]);
  std::unique_ptr<double[]> Amem(new double[q * n]);
  std::unique_ptr<double[]> bmem(new double[q]);

  Eigen::Map<MatrixXd> H(Hmem.get(), n, n);
  Eigen::Map<VectorXd> f(fmem.get(), n);
  Eigen::Map<MatrixXd> A(Amem.get(), q, n);
  Eigen::Map<VectorXd> b(bmem.get(), q);
  Eigen::Map<MatrixXd> G(Gmem.get(), m, n);
  Eigen::Map<VectorXd> h(hmem.get(), m);
  H << 1, 0, 0, 0;
  f << 1, 0;
  A << 0, 0, 1, 0, 0, 1, -1, 0, 0, -1;
  b << 0, 3, 3, -1, -1;

  FBstabDense::ProblemDataRef data(&H, &f, &G, &h, &A, &b);

  FBstabDense solver(n, m, q);
  FBstabDense::Options opts = FBstabDense::DefaultOptions();
  opts.abs_tol = 1e-8;
  opts.display_level = Display::OFF;
  solver.UpdateOptions(opts);

  SolverOut out = solver.Solve(data, &x0);

  ASSERT_EQ(out.eflag, ExitFlag::SUCCESS);
  EXPECT_NEAR(x0.z(0), 1, 1e-8);
  EXPECT_TRUE((x0.z(1) >= 1) && (x0.z(1) <= 3));

  // Check satisfaction of KKT conditions.
  VectorXd r1 = data.H * x0.z + data.f + data.A.transpose() * x0.v;
  VectorXd r2 = x0.y.cwiseMin(x0.v);

  ASSERT_NEAR(r1.norm() + r2.norm(), 0, 1e-6);
}

/**
 * Tests FBstab with
 *
 * H = [1 0]  f = [1 ]
 *     [0 0]      [-1]
 *
 * A = [1  1] b = [0 ]
 *     [1  0]     [3 ]
 *     [0  1]     [3 ]
 *     [-1 0]     [-1]
 *     [0 -1]     [-1]
 *
 * This QP is infeasible, i.e.,
 * there is no z satisfying Az <= b
 */

GTEST_TEST(FBstabDense, InfeasibleQP) {
  int n = 2;
  int m = 0;
  int q = 5;

  FBstabDense::ProblemData data(n, m, q);
  data.H << 1, 0, 0, 0;
  data.f << 1, -1;
  data.A << 1, 1, 1, 0, 0, 1, -1, 0, 0, -1;
  data.b << 0, 3, 3, -1, -1;

  FBstabDense::Variable x0(n, m, q);

  FBstabDense solver(n, m, q);
  FBstabDense::Options opts = FBstabDense::DefaultOptions();
  opts.abs_tol = 1e-8;
  opts.display_level = Display::OFF;
  solver.UpdateOptions(opts);

  SolverOut out = solver.Solve(data, &x0);

  ASSERT_EQ(out.eflag, ExitFlag::PRIMAL_INFEASIBLE);
}

/**
 * Tests FBstab with
 *
 * H = [1 0]  f = [1 ]
 *     [0 0]      [-1]
 *
 * A = [0  0] b = [0 ]
 *     [1  0]     [3 ]
 *     [-1 0]     [-1]
 *     [0 -1]     [-1]
 *
 * This QP is unbounded below, i.e.,
 * its optimal value is -infinity
 */
GTEST_TEST(FBstabDense, UnboundedQP) {
  int n = 2;
  int m = 0;
  int q = 4;

  FBstabDense::ProblemData data(n, m, q);
  data.H << 1, 0, 0, 0;
  data.f << 1, -1;
  data.A << 0, 0, 1, 0, -1, 0, 0, -1;
  data.b << 0, 3, -1, -1;

  FBstabDense::Variable x0(n, m, q);

  FBstabDense solver(n, m, q);

  FBstabDense::Options opts = FBstabDense::DefaultOptions();
  opts.abs_tol = 1e-8;
  opts.display_level = Display::OFF;
  solver.UpdateOptions(opts);

  SolverOut out = solver.Solve(data, &x0);

  ASSERT_EQ(out.eflag, ExitFlag::DUAL_INFEASIBLE);
}

}  // namespace test
}  // namespace fbstab


// Copyright (c) 2017 Evan S Weinberg
// A reference piece of code which computes matrix elements
// of a reference sparse first derivative operator to fill a dense
// Eigen matrix, then computes the spectrum and prints
// the Eigenvalues.

// This code is for real, indefinite matrices.
// This code lives on github at github.com/weinbe2/eigens-with-eigen/

#include <iostream>
#include <iomanip>
#include <cmath>
#include <string>

// Borrow dense matrix eigenvalue routines.
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

// This is just for convenience. By default, all matrices
// in Eigen are column major. You can replace "Dynamic"
// with a specific number to template just one size.
// You can also ust use "MatrixXd".
typedef Matrix<double, Dynamic, Dynamic, ColMajor> dMatrix;
// Likewise, we can use "MatrixXcd".
typedef Matrix<std::complex<double>, Dynamic, Dynamic, ColMajor> cMatrix;
// We need both real and complex because, while the matrix itself is real,
// the eigenvalues and eigenvectors can generically be complex.

// Reference 1-D central difference operator
void central_diff_1d(double* out, double* in, const int L, const double m2);

int main(int argc, char** argv)
{
  double *in_real;
  double *out_real;
  complex<double> *out_cplx; // needed for eigenvectors

  // Set output precision to be long.
  cout << setprecision(10);

  // Basic information about the lattice.
  const int length = 8;
  const double mass = 0.001;

  // Print the basic info.
  std::cout << "1D central difference operator, length " << length << ", mass " << mass << ", periodic boundary conditions.\n";
  std::cout << "Change the length and mass by modifying the source.\n";

  // Allocate.
  in_real = new double[length];
  out_real = new double[length];
  out_cplx = new complex<double>[length];

  // Zero out.
  for (int i = 0; i < length; i++)
  {
    in_real[i] = out_real[i] = 0.0;
  }

  ///////////////////////////
  // REAL, INDEFINITE CASE //
  ///////////////////////////

  std::cout << "Real, Indefinite case.\n\n";

  // Allocate a sufficiently gigantic matrix.
  dMatrix mat_real = dMatrix::Zero(length, length);

  // Form matrix elements. This is where it's important that
  // dMatrix is column major.
  for (int i = 0; i < length; i++)
  {
    // Set a point on the rhs for a matrix element.
    // If appropriate, zero out the previous point.
    if (i > 0)
    {
      in_real[i-1] = 0.0;
    }
    in_real[i] = 1.0;

    // Zero out the "out" vector. I defined "laplace_1d" to
    // not require this, but I put this here for generality.
    for (int j = 0; j < length; j++)
      out_real[j] = 0.0;

    // PUT YOUR MAT-VEC HERE.
    central_diff_1d(out_real, in_real, length, mass);

    // Copy your output into the right memory location.
    // If your data layout supports it, you can also pass
    // "mptr" directly as your "output vector" when you call
    // your mat-vec.
    double* mptr = &(mat_real(i*length));

    for (int j = 0; j < length; j++)
    {
      mptr[j] = out_real[j];
    }
  }

  // We've now formed the dense matrix. We print it here
  // as a sanity check if it's small enough.
  if (length <= 16)
  {
    std::cout << mat_real << "\n";
  }

  // Get the eigenvalues and eigenvectors.
  EigenSolver< dMatrix > eigsolve_real_indef(length);
  eigsolve_real_indef.compute(mat_real);

  // Remark: if you only want the eigenvalues, you can call
  // eigsolve_real.compute(mat_real, EigenvaluesOnly);

  // Print the eigenvalues.
  // Note that the eigenvalues are of type "cMatrix" as typedef'd above or
  // MatrixXcd generically.
  cMatrix evals = eigsolve_real_indef.eigenvalues();
  std::cout << "The eigenvalues are:\n" << evals << "\n\n";
  // You can also index individual eigenvalues as "evals(i)", where
  // "i" zero-indexes the eigenvalues.

  // Print the eigenvectors if the matrix is small enough.
  // As a remark, this also shows you how to access the eigenvectors.
  if (length <= 16)
  {
    for (int i = 0; i < length; i++)
    {
      // You can use "VectorXcd" as the type instead.
      cMatrix evec = eigsolve_real_indef.eigenvectors().col(i);

      // Print the eigenvector.
      std::cout << "Eigenvector " << i << " equals:\n" << evec << "\n\n";

      // You can also copy the eigenvector into another array as such:
      for (int j = 0; j < length; j++)
      {
        out_cplx[j] = evec(j);
      }
    }
  }

  // Clean up.
  delete[] in_real;
  delete[] out_real;
  delete[] out_cplx;

  return 0;
}

// Reference 1-D central difference function.
void central_diff_1d(double* out, double* in, const int L, const double mass)
{
  // Central difference operator with periodic boundary conditions.
  for (int i = 0; i < L; i++)
  {
    out[i] = mass*in[i] + 0.5*(in[(i+1)%L] - in[(i-1+L)%L]);
  }

  // Done.
  return;
}


#include <iostream>
#include <string>
#include <map>
#include <Eigen/Dense>
#include "../simple_lib/include/two_layer_net.h"

using namespace Eigen;
using namespace MyDL;

int main(){
    using std::cout;
    using std::endl;
    using std::string;
    using std::map;

    int batch_size = 2;
    int input_size = 2;
    int hidden_size = 3;
    int output_size = 2;

    // int batch_size = 3;
    // int input_size = 2;
    // int hidden_size = 3;
    // int output_size = 5;

//     int batch_size = 5;
//     int input_size = 28 * 28;
//     int hidden_size = 100;
//     int output_size = 10;

    TwoLayerNet net(input_size, hidden_size, output_size, 0.01);

    MatrixXd X = MatrixXd::Random(batch_size, input_size);
    MatrixXd y; // (batch_size, output_size)
    MatrixXd t = MatrixXd::Zero(batch_size, output_size);
//     MatrixXd t = MatrixXd::Random(batch_size, output_size);
    double loss;
    double accuracy;
    map<string, MatrixXd> grads, numerical_grads;

    X = -2 * MatrixXd::Identity(2, 2) + MatrixXd::Ones(2, 2);
    t = MatrixXd::Identity(2, 2);

    // X << 1, 2,
    //      3, 4,
    //      5, 6;
    // t << 0, 1, 0, 0, 0,
    //      0, 0, 0, 1, 0,
    //      0, 0, 0, 1, 0;

    y = net.predict(X);
    cout << "y = " << y << endl; // predictの結果は、行方向に総和を取った場合、「1」となっていることを確認

    loss = net.loss(X, t);
    cout << "loss: " << loss << endl;

    accuracy = net.accuracy(X, t);
    cout << "accuracy: " << accuracy << endl; // この入力の場合、accuracyは0.666..が正解

    grads = net.gradient(X, t);
    cout << "gradient of W1: " << grads["W1"] << endl;
    cout << "gradient of W2: " << grads["W2"] << endl;
    cout << "gradient of b1: " << grads["b1"] << endl;
    cout << "gradient of b2: " << grads["b2"] << endl;

    cout << "numerical gradient" << endl;
    numerical_grads = net.numerical_gradient(X, t);
    cout << "gradient of W1: " << numerical_grads["W1"] << endl;
    cout << "gradient of W2: " << numerical_grads["W2"] << endl;
    cout << "gradient of b1: " << numerical_grads["b1"] << endl;
    cout << "gradient of b2: " << numerical_grads["b2"] << endl;

    return 0;
}


//  (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)

#include "math_unit_test.hpp"
#include <boost/math/statistics/z_test.hpp>
#include <boost/math/statistics/univariate_statistics.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <limits>
#include <vector>
#include <random>
#include <utility>

using quad = boost::multiprecision::cpp_bin_float_quad;
using std::sqrt;

template<typename Real>
void test_one_sample_z()
{
    std::pair<Real, Real> temp = boost::math::statistics::one_sample_z_test(Real(10), Real(2), Real(100), Real(10));
    Real computed_statistic = std::get<0>(temp);
    Real computed_pvalue = std::get<1>(temp);
    CHECK_ULP_CLOSE(Real(0), computed_statistic, 5);
    CHECK_MOLLIFIED_CLOSE(Real(0), computed_pvalue, 5*std::numeric_limits<Real>::epsilon());

    temp = boost::math::statistics::one_sample_z_test(Real(10), Real(2), Real(100), Real(5));
    Real computed_statistic_2 = std::get<0>(temp);
    CHECK_ULP_CLOSE(Real(25), computed_statistic_2, 5);

    temp = boost::math::statistics::one_sample_z_test(Real(1)/2, Real(10), Real(100), Real(1)/3);
    Real computed_statistic_3 = std::get<0>(temp);
    CHECK_ULP_CLOSE(Real(1)/6, computed_statistic_3, 5);
}

template<typename Z>
void test_integer_one_sample_z()
{
    std::pair<double, double> temp = boost::math::statistics::one_sample_z_test(Z(10), Z(2), Z(100), Z(10));
    double computed_statistic = std::get<0>(temp);
    double computed_pvalue = std::get<1>(temp);
    CHECK_ULP_CLOSE(0.0, computed_statistic, 5);
    CHECK_MOLLIFIED_CLOSE(0.0, computed_pvalue, 5*std::numeric_limits<double>::epsilon());

    temp = boost::math::statistics::one_sample_z_test(Z(10), Z(2), Z(100), Z(5));
    double computed_statistic_2 = std::get<0>(temp);
    CHECK_ULP_CLOSE(25.0, computed_statistic_2, 5);
}

template<typename Real>
void test_two_sample_z()
{
    std::vector<Real> set_1 {1,2,3,4,5};
    std::vector<Real> set_2 {2,3,4,5,6};

    std::pair<Real, Real> temp = boost::math::statistics::two_sample_z_test(set_2, set_1);
    Real computed_statistic = std::get<0>(temp);
    Real computed_pvalue = std::get<1>(temp);
    CHECK_ULP_CLOSE(Real(1), computed_statistic, 5);
    CHECK_MOLLIFIED_CLOSE(Real(0), computed_pvalue, sqrt(std::numeric_limits<Real>::epsilon()));
}

template<typename Z>
void test_integer_two_sample_z()
{
    std::vector<Z> set_1 {1,2,3,4,5};
    std::vector<Z> set_2 {2,3,4,5,6};

    std::pair<double, double> temp = boost::math::statistics::two_sample_z_test(set_2, set_1);
    double computed_statistic = std::get<0>(temp);
    double computed_pvalue = std::get<1>(temp);
    CHECK_ULP_CLOSE(1.0, computed_statistic, 5);
    CHECK_MOLLIFIED_CLOSE(0.0, computed_pvalue, 5*std::numeric_limits<double>::epsilon());
}

int main()
{
    test_one_sample_z<float>();
    test_one_sample_z<double>();
    test_one_sample_z<quad>();

    test_integer_one_sample_z<int>();
    test_integer_one_sample_z<int32_t>();
    test_integer_one_sample_z<int64_t>();
    test_integer_one_sample_z<uint32_t>();

    test_two_sample_z<float>();
    test_two_sample_z<double>();
    test_two_sample_z<quad>();

    test_integer_two_sample_z<int>();
    test_integer_two_sample_z<int32_t>();
    test_integer_two_sample_z<int64_t>();
    test_integer_two_sample_z<uint32_t>();
}


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXd m(2,3);
m << 2, -4, 6,
     -5, 1, 0;
cout << m.cwiseAbs() << endl;

  return 0;
}


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>

TEST(MathFunctions, inv_logit) {
  using stan::math::inv_logit;
  EXPECT_FLOAT_EQ(0.5, inv_logit(0.0));
  EXPECT_FLOAT_EQ(1.0 / (1.0 + exp(-5.0)), inv_logit(5.0));
}

TEST(MathFunctions, inv_logit_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_PRED1(boost::math::isnan<double>, stan::math::inv_logit(nan));
}


/*
    Boost Competency Test - GSoC 2020
    digu_J - Digvijay Janartha
    NIT Hamirpur - INDIA
*/

#include <algorithm>
#include <iostream>
#include <iterator>
#include <set>
#include <utility>
#include <vector>

#include <boost/geometry/geometry.hpp>

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

namespace algo3
{
// Solves 2D concave hull in O((n ^ 2) * log(n)) complexity for Multipoint concept
// Where n is the total input points
template <typename MultiPoint, typename Size, typename Factor>
class ConcaveHull {

    private:
        MultiPoint hull;
        Size n;
        Factor k;

    public:
        ConcaveHull(MultiPoint mp, Size sz, Factor cur_k);
        void print_hull();
};

template <typename MultiPoint, typename Size, typename Factor>
ConcaveHull<MultiPoint, Size, Factor>::ConcaveHull(MultiPoint mp, Size sz, Factor cur_k)
{
    hull = mp;
    n = sz;
    k = cur_k;
}

// To print the concave hull
template <typename MultiPoint, typename Size, typename Factor>
void ConcaveHull<MultiPoint, Size, Factor>::print_hull()
{
    std::cout << "Current value of k is: " << k << std::endl;
    std::cout << "Resulting points of Concave Hull are: " << std::endl;
    for (Size i = 0; i < n; ++i)
    {
        std::cout << bg::get<0>(hull[i]) << " " << bg::get<1>(hull[i]) << std::endl;
    }
}

template <typename DataType>
static inline DataType max(DataType a, DataType b)
{
    return (a > b ? a : b);
}

template <typename DataType>
static inline DataType min(DataType a, DataType b)
{
    return (a > b ? b : a);
}

// Sorts input in increasing order of x values and in case of ties, increasing y values
template <typename Range>
static inline void sort(Range& range)
{
    typedef typename boost::range_value<Range>::type point_type;
    typedef bg::less<point_type> comparator;

    std::sort(boost::begin(range), boost::end(range), comparator());
}

// This function returns 1 if points are oriented in counter-clockwise manner and -1 if collinear
template <typename Point>
static inline int RightHandTurn(Point a, Point b, Point c)
{
    typedef typename bg::coordinate_type<Point>::type c_type;

    c_type value = (bg::get<1>(b) - bg::get<1>(a)) * (bg::get<0>(c) - bg::get<0>(b))
                   - (bg::get<0>(b) - bg::get<0>(a)) * (bg::get<1>(c) - bg::get<1>(b));
    if (value == 0)
    {
        return -1;
    }
    return (value < 0 ? 1 : 0);
}

// Removes duplicates from input in O(n * log(n)) complexity
template <typename MultiPoint>
static inline void CleanList(MultiPoint& mp)
{
    typedef typename boost::range_size<MultiPoint>::type size_type;
    typedef typename bg::coordinate_type<MultiPoint>::type c_type;
    typedef typename bg::point_type<MultiPoint>::type point_type;
    typedef typename std::pair<c_type, c_type> pair_type;
    typedef typename std::set<pair_type>::iterator itr_type;

    size_type n = boost::size(mp);
    std::set<pair_type> st;
    for (size_type i = 0; i < n; ++i)
    {
        st.insert({bg::get<0>(mp[i]), bg::get<1>(mp[i])});
    }
    bg::clear(mp);

    itr_type it;
    for (it = st.begin(); it != st.end(); ++it)
    {
        pair_type p = *it;
        point_type cur = {p.first, p.second};
        bg::append(mp, cur);
    }
}

// Removes a point from multipoint in O(n) complexity by swapping elements and poping out last one
template <typename MultiPoint, typename Point>
static inline void RemovePoint(MultiPoint& mp, Point cur)
{
    typedef typename boost::range_size<MultiPoint>::type size_type;

    size_type n = boost::size(mp);
    for (size_type i = 0; i < n; ++i)
    {
        if (bg::equals(mp[i], cur))
        {
            for (size_type j = i; j < n - 1; ++j)
            {
                mp[j] = mp[j + 1];
            }
            break;
        }
    }
    mp.pop_back();
}

// Building Rtree for a given container
template <typename MultiPoint, typename RTree>
inline void BuildRTree(MultiPoint& mp, RTree& rt)
{
    typedef typename boost::range_size<MultiPoint>::type size_type;

    size_type n = boost::size(mp);
    for (size_type i = 0; i < n; ++i)
    {
        bgi::insert(rt, mp[i]);
    }
}

// Runs the main algorithm on dataset of points for a particular k value
// Returns empty container when current k gives no solution
template <typename MultiPoint, typename Factor>
static inline MultiPoint Solve(MultiPoint dataset, Factor k)
{
    typedef typename bg::point_type<MultiPoint>::type point_type;
    typedef typename boost::range_size<MultiPoint>::type size_type;
    typedef typename bg::model::linestring<point_type> linestring_type;
    typedef typename bg::model::segment<point_type> segment_type;
    typedef typename bg::model::polygon<point_type> poly_type;

    MultiPoint ans, discard;
    bg::append(ans, dataset[0]);
    point_type cur_point = dataset[0];
    RemovePoint(dataset, cur_point);
    int step = 0;

    bgi::rtree<point_type, bgi::rstar<32>> rt1;
    bgi::rtree<segment_type, bgi::rstar<8>> rt2;
    BuildRTree(dataset, rt1);

    while (boost::size(dataset))
    {
        if (step == 3)
        {
            // once a polygon with more than 3 edges is formed add the first point again
            bg::append(dataset, ans[0]);
            bgi::insert(rt1, ans[0]);
        }

        // finding k-NN using RTree
        std::vector<point_type> k_nearest;
        Factor cur_k = min(k, Factor(boost::size(dataset)));
        rt1.query(bgi::nearest(cur_point, cur_k), std::back_inserter(k_nearest));

        size_type size1 = boost::size(k_nearest);
        std::vector<point_type> good;
        for (size_type i = 0; i < size1; ++i)
        {
            bool ok = true;
            linestring_type ls1{k_nearest[i], cur_point};

            // querying RTree to find all the intersecting edges of the hull with the current edge
            std::vector<segment_type> bad;
            segment_type seg(k_nearest[i], cur_point);
            rt2.query(bgi::intersects(seg), std::back_inserter(bad));

            size_type size2 = boost::size(bad);
            if (size2 == 1 and bg::equals(k_nearest[i], ans[0]))
            {
                // if linestring touches first edge, then its the last edge and is ok
                linestring_type ls2{ans[0], ans[1]};
                if (!bg::touches(ls1, ls2))
                {
                    // if this linestring does not touch and only intersect then discard this point
                    ok = false;
                }
            }
            else if (size2 > 0)
            {
                // interesections means point is discarded
                ok = false;
            }

            if (ok)
            {
                // non intersecting edge is good
                good.push_back(k_nearest[i]);
            }
        }
        if (good.empty())
        {
            // if no good points are remaining, current k has no solution
            return discard;
        }

        point_type best = good[0];
        size1 = boost::size(good);
        for (size_type i = 1; i < size1; ++i)
        {
            int value = RightHandTurn(cur_point, good[i], best);
            if (value == -1)
            {
                // in case of collinear points take the nearest one as best
                if (bg::distance(good[i], cur_point) < bg::distance(best, cur_point))
                {
                    best = good[i];
                }
            }
            else if (value)
            {
                // update best if we have counter-clockwise orientation
                best = good[i];
            }
        }
        if (step)
        {
            // adding previously formed edge to RTree
            size_type last = boost::size(ans) - 2;
            segment_type cur_segment(cur_point, ans[last]);
            bgi::insert(rt2, cur_segment);
        }
        cur_point = best;
        bg::append(ans, cur_point);
        RemovePoint(dataset, cur_point);
        bgi::remove(rt1, cur_point);

        if (bg::equals(cur_point, ans[0]))
        {
            // cycle completed
            break;
        }
        ++step;
    }

    // If any point is outside the final polygon, then current k has no solution
    poly_type poly;
    for (size_type i = 0; i < boost::size(ans); ++i)
    {
        bg::append(poly, ans[i]);
    }
    size_type n = boost::size(dataset);
    for (size_type i = 0; i < n; ++i)
    {
        if (!(bg::covered_by(dataset[i], poly)))
        {
            return discard;
        }
    }
    return ans;
}

// Driver code
template <typename MultiPoint, typename Factor>
inline void ConcaveHullKNN(MultiPoint input, MultiPoint& hull, Factor& k)
{
    typedef typename boost::range_size<MultiPoint>::type size_type;

    CleanList(input);
    size_type n = boost::size(input);
    k = max(k, 2);
    k = min(k, Factor(n));

    // At least three different points are required for concave hull to be formed
    if (n < size_type(3))
    {
        return;
    }
    else if (n == size_type(3))
    {
        hull = input;
        return;
    }

    sort(input);
    while (k <= Factor(n))
    {
        hull = Solve(input, k);
        // If current k given empty solution, then k is increased by one
        if (bg::is_empty(hull))
        {
            ++k;
        }
        else
        {
            break;
        }
    }

    // Uncomment lines below to print information about current concave hull
    // size_type h = boost::size(hull);
    // ConcaveHull<MultiPoint, size_type, Factor> debug(hull, h, k);
    // debug.print_hull();
}

} // namespace algo3


#include <iostream>
#include <blitz.h>

using namespace blitz;
// M N
Shape input_shape(2);
// N M
Shape output_shape(2);

void output_matrix(size_t dim_left, size_t dim_right, float* matrix) {
  for (size_t i = 0; i < dim_left; ++i) {
    for (size_t j = 0; j < dim_right; ++j) {
      std::cout << matrix[i * dim_right + j] << " ";
    }
    std::cout << std::endl;
  }
}

void transpose(size_t m, size_t n) {
  // set up cpu
  CPUTensor<float> input_cpu(input_shape);
  CPUTensor<float> output_cpu(output_shape);
  // init values
  std::cout << "Init:" << std::endl;
  Backend<CPUTensor, float>::UniformDistributionFunc(&input_cpu, 0.0, 1.0);
  output_matrix(m, n, input_cpu.data());
  std::cout << "Transposed:" << std::endl;
  // transpose
  Backend<CPUTensor, float>::Transpose2DFunc(&input_cpu, &output_cpu);
  output_matrix(n, m, output_cpu.data());
}

int main(int argc, char** argv) {
  const size_t NUM_ARGS = 2;
  // M N
  if (argc != NUM_ARGS + 1) {
    std::cerr << "Not matchable args!" << std::endl;
    exit(1);
  }
  const size_t M = atoi(argv[1]);
  const size_t N = atoi(argv[2]);
  // set shapes
  input_shape[0] = M;
  input_shape[1] = N;
  output_shape[0] = N;
  output_shape[1] = M;
  // run
  transpose(M, N);
  return 0;
}


#ifndef AMT_ML_MODEL_LINEAR_REGRESSION_OPTIMIZER_HPP
#define AMT_ML_MODEL_LINEAR_REGRESSION_OPTIMIZER_HPP

#include <armadillo>
#include <functional>

namespace amt::regression{

    struct default_opt{

        auto operator()(arma::Mat<double>& beta, arma::Mat<double> const& x, arma::Mat<double> const& y, double lm) const {
            // x_t = x^T
            auto x_t = x.t();

            // xt_y = x^T * y
            auto xt_y = x_t * y;

            if( lm == 0 ){
                // xt_x = x^T * x
                auto xt_x = x_t * x;

                // (X * X^T) * B = X^T * Y
                beta = arma::solve(xt_x,xt_y,arma::solve_opts::fast);
            }else{

                arma::Mat<double> l_I = arma::eye(x.n_cols, x.n_cols) * lm;

                // xt_x = x^T * x + l_I
                auto xt_x_lI = ( x_t * x ) + l_I;

                // (X * X^T) * B = X^T * Y
                beta = arma::solve(xt_x_lI,xt_y,arma::solve_opts::fast);
            }

        }

    };

    struct gradient_descent{

        constexpr gradient_descent() noexcept = default;
        constexpr gradient_descent(gradient_descent const&  other) noexcept = default;
        constexpr gradient_descent(gradient_descent &&  other) noexcept = default;
        constexpr gradient_descent& operator=(gradient_descent const&  other) noexcept = default;
        constexpr gradient_descent& operator=(gradient_descent &&  other) noexcept = default;
        ~gradient_descent() = default;

        constexpr gradient_descent(double alpha, std::size_t iter)
            : alpha(alpha)
            , iteration(iter)
        {}

        auto operator()(arma::Mat<double>& beta, arma::Mat<double> const& x, arma::Mat<double> const& y, double lm) const {
            beta.resize(x.n_cols, 1ul);
            beta.randu();
            if( lm == 0.0 )
                eval(beta,x,y,lm);
            else
                eval(beta,x,y);
        }

        // double compute_cost(arma::Mat<double> const& beta, arma::Mat<double> const& x, arma::Mat<double> const& y){
        //     auto temp = ( x * beta ) - y;
        //     auto m = 2 * x.n_rows;
        //     auto J = ( temp * temp.t() ) / static_cast<double>(m);
        //     return J[0];
        // }

        void eval(arma::Mat<double>& beta, arma::Mat<double> const& x, arma::Mat<double> const& y, double lm) const{
            auto m = alpha / static_cast<double>(x.n_rows);
            auto reg_coef = m * lm;

            auto x_t = x.t();
            for( auto i = 0u; i < iteration; ++i ){
                auto p = ( x * beta ) - y;
                auto temp = m * (x_t * p);
                beta -= ( temp + ( reg_coef * beta ) );
            }
        }

        void eval(arma::Mat<double>& beta, arma::Mat<double> const& x, arma::Mat<double> const& y) const{
            auto m = alpha / static_cast<double>(x.n_rows);
            auto x_t = x.t();
            for( auto i = 0u; i < iteration; ++i ){
                auto p = ( x * beta ) - y;
                auto temp = m * (x_t * p);
                beta -= temp;
            }
        }

        double alpha{0.01};
        std::size_t iteration{1500};
    };

    template<typename T>
    inline static constexpr bool is_default_opt_v = std::is_same_v<T,default_opt>;

    template<typename T>
    inline static constexpr bool is_gradient_descent_v = std::is_same_v<T,gradient_descent>;

} // namespace amt::regression

#endif


/**
 * @file eigen.cpp
 * @author Wei Du
 *
 * @version 0.1
 * @date 2021-08-18
 */

// STL include
#include <array>
#include <cassert>
#include <iostream>

// external include
#include <Eigen/Dense>
#include <Eigen/Geometry>

/*
 * int main() {
 *   //  x, y, z, w
 *   std::array<double, 4> arr{0.3, 0.27, 0, 1.0};
 *   Eigen::Vector4d vct{0.3, 0.27, 0, 1};
 *
 *   // Eigen::Quaterniond quat1(vct(0), vct(1), vct(2), vct(3));
 *   // Eigen::Quaterniond quat2(arr[0], arr[1], arr[2], arr[3]);
 *   Eigen::Quaterniond quat1(vct.data());
 *   Eigen::Quaterniond quat2(arr.data());
 *
 *   printf("should be: %.2f %.2f %.2f %.2f\n", arr[0], arr[1], arr[2], arr[3]);
 *   printf("%.2f %.2f %.2f %.2f\n", quat1.x(), quat1.y(), quat1.z(),
 * quat1.w()); printf("%.2f %.2f %.2f %.2f\n", quat2.x(), quat2.y(), quat2.z(),
 * quat2.w()); return 0;
 * }
 */

/*
 * int main() {
 *   Eigen::VectorXd v1(4);
 *   Eigen::Vector3d v2{0.1, 0.2, 0.3};
 *
 *   v1.head(3) = v2;
 *   v1(3) = 0.8;
 *   std::cout << v1 << std::endl;
 *   return 0;
 * }
 */

/*
int main() {
  Eigen::Vector2d tran{0.2, 0.3};
  Eigen::Rotation2Dd ori(0.785398);

  Eigen::Affine2d pose;
  pose.setIdentity();
  pose.translate(tran);
  pose.rotate(ori);

  Eigen::Vector2d origin{0.1, 0.0};
  Eigen::Vector2d newpt = pose * origin;
  std::cout << newpt << std::endl;
  return 0;
}
*/

/*
int main() {
  Eigen::Vector3d vct1;
  Eigen::VectorXd vct2;
  if (vct1.isApprox(vct2)) {
    printf("they are the same\n");
  } else {
    printf("they are not the same\n");
  }
  std::cout << vct1 << std::endl;

  return 0;
}
*/

/*
 * int main() {
 *   Eigen::MatrixXd mt;
 *   mt.resize(1000, 1000);
 *   mt.fill(-1);
 *   std::cout << mt;
 *   return 0;
 * }
 */

/*
 * int main() {
 *   Eigen::MatrixXd mat(5, 4);
 *   mat << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19,
 * 20;
 *
 *   std::cout << mat.rows() << std::endl;
 *   mat.conservativeResize(mat.rows() - 2, 3);
 *   std::cout << mat << std::endl;
 *   std::cout << mat.rows() << std::endl;
 *
 *   return 0;
 * }
 */

/*
 * int main() {
 *   const Eigen::Vector3d vec{0, 1, 0};
 *   std::cout << vec << std::endl;
 *   return 0;
 * }
 */

int main() {
  Eigen::MatrixXd mt(2, 2);
  mt << 0, 1, 2, 3;

  Eigen::MatrixXd new_mt = mt;
  mt(1, 1) = 8;
  std::cout << mt << std::endl;
  std::cout << new_mt << std::endl;

  return 0;
}


#include "convex_hull.hpp"

#include <boost/test/unit_test.hpp>

using namespace GooBalls;
using namespace Spatial;
using namespace impl;

BOOST_AUTO_TEST_CASE(convex_hull_angles){
	Point p0;
	p0.x = 1;
	p0.y = 1;

	std::vector<Point> ps(3);

	// -45°
	ps[0].x = 2;
	ps[0].y = 0;

	// 0°
	ps[1].x = 2;
	ps[1].y = 1;

	ps[2].x = 2;
	ps[2].y = 2;

	computeAngles(ps.begin(), ps.end(), p0);

	BOOST_CHECK_EQUAL(-45.0/180*M_PI, ps[0].angle);
	BOOST_CHECK_EQUAL(0.0/180*M_PI, ps[1].angle);
	BOOST_CHECK_EQUAL(45.0/180*M_PI, ps[2].angle);
}

BOOST_AUTO_TEST_CASE(convex_hull_turn_direction){
	Point p0;
	p0.x = 1;
	p0.y = 1;
	Point p1;
	p1.x = 2;
	p1.y = 1;
	Point p2;
	p2.x = 1;
	p2.y = 2;
	Point p3;
	p3.x = 3;
	p3.y = 1;

	BOOST_CHECK_EQUAL(Left, turnDirection(p0, p1, p2));
    BOOST_CHECK_EQUAL(Right, turnDirection(p0, p2, p1));
	BOOST_CHECK_EQUAL(None, turnDirection(p0, p1, p3));
}

BOOST_AUTO_TEST_CASE(convex_hull_test1){
	Coordinates2d pointSet(5, 2);
	// lower left corner
	pointSet(0,0) = -1;
	pointSet(0,1) = -1;
	// upper left corner
	pointSet(1,0) = -1;
	pointSet(1,1) = 1;
	// center
	pointSet(2,0) = 0.5;
	pointSet(2,1) = -0.1;
	// lower right corner
	pointSet(3,0) = 1;
	pointSet(3,1) = -1;
	// upper right corner
	pointSet(4,0) = 1;
	pointSet(4,1) = 1;

    pointSet = convex_hull(pointSet);

    BOOST_CHECK_EQUAL(4, pointSet.rows());
    // To be precise: this uses knowledge of the implementation, according to the contract, we could start with any point of the convex hull
    BOOST_CHECK_EQUAL(-1, pointSet(0,0));
    BOOST_CHECK_EQUAL(-1, pointSet(0,1));
    BOOST_CHECK_EQUAL(1, pointSet(1,0));
    BOOST_CHECK_EQUAL(-1, pointSet(1,1));
    BOOST_CHECK_EQUAL(1, pointSet(2,0));
    BOOST_CHECK_EQUAL(1, pointSet(2,1));
    BOOST_CHECK_EQUAL(-1, pointSet(3,0));
    BOOST_CHECK_EQUAL(1, pointSet(3,1));
}


//============================================================================
// Daniel J. Greenhoe
// normed linear space R^2
//============================================================================
//=====================================
// headers
//=====================================
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <Eigen/Dense>
#include "r1.h"
#include "r3.h"

//=====================================
// VectR3
//=====================================*/
//-----------------------------------------------------------------------------
//! \brief Calculate magnitude of vector
//-----------------------------------------------------------------------------
double vectR3::mag(void) const
{
  const double x = getx();
  const double y = gety();
  const double z = getz();
  const double magSq  = x*x + y*y + z*z;
  const double magVal = sqrt( magSq );
  return magVal;
}

//-----------------------------------------------------------------------------
//! \brief Convert polar (r, theta, phi) coordinate to rectangular (x,y,z) coordinate
//-----------------------------------------------------------------------------
void vectR3::polartoxyz(const double r, const double theta, const double phi)
{
  const double x = r*cos(phi)*cos(theta);
  const double y = r*cos(phi)*sin(theta);
  const double z = r*sin(phi);
  otriple::put( x, y, z );
}

//-----------------------------------------------------------------------------
//! \brief Add vector q to vectR3 vector
//-----------------------------------------------------------------------------
void vectR3::operator+=(const vectR3 q)
{
  Eigen::Map< Eigen::Vector3d > a(   getdataa() );
  const Eigen::Map< const Eigen::Vector3d > b( q.getdata() );
  a += b;
}

//-----------------------------------------------------------------------------
//! \brief Subtract vector q from vectR3 vector
//-----------------------------------------------------------------------------
void vectR3::operator-=(const vectR3 q)
{
  Eigen::Map< Eigen::Vector3d > a(   getdataa() );
  const Eigen::Map< const Eigen::Vector3d > b( q.getdata() );
  a -= b;
}

//=====================================
// seqR3
//=====================================*/
//-----------------------------------------------------------------------------
//! \brief Constructor initializing seqR3 to 0
//-----------------------------------------------------------------------------
seqR3::seqR3(long M)
{
  N=M;
  seqr3 = new vectR3[N];
  clear();
}

//-----------------------------------------------------------------------------
//! \brief Constructor initializing seqR3 to <u>
//-----------------------------------------------------------------------------
seqR3::seqR3(long M, double u)
{
  N = M;
  seqr3 = new vectR3[N];
  fill( u );
}

//-----------------------------------------------------------------------------
//! \brief Fill the seqR3 with a value 0
//-----------------------------------------------------------------------------
void seqR3::clear(void)
{
  for( long n=0; n<N; n++) put( n, 0 );
}

//-----------------------------------------------------------------------------
//! \brief Fill the seqR3 with a value <u>
//-----------------------------------------------------------------------------
void seqR3::fill(double u)
{
  long n;
  for(n=0; n<N; n++) put( n, u );
}

//-----------------------------------------------------------------------------
//! \brief Put a single value <u> into the seqR3 x at location n
//-----------------------------------------------------------------------------
int seqR3::put(long n, double u, double v, double w)
{
  if(n<N){
    seqr3[n].otriple::put( u, v, w );
    return 0;
    }
  else{
    fprintf(stderr,"n=%ld larger than seqR3 size N=%ld\n",n,N);
    return -1;
    }
}

//-----------------------------------------------------------------------------
//! \brief Put a single value <u> into the seqR3 x at location n
//-----------------------------------------------------------------------------
int seqR3::put(long n, vectR3 abc)
{
  if(n<N)
  {
    (seqr3[n]).otriple::put( abc.getx(), abc.gety(), abc.gety() );
    return 0;
  }
  else
  {
    fprintf(stderr,"n=%ld larger than seqR3 size N=%ld\n",n,N);
    return -1;
  }
}

//-----------------------------------------------------------------------------
//! \brief Get a single value from the sequence x at location n
//-----------------------------------------------------------------------------
vectR3 seqR3::get(long n)
{
  return seqr3[n];
}

//-----------------------------------------------------------------------------
//! \brief Get the x element from the sequence at location n
//-----------------------------------------------------------------------------
double seqR3::getx(long n)
{
  double u=0;
  if(n<N)u = seqr3[n].getx();
  else   fprintf(stderr,"n=%ld larger than x seqR3 size N=%ld\n",n,N);
  return u;
}

//-----------------------------------------------------------------------------
//! \brief Get the y element from the sequence at location n
//-----------------------------------------------------------------------------
double seqR3::gety(long n){
  double u=0;
  if(n<N)u = seqr3[n].gety();
  else   fprintf(stderr,"n=%ld larger than y seqR3 size N=%ld\n",n,N);
  return u;
  }

//-----------------------------------------------------------------------------
//! \brief Get the z element from the sequence at location n
//-----------------------------------------------------------------------------
double seqR3::getz(long n){
  double u=0;
  if(n<N)u = seqr3[n].getz();
  else   fprintf(stderr,"n=%ld larger than z seqR3 size N=%ld\n",n,N);
  return u;
  }

//-----------------------------------------------------------------------------
//! \brief list contents of sequence
//-----------------------------------------------------------------------------
void seqR3::list(const long start, const long end, const char *str1, const char *str2, FILE *ptr){
  long n,m;
  if(ptr!=NULL){
    if(strlen(str1)>0) fprintf(ptr,"%s",str1);
    for(n=start,m=1; n<=end; n++,m++){
      fprintf(ptr,"(%6.3lf,%6.3lf,%6.3lf) ", seqr3[n].getx(), seqr3[n].gety(), seqr3[n].getz() );
      if(m%3==0)fprintf(ptr,"\n");
      }
    if(strlen(str2)>0)fprintf(ptr,"%s",str2);
    }
  }

//-----------------------------------------------------------------------------
//! \brief list contents of seqR3 using 1 digit per element
//-----------------------------------------------------------------------------
void seqR3::list1(void){
  long n,m;
  for(n=0,m=1; n<N; n++,m++){
    printf("(%2.0lf,%2.0lf,%2.0lf)   ", seqr3[n].getx(), seqr3[n].gety(), seqr3[n].getz() );
    if(m%5==0)printf("\n");
    }
  }
void seqR3::list1(long start, long end){
  long n,m;
  for(n=start,m=1; n<=end; n++,m++){
    printf("(%2.0lf,%2.0lf,%2.0lf)   ", seqr3[n].getx(), seqr3[n].gety(), seqr3[n].getz() );
    if(m%50==0)printf("\n");
    else if(m%10==0)printf(" ");
    }
  }

//=====================================
// external operations
//=====================================
//-----------------------------------------------------------------------------
//! \brief operator: return p+q
//-----------------------------------------------------------------------------
vectR3 operator+(vectR3 p, vectR3 q)
{
  const Eigen::Map< const Eigen::Vector3d > a( p.getdataa() );
  const Eigen::Map< const Eigen::Vector3d > b( q.getdataa() );
  const Eigen::Vector3d c = a + b;
  const vectR3 r( c(0), c(1), c(2) );
  return r;
}

//-----------------------------------------------------------------------------
//! \brief operator: return p-q
//-----------------------------------------------------------------------------
vectR3 operator-(vectR3 p, vectR3 q){
  const Eigen::Map< const Eigen::Vector3d > a( p.getdataa() );
  const Eigen::Map< const Eigen::Vector3d > b( q.getdataa() );
  const Eigen::Vector3d c = a - b;
  const vectR3 r( c(0), c(1), c(2) );
  return r;
  }

//-----------------------------------------------------------------------------
//! \brief  operator: return -p
//-----------------------------------------------------------------------------
vectR3 operator-(vectR3 p)
{
  const Eigen::Map< const Eigen::Vector3d > a( p.getdataa() );
  const Eigen::Vector3d b = -a;
  const vectR3 q( b(0), b(1), b(2) );
  return q;
}

//-----------------------------------------------------------------------------
//! \brief return the angle theta in radians between the two vectors induced by
//! the points <p> and <q> in the space R^3.
//! \returns On SUCCESS return theta in the closed interval [0:PI];
//!          On ERROR   return negative value or exit with value EXIT_FAILURE
//-----------------------------------------------------------------------------
double pqtheta(const vectR3 p, const vectR3 q)
{
  const double rp=p.mag();
  const double rq=q.mag();
  if(rp==0) return -1;
  if(rq==0) return -2;
  const double y = (p^q)/(rp*rq);
  if(y>+1)  {fprintf(stderr,"\nERROR using pqtheta(vectR3 p, vectR3 q): (p^q)/(rp*rq)=%lf>+1\n",y); exit(EXIT_FAILURE);}
  if(y<-1)  {fprintf(stderr,"\nERROR using pqtheta(vectR3 p, vectR3 q): (p^q)/(rp*rq)=%lf<-1\n",y); exit(EXIT_FAILURE);}
  const double theta = acos(y);
  return theta;
}

//-----------------------------------------------------------------------------
//! \brief Return the minimum element of the tupple
//-----------------------------------------------------------------------------
double otriple::min(void) const
{
  const Eigen::Map< const Eigen::Vector3d > abc( getdata() );
  int row, col;
  const double min = abc.minCoeff( &row, &col );
  return min;
}

//-----------------------------------------------------------------------------
//! \brief Return the maximum element of the tupple
//-----------------------------------------------------------------------------
double otriple::max(void) const
{
  const Eigen::Map< const Eigen::Vector3d > abc( getdata() );
  int row, col;
  const double max = abc.maxCoeff( &row, &col );
  return max;
}


#define BOOST_TEST_MODULE "test_matrix_vector"

#ifdef UNITTEST_FRAMEWORK_LIBRARY_EXIST
#include <boost/test/unit_test.hpp>
#else
#define BOOST_TEST_NO_LIB
#include <boost/test/included/unit_test.hpp>
#endif

#include <mill/math/Vector.hpp>

#include <random>
constexpr static unsigned int seed = 32479327;
constexpr static std::size_t N = 10000;

BOOST_AUTO_TEST_CASE(multiple_unit_matrix_and_vector)
{
    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> uni(-1.0, 1.0);
    const mill::Matrix<double, 3, 3> E(1., 0., 0., 0., 1., 0., 0., 0., 1.);

    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const mill::Vector<double, 3> lhs(uni(mt), uni(mt), uni(mt));
        const mill::Vector<double, 3> same = E * lhs;

        BOOST_CHECK_EQUAL(lhs[0], same[0]);
        BOOST_CHECK_EQUAL(lhs[1], same[1]);
        BOOST_CHECK_EQUAL(lhs[2], same[2]);
    }

    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const mill::Vector<double, 3> lhs(uni(mt), uni(mt), uni(mt));
        const mill::Matrix<double, 1, 3> same = mill::transpose(lhs) * E;
        const mill::Vector<double, 3> rhs = mill::transpose(same);

        BOOST_CHECK_EQUAL(lhs[0], rhs[0]);
        BOOST_CHECK_EQUAL(lhs[1], rhs[1]);
        BOOST_CHECK_EQUAL(lhs[2], rhs[2]);
    }
}

BOOST_AUTO_TEST_CASE(multiple_matrix_and_vector)
{
    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> uni(-1.0, 1.0);

    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const double coef = uni(mt);
        const mill::Matrix<double, 3, 3> M(coef, 0., 0., 0., coef, 0., 0., 0., coef);
        const mill::Vector<double, 3> lhs(uni(mt), uni(mt), uni(mt));
        const mill::Vector<double, 3> same = M * lhs;

        BOOST_CHECK_EQUAL(lhs[0] * coef, same[0]);
        BOOST_CHECK_EQUAL(lhs[1] * coef, same[1]);
        BOOST_CHECK_EQUAL(lhs[2] * coef, same[2]);
    }
}


#ifndef MATHTOOLBOX_STRONG_WOLFE_CONDITIONS_LINE_SEARCH_HPP
#define MATHTOOLBOX_STRONG_WOLFE_CONDITIONS_LINE_SEARCH_HPP

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

#define MATHTOOLBOX_VERBOSE_LINE_SEARCH_WARNINGS

namespace mathtoolbox
{
    namespace optimization
    {
        // Algorithm 3.5: Line Search Algorithm
        //
        // This algoritmh tries to find an appropriate step size that satisfies the strong Wolfe conditions (i.e., both
        // the safficient decreasing condition and the curvature condition).
        inline double
        RunStrongWolfeConditionsLineSearch(const std::function<double(const Eigen::VectorXd&)>&          f,
                                           const std::function<Eigen::VectorXd(const Eigen::VectorXd&)>& g,
                                           const Eigen::VectorXd&                                        x,
                                           const Eigen::VectorXd&                                        p,
                                           const double                                                  alpha_init,
                                           const double alpha_max = 50.0,
                                           const double c_1       = 1e-04,
                                           const double c_2       = 0.9)
        {
            auto phi      = [&](const double alpha) { return f(x + alpha * p); };
            auto phi_grad = [&](const double alpha) { return static_cast<double>(g(x + alpha * p).transpose() * p); };

            const double phi_zero      = phi(0.0);
            const double phi_grad_zero = phi_grad(0.0);

            double alpha_prev     = 0.0;
            double alpha          = alpha_init;
            double phi_alpha_prev = phi_zero;

            bool is_first = true;

            // Algorithm 3.6: Zoom
            auto zoom = [&](double alpha_l, double alpha_h) {
                constexpr unsigned int max_num_iterations = 50;
                for (unsigned int i = 0; i < max_num_iterations; ++i)
                {
                    const double alpha_j     = 0.5 * (alpha_l + alpha_h); // TODO: Use a better strategy
                    const double phi_alpha_j = phi(alpha_j);

                    if (phi_alpha_j > phi_zero + c_1 * alpha_j * phi_grad_zero || phi_alpha_j >= phi(alpha_l))
                    {
                        alpha_h = alpha_j;
                    }
                    else
                    {
                        const double phi_grad_alpha_j = phi_grad(alpha_j);
                        if (std::abs(phi_grad_alpha_j) <= -c_2 * phi_grad_zero)
                        {
                            return alpha_j;
                        }
                        if (phi_grad_alpha_j * (alpha_h - alpha_l) >= 0.0)
                        {
                            alpha_h = alpha_l;
                        }
                        alpha_l = alpha_j;
                    }
                }
#ifdef MATHTOOLBOX_VERBOSE_LINE_SEARCH_WARNINGS
                std::cerr << "Warning: The line search did not converge." << std::endl;
#endif
                return 0.5 * (alpha_l + alpha_h);
            };

            constexpr unsigned int max_num_iterations = 50;
            for (int i = 0; i < max_num_iterations; ++i)
            {
                const double phi_alpha = phi(alpha);

                if (phi_alpha > phi_zero + c_1 * alpha * phi_grad_zero || (!is_first && (phi_alpha >= phi_alpha_prev)))
                {
                    return zoom(alpha_prev, alpha);
                }

                const double phi_grad_alpha = phi_grad(alpha);

                if (std::abs(phi_grad_alpha) <= -c_2 * phi_grad_zero)
                {
                    return alpha;
                }

                if (phi_grad_alpha >= 0.0)
                {
                    return zoom(alpha, alpha_prev);
                }

                alpha_prev     = alpha;
                phi_alpha_prev = phi_alpha;

                alpha = 0.5 * (alpha + alpha_max); // TODO: Use a better strategy
            }
#ifdef MATHTOOLBOX_VERBOSE_LINE_SEARCH_WARNINGS
            std::cerr << "Warning: The line search did not converge." << std::endl;
#endif
            return alpha_init;
        }
    } // namespace optimization
} // namespace mathtoolbox

#endif // MATHTOOLBOX_STRONG_WOLFE_CONDITIONS_LINE_SEARCH_HPP


/*
 * Rand_normal: generate random numbers (normal distribution) using boost::random.
 *
 * Copyright (c) 2012 Jérémie Decock
 *
 * Required: boost.random library
 * Usage: g++ rand_normal.cc
 *
 */

#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
#include <ctime>
#include <iostream>

const int num_it = 100000;
const double min = 1.0;
const double max = 10.0;
const double mu = 0.0;
const double sigma = 1.0;

main() {

    // Pseudo-random number generator
    //boost::mt19937 rng;               // deterministic
    boost::mt19937 rng(std::time(NULL));

    // Random number distribution
    boost::normal_distribution<> distrib(mu, sigma);

    // Generator (wrapper)
    boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > random_generator(rng, distrib);

    for(int i=0 ; i<num_it ; i++) {
        double x = random_generator();
        double y = random_generator();
        std::cout << x << " " << y << std::endl;
    }

}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

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

//[comparable_distance
//` Shows how to efficiently get the closest point

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include <boost/numeric/conversion/bounds.hpp>
#include <boost/foreach.hpp>

int main()
{
    typedef boost::geometry::model::d2::point_xy<double> point_type;

    point_type p(1.4, 2.6);

    std::vector<point_type> v;
    for (double x = 0.0; x <= 4.0; x++)
    {
        for (double y = 0.0; y <= 4.0; y++)
        {
            v.push_back(point_type(x, y));
        }
    }

    point_type min_p;
    double min_d = boost::numeric::bounds<double>::highest();
    BOOST_FOREACH(point_type const& pv, v)
    {
        double d = boost::geometry::comparable_distance(p, pv);
        if (d < min_d)
        {
            min_d = d;
            min_p = pv;
        }
    }

    std::cout
        << "Closest: " << boost::geometry::dsv(min_p) << std::endl
        << "At: " << boost::geometry::distance(p, min_p) << std::endl;

    return 0;
}

//]

//[comparable_distance_output
/*`
Output:
[pre
Closest: (1, 3)
At: 0.565685
]
*/
//]


#include <Eigen/Core>
#include <Eigen/LU>

#include <algorithm>
#include <cmath>

#include "gtest/gtest.h"
#include "math/matrix/rq_decomposition.h"

namespace GraphSfM {

using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::Matrix3f;
using Eigen::Matrix4d;
using Eigen::Matrix4f;
using Eigen::MatrixXd;

const double kSmallTolerance = 1e-12;
const double kLargeTolerance = 1e-6;

template <typename MatrixType>
void TestRQDecomposition(const int rows, const int cols, const double tol) {
  for (int i = 0; i < 100; ++i) {
    const MatrixType& m = MatrixType::Random(rows, cols);
    RQDecomposition<MatrixType> rq(m);
    EXPECT_TRUE(rq.matrixR().bottomRows(std::min(rows, cols))
                    .isUpperTriangular());
    EXPECT_TRUE(rq.matrixQ().isUnitary());
    EXPECT_NEAR(rq.matrixQ().determinant(), 1.0, kLargeTolerance);

    const MatrixType& diff = rq.matrixR() * rq.matrixQ() - m;
    EXPECT_LE(diff.cwiseAbs().maxCoeff(), tol)
        << rq.matrixR() << "\n\n" << rq.matrixQ() << "\n\n"
        << rq.matrixR() * rq.matrixQ() << "\n\n" << m;
  }
}

TEST(RQTest, FixedSizeMatrices) {
  // Square matrices.
  TestRQDecomposition<Matrix3d>(3, 3, kSmallTolerance);
  TestRQDecomposition<Matrix4d>(4, 4, kSmallTolerance);
  TestRQDecomposition<Matrix<double, 5, 5> >(5, 5, kSmallTolerance);
  TestRQDecomposition<Matrix<double, 6, 6> >(6, 6, kSmallTolerance);

  // Non-square matrices.
  TestRQDecomposition<Matrix<double, 3, 4> >(3, 4, kSmallTolerance);
  TestRQDecomposition<Matrix<double, 4, 3> >(4, 3, kSmallTolerance);
}

TEST(RQTest, DynamicSizedMatrices) {
  // Square matrices.
  TestRQDecomposition<MatrixXd>(25, 25, kSmallTolerance);
  TestRQDecomposition<MatrixXd>(50, 50, kSmallTolerance);
  TestRQDecomposition<MatrixXd>(75, 75, kSmallTolerance);
  TestRQDecomposition<MatrixXd>(100, 100, kSmallTolerance);

  // Non-square matrices.
  TestRQDecomposition<MatrixXd>(30, 40, kSmallTolerance);
  TestRQDecomposition<MatrixXd>(40, 30, kSmallTolerance);
}

TEST(RQTest, FloatMatrices) {
  TestRQDecomposition<Matrix3f>(3, 3, kLargeTolerance);
  TestRQDecomposition<Matrix4f>(4, 4, kLargeTolerance);
}

TEST(RQTest, RowMajorMatrices) {
  TestRQDecomposition<Matrix<double, 3, 3, Eigen::RowMajor> >(3, 3,
                                                              kSmallTolerance);
  TestRQDecomposition<Matrix<float, 3, 3, Eigen::RowMajor> >(3, 3,
                                                             kLargeTolerance);
}

}  // namespace GraphSfM


// Copyright Louis Dionne 2015
// Distributed under the Boost Software License, Version 1.0.

#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/multiplies.hpp>
#include <boost/mpl/pair.hpp>
#include <boost/mpl/plus.hpp>

#include <boost/hana/constant.hpp>
#include <boost/hana/core/models.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/pair.hpp>

#include <cassert>
#include <type_traits>
namespace hana = boost::hana;

template <typename T, typename = std::enable_if_t<
  !hana::models<hana::Constant, T>()
>>
constexpr T sqrt(T x) {
  T inf = 0, sup = (x == 1 ? 1 : x/2);
  while (!((sup - inf) <= 1 || ((sup*sup <= x) && ((sup+1)*(sup+1) > x)))) {
    T mid = (inf + sup) / 2;
    bool take_inf = mid*mid > x ? 1 : 0;
    inf = take_inf ? inf : mid;
    sup = take_inf ? mid : sup;
  }

  return sup*sup <= x ? sup : inf;
}

template <typename T, typename = std::enable_if_t<
  hana::models<hana::Constant, T>()
>>
constexpr auto sqrt(T const&) {
  return hana::integral_constant<typename T::value_type, sqrt(T::value)>;
}

namespace then {
using namespace boost::mpl;

template <typename N>
struct sqrt
  : integral_c<typename N::value_type, ::sqrt(N::value)>
{ };

template <typename X, typename Y>
struct point {
  using x = X;
  using y = Y;
};

// sample(arithmetic-then)
template <typename P1, typename P2>
struct distance {
  using xs = typename minus<typename P1::x,
                            typename P2::x>::type;
  using ys = typename minus<typename P1::y,
                            typename P2::y>::type;
  using type = typename sqrt<
    typename plus<
      typename multiplies<xs, xs>::type,
      typename multiplies<ys, ys>::type
    >::type
  >::type;
};

static_assert(equal_to<
  distance<point<int_<3>, int_<5>>, point<int_<7>, int_<2>>>::type,
  int_<5>
>::value, "");
// end-sample
}

namespace now {
using namespace boost::hana;
using namespace boost::hana::literals;

template <typename X, typename Y>
struct _point {
  X x;
  Y y;
};
template <typename X, typename Y>
constexpr _point<X, Y> point(X x, Y y) { return {x, y}; }

// sample(arithmetic-now)
template <typename P1, typename P2>
constexpr auto distance(P1 p1, P2 p2) {
  auto xs = p1.x - p2.x;
  auto ys = p1.y - p2.y;
  return sqrt(xs*xs + ys*ys);
}

static_assert(distance(point(3_c, 5_c), point(7_c, 2_c)) == 5_c, "");
// end-sample

void test() {

// sample(arithmetic-now-dynamic)
auto p1 = point(3, 5); // dynamic values now
auto p2 = point(7, 2); //
assert(distance(p1, p2) == 5); // same function works!
// end-sample

}
}

int main() {
  now::test();
}


//
// Created by lei on 4/21/19.
//

#include "catch.hpp"

#include "topography.hpp"
#include <armadillo>
#include <vector>
#include <fmt/format.h>

arma::vec func1(const arma::vec &x, double c = 0.) {
    return arma::pow((x-c), 2);
}

TEST_CASE("f = x^2", "[topo]") {
    int n = 8;
    arma::vec x = arma::linspace(-1.1, 1, n);
    arma::vec f = func1(x);
    Topography topo(x, f, 3);
    std::vector<double> min_pool = topo.minimize_pool();
    double dx = (x(1) - x(0)) / 2.;
    REQUIRE(min_pool[0] == Approx(0.0).margin(dx));
}

TEST_CASE("f = x^2 * (x-1)^2", "[topo]") {
    int n = 100;
    arma::vec x = arma::linspace(-1.1, 1, n);
    arma::vec f = func1(x) % func1(x, 1.0);
    Topography topo(x, f, 3);
    std::vector<double> min_pool = topo.minimize_pool();
    double dx = (x(1) - x(0)) / 2.;
    REQUIRE(min_pool[0] == Approx(0.0).margin(dx));
    REQUIRE(min_pool[1] == Approx(1.0).margin(dx));
}


// solving A * X = B
// A hermitian
// driver function hesv()

#include <cstddef>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/lapack/hesv.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/ublas_hermitian.hpp>
#include <boost/numeric/bindings/traits/std_vector.hpp>
#include "utils.h"

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

using std::size_t;
using std::cin;
using std::cout;
using std::endl;

typedef double real_t;
typedef std::complex<real_t> cmplx_t;

typedef ublas::matrix<cmplx_t, ublas::column_major> cm_t;
typedef ublas::hermitian_adaptor<cm_t, ublas::lower> cherml_t;
typedef ublas::hermitian_adaptor<cm_t, ublas::upper> chermu_t;

int main() {

  cm_t cal (3, 3), cau (3, 3);   // matrices (storage)
  cherml_t hcal (cal);   // hermitian adaptor
  chermu_t hcau (cau);   // hermitian adaptor
  cm_t cx (3, 1);
  cm_t cbl (3, 1), cbu (3, 1);  // RHS

  hcal (0, 0) = cmplx_t (3, 0);
  hcal (1, 0) = cmplx_t (4, -2);
  hcal (1, 1) = cmplx_t (5, 0);
  hcal (2, 0) = cmplx_t (-7, -5);
  hcal (2, 1) = cmplx_t (0, 3);
  hcal (2, 2) = cmplx_t (2, 0);

  hcau (0, 0) = cmplx_t (3, 0);
  hcau (0, 1) = cmplx_t (4, 2);
  hcau (0, 2) = cmplx_t (-7, 5);
  hcau (1, 1) = cmplx_t (5, 0);
  hcau (1, 2) = cmplx_t (0, -3);
  hcau (2, 2) = cmplx_t (2, 0);

  print_m (cal, "cal");
  cout << endl;
  print_m (cau, "cau");
  cout << endl;

  for (int i = 0; i < cx.size1(); ++i)
    cx (i, 0) = cmplx_t (1, -1);
  print_m (cx, "cx");
  cout << endl;
  cbl = prod (hcal, cx);
  cbu = prod (hcau, cx);
  print_m (cbl, "cbl");
  cout << endl;
  print_m (cbu, "cbu");
  cout << endl;

  cm_t cal2 (hcal), cau2 (hcau);  // for part 2
  cm_t cbl2 (cbl), cbu2 (cbu);

  int ierr = lapack::hesv (hcal, cbl);
  if (ierr == 0)
    print_m (cbl, "cxl");
  else
    cout << "matrix is not regular: ierr = "
         << ierr << endl;
  cout << endl;

  std::vector<integer_t> ipiv (3);
  std::vector<cmplx_t> cwork (3*64);
  // 3*64 -- optimal size

  ierr = lapack::hesv (hcau, ipiv, cbu, cwork);
  if (ierr == 0) {
    print_v (ipiv, "ipiv");
    cout << endl;
    print_m (cbu, "cxu");
  }
  else
    cout << "matrix is not regular: ierr = "
         << ierr << endl;
  cout << endl;

  // part 2

  swap (row (cal2, 0), row (cal2, 2));
  swap (column (cal2, 0), column (cal2, 2));

  swap (row (cau2, 1), row (cau2, 2));
  swap (column (cau2, 1), column (cau2, 2));

  print_m (cal2, "cal2");
  cout << endl;
  print_m (cau2, "cau2");
  cout << endl;

  cbl2 = prod (cal2, cx);
  cbu2 = prod (cau2, cx);
  print_m (cbl2, "cbl2");
  cout << endl;
  print_m (cbu2, "cbu2");
  cout << endl;

  ierr = lapack::hesv ('L', cal2, cbl2);
  if (ierr == 0)
    print_m (cbl2, "cxl2");
  else
    cout << "matrix is not regular: ierr = "
         << ierr << endl;
  cout << endl;

  ierr = lapack::hesv ('U', cau2, ipiv, cbu2, cwork);
  if (ierr == 0) {
    print_v (ipiv, "ipiv");
    cout << endl;
    print_m (cbu2, "cxu");
  }
  else
    cout << "matrix is not regular: ierr = "
         << ierr << endl;
  cout << endl;

}


#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>

using namespace std;
using boost::multiprecision::cpp_int;

// Returns n!
cpp_int factorial(cpp_int n) {
	if(n == 0) {
		return 1;
	}
	return n * factorial(n - 1);
}

// Returns the sum of the factorials of the digits of a number.
cpp_int sumDigitsFactorial(cpp_int n) {
	cpp_int sum = 0;
	while(n) {
		sum += factorial(n % 10);
		n /= 10;
	}
	return sum;
}

int main(int argc, char *argv[]) {
	cpp_int sum = 0;
	// Randomly decided to put limit as 100,000 for now.
	for(cpp_int i = 3; i <= 100'000; i++) {
		sum = (i == sumDigitsFactorial(i)) ? sum + i : sum;
	}
	cout << sum << endl;
	return 0;
}


#pragma once
#include <Eigen/Core>
#include <igl/boundary_facets.h>
#include <igl/colon.h>
#include <igl/setdiff.h>
#include <igl/unique.h>

//! Finds the boundary and interior vertices, and evaluates
//! the function g on the boundary vertices.
//!
//! @param[out] u should be of size <number of vertices>. At the end:
//!             u(index) = g(vertices(index))  for each boundary index.
//!
//! @param[out] interiorVertexIndices the list of vertices that are not on
//!                                   the boundary.
//!
//! @param[in] vertices a list of vertices for the mesh
//!
//! @param[in] triangles the elements represented as triangles
//!
//! @param[in] g the boundary value function g.
void setDirichletBoundary(Eigen::VectorXd &      u,
                          Eigen::VectorXi &      interiorVertexIndices,
                          const Eigen::MatrixXd &vertices,
                          const Eigen::MatrixXi &triangles,
                          const std::function<double(double, double)> &g) {
	// Find boundary edges
	Eigen::MatrixXi boundaryEdgeIndices;
	igl::boundary_facets(triangles, boundaryEdgeIndices);

	// Find boundary vertices
	Eigen::VectorXi boundaryVertexIndices, IA, IC;
	igl::unique(boundaryEdgeIndices, boundaryVertexIndices, IA, IC);

	for (int i = 0; i < boundaryVertexIndices.size(); ++i) {
		const int   index = boundaryVertexIndices(i);
		const auto &x     = vertices.row(index);
		u(index)          = g(x(0), x(1));
	}

	Eigen::VectorXi allIndices;
	igl::colon<int>(0, vertices.rows() - 1, allIndices);
	igl::setdiff(allIndices, boundaryVertexIndices, interiorVertexIndices, IA);
}


//
// Created by philipp on 22.12.19.
//

#ifndef FUNNELS_CPP_LYAPUNOV_HH
#define FUNNELS_CPP_LYAPUNOV_HH

#include <iostream>

#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <cmath>

namespace lyapunov{

  template <class DERIVED>
  inline double projected_max_radius(const Eigen::MatrixBase<DERIVED> & C0,
                                     const Eigen::MatrixBase<DERIVED> & C1){
    // todo optimize for usage of triangular shape
    // use spezialised version to compute smallest eigenvalue only
    auto Cinv = C0.inverse();
    auto P1_prime = (Cinv.transpose()*(C1.transpose()*C1)*Cinv);
    double max_rad = 1./P1_prime.eigenvalues().real().array().minCoeff();
    return std::sqrt(max_rad);
  }

  struct lyap_zone_t{};

  class lyapunov_t{
  public:
    virtual bool intersect(const lyap_zone_t & other);
    virtual bool covers(const lyap_zone_t & other);
    virtual bool is_covered(const lyap_zone_t & other);
  };

  struct fixed_ellipsoidal_zone_t{
    virtual const Eigen::VectorXd & x0() const {
      throw std::runtime_error("Virtual");
      return Eigen::Vector2d::Zero();
    };
    virtual const Eigen::MatrixXd & C() const {
      throw std::runtime_error("Virtual");
      return Eigen::Matrix2d::Zero();
    };
  };

  struct fixed_ellipsoidal_zone_copied_t: public fixed_ellipsoidal_zone_t{
  public:
    fixed_ellipsoidal_zone_copied_t( const Eigen::VectorXd & x0,
        const Eigen::MatrixXd & C);
    const Eigen::VectorXd & x0() const {
      return _x0;
    }
    const Eigen::MatrixXd & C() const {
      return _C;
    }
    // (x-x0)^T.P.(x-x0) <= 1
    // ||C.(x-x0)||_2^2 <= 1
    Eigen::VectorXd _x0;
    Eigen::MatrixXd _C;
  };

  struct fixed_ellipsoidal_zone_ref_t: public fixed_ellipsoidal_zone_t{
  public:
    fixed_ellipsoidal_zone_ref_t( const Eigen::VectorXd & x0,
                                     const Eigen::MatrixXd & C);
    const Eigen::VectorXd & x0() const {
      return _x0;
    }
    const Eigen::MatrixXd & C() const {
      return _C;
    }
    // (x-x0)^T.P.(x-x0) <= 1
    // ||C.(x-x0)||_2^2 <= 1
    const Eigen::VectorXd &_x0;
    const Eigen::MatrixXd &_C;
  };

  // Does zone0 cover zone1
  template<class DIST>
  bool covers_helper(const fixed_ellipsoidal_zone_t &zone0,
                     const fixed_ellipsoidal_zone_t &zone1,
                     DIST &dist){

    // First compute the projected distance
    auto dy = zone0.C()*(dist.cp_vv(zone1.x0(), zone0.x0()));
    double dy_norm = dy.norm(); //l2 norm
    // center out of bounds
    if (dy_norm>=1.){
      return false;
    }

    // Compute radius
    return dy_norm+projected_max_radius(zone0.C(), zone1.C())<=1.;
  }

  template<class DIST>
  class fixed_ellipsoidal_lyap_t:public lyapunov_t{
  public:
    using dist_t = DIST;

    fixed_ellipsoidal_lyap_t(const Eigen::MatrixXd &P,
        double gamma, DIST &dist):
        _gamma(gamma), _dist(dist){
      // Compute cholesky
      Eigen::LLT<Eigen::MatrixXd> llt_pre_comp;
      llt_pre_comp.compute(P);
      _C = llt_pre_comp.matrixU();
    }

    const Eigen::MatrixXd &C() const {
      return _C;
    }
    double gamma() const {
      return _gamma;
    }

    // Conservative intersect
    // If true, the zone may intersect with this
    // If false, the zone does definitively not intersect
    template<class DERIVED>
    bool intersect(const Eigen::MatrixBase<DERIVED> &x0,
        const fixed_ellipsoidal_zone_t &zone) {
      // First compute the projected distance
      auto dy = _C*(_dist.cp_vv(zone.x0(), x0));
      double dy_norm = dy.norm(); //l2 norm

      if (dy_norm <= 1.){
        return true;
      }

      return dy_norm>=1.+projected_max_radius(_C, zone.C());
    }

    // Conservative cover
    // If true, zone is definitively covered by this
    // If false, it may not be covered
    template<class DERIVED>
    bool covers(const Eigen::MatrixBase<DERIVED> &x0, const fixed_ellipsoidal_zone_t &zone) {
      fixed_ellipsoidal_zone_ref_t this_zone(x0, _C);
      return covers_helper(this_zone, zone, _dist);
    }

    // Conservative cover
    // If true, this is definitively covered by zone
    // If false, it may not be covered
    template<class DERIVED>
    bool is_covered(const Eigen::MatrixBase<DERIVED> &x0, const fixed_ellipsoidal_zone_t &zone) {
      fixed_ellipsoidal_zone_ref_t this_zone(x0, _C);
      return covers_helper(zone, this_zone, _dist);
    }

    dist_t & dist(){
      return _dist;
    }

  protected:
    Eigen::MatrixXd _C;
    double _gamma;
    DIST &_dist;
  };

}

#endif //FUNNELS_CPP_LYAPUNOV_HH


#include <gtest/gtest.h>
#include <boost/lexical_cast.hpp>
#include "quadrature/qhermite.hpp"
#include "spectral/lagrange_polynomial.hpp"

using namespace std;
using namespace boltzmann;

TEST(spectral, LagrangePolySimple)
{
  std::vector<int> ks({10, 20, 30, 40, 80});
  for (int K : ks) {
    QHermite qh(1.0, K);

    lagrange_poly_simple lp(qh.points_data(), qh.size());

    for (int j = 0; j < qh.size(); ++j) {
      for (int i = 0; i < qh.size(); ++i) {
        double val = lp.eval(j, qh.pts(i));
        if (i == j)
          EXPECT_NEAR(val, 1, 1e-12);
        else
          EXPECT_NEAR(val, 0, 1e-12);
      }
    }
  }
}


//StandardDeviation_StandardError.hpp
//purpose: header file for standard deriation
//author: bondxue
//version: 1.0 11/22/2017

#ifndef STANDARDDEVIATION_STANDARDERROR_HPP
#define STANDARDDEVIATION_STANDARDERROR_HPP

#include <iostream>
#include <cmath>
#include <vector>

#include <boost/tuple/tuple.hpp> // include tuple class
#include <boost/tuple/tuple_io.hpp> // include I/O for tuple

using namespace std;
using boost::tuple;

// function to obtain standard deviation and standard error
boost::tuple <double, double> get_SD_SE(vector<double> price_vec, double r, double T)
{
	int price_size = price_vec.size();
	double sum_price = 0.0;
	double square_sum_price = 0.0;

	for (int i = 0; i < price_size; i++)
	{
		sum_price += price_vec[i];
		square_sum_price += pow(price_vec[i], 2);
	}

	double temp = sqrt((square_sum_price - pow(sum_price, 2)/double(price_size))/(double(price_size)-1.0)); // use temp to store the square root part in SD formula
	double SD = temp * exp(-r*T);
	double SE = SD / sqrt(double(price_size));

	return boost::tuple<double, double>(SD, SE);
}

#endif

Part D Monte Carlo Pricing Methods/Exericse D Monte Carlo Pricing Methods/StandardDeviation_StandardError.hpp


#include <Engine/MeshEdit/DeformRBF.h>

#include <Engine/Primitive/TriMesh.h>

#include <Eigen/Dense>

using namespace Ubpa;

using namespace std;
using namespace Eigen;

void DeformRBF::Clear() {
	triMesh = nullptr;
	origPos.clear();
}

bool DeformRBF::Init(Ptr<TriMesh> triMesh) {
	if (triMesh == this->triMesh)
		return true;

	if (triMesh->GetType() == TriMesh::INVALID) {
		printf("ERROR::DeformRBF::Init\n"
			"\t""triMesh->GetType() == TriMesh::INVALID\n");
		return false;
	}

	this->triMesh = triMesh;
	origPos = triMesh->GetPositions();
	return true;
}

bool DeformRBF::Run(const std::vector<Constraint> & constraints, const vector<size_t> & updateIndice) {
	if (!triMesh) {
		printf("ERROR::DeformRBF::Run\n"
			"\t""!triMesh\n");
		return false;
	}

	for (auto constraint : constraints) {
		if (constraint.id >= origPos.size()) {
			printf("ERROR::DeformRBF::Run\n"
				"\t""constraint.id >= origPos.size()\n");
			return false;
		}
	}

	// compute RBF matrix
	auto phi = [](float r) -> float {return r*r*r; }; // basis function
	size_t m = constraints.size();
	size_t k = 10;

	// A = -       -
	//     | G   P |
	//     |       |
	//     |  t    |
	//     | P   0 |
	//     -       -
	MatrixXf A(m + k, m + k);

	// G : m x m
	// Gij = gj(ci) = basicFunc(||ci-cj||)
	for (size_t i = 0; i < m; i++) {
		for (size_t j = 0; j < m; j++) {
			auto ci = origPos[constraints[i].id];
			auto cj = origPos[constraints[j].id];
			float norm = pointf3::distance(ci, cj);
			A(i, j) = phi(norm);
		}
	}

	// P : m x k
	// Pij = pj(ci)
	// [p0, p1, ..., p9] is a basis of the space of trivariate quadratic polunomials
	// e[1]. [1, xx, xy, xz, yx, yy, yz, zx, zy, zz]
	for (size_t i = 0; i < m; i++) {
		auto ci = origPos[constraints[i].id];
		A(m + 0, i) = A(i, m + 0) = 1.f;
		A(m + 1, i) = A(i, m + 1) = ci[0] * ci[0];
		A(m + 2, i) = A(i, m + 2) = ci[0] * ci[1];
		A(m + 3, i) = A(i, m + 3) = ci[0] * ci[2];
		A(m + 4, i) = A(i, m + 4) = ci[1] * ci[0];
		A(m + 5, i) = A(i, m + 5) = ci[1] * ci[1];
		A(m + 6, i) = A(i, m + 6) = ci[1] * ci[2];
		A(m + 7, i) = A(i, m + 7) = ci[2] * ci[0];
		A(m + 8, i) = A(i, m + 8) = ci[2] * ci[1];
		A(m + 9, i) = A(i, m + 9) = ci[2] * ci[2];
	}

	// 0 : k x k
	for (size_t i = 0; i < k; i++) {
		for (size_t j = 0; j < k; j++) {
			A(m + i, m + j) = 0.f;
		}
	}

	MatrixXf b(m + k, 3);
	for (int i = 0; i < k; i++)
		b(m + i,0) = b(m + i, 1) = b(m + i, 2) = 0;
	for (int i = 0; i < m; i++) {
		auto bi = constraints[i].pos;
		b(i, 0) = bi[0];
		b(i, 1) = bi[1];
		b(i, 2) = bi[2];
	}

	MatrixXf RBF = A.colPivHouseholderQr().solve(b);

	// compute target pos of vertex
	vector<pointf3> qVec;
	for (auto id : updateIndice) {
		auto pos = origPos[id];
		// compute p
		MatrixXf p(1, m + k);
		for (size_t i = 0; i < m; i++) {
			auto ci = origPos[constraints[i].id];
			p(i) = phi(pointf3::distance(ci, pos));
		}
		p(0, m + 0) = 1;
		p(0, m + 1) = pos[0] * pos[0];
		p(0, m + 2) = pos[0] * pos[1];
		p(0, m + 3) = pos[0] * pos[2];
		p(0, m + 4) = pos[1] * pos[0];
		p(0, m + 5) = pos[1] * pos[1];
		p(0, m + 6) = pos[1] * pos[2];
		p(0, m + 7) = pos[2] * pos[0];
		p(0, m + 8) = pos[2] * pos[1];
		p(0, m + 9) = pos[2] * pos[2];

		// compute q
		MatrixXf q = p * RBF;
		triMesh->GetPositions()[id] = { q(0,0),q(0,1),q(0,2) };
	}

	return true;
}

void DeformRBF::Restore() {
	if (triMesh == nullptr)
		return;

	triMesh->Update(origPos);
}


#include <blitzml/smooth_loss/logistic_loss.h>
#include <blitzml/base/math_util.h>
#include <math.h>

namespace BlitzML {

value_t LogisticLoss::compute_loss(value_t a_dot_omega, value_t label) const {
  return log1p(exp(-label * a_dot_omega));
}

value_t LogisticLoss::compute_conjugate(value_t dual_variable,
                                        value_t label) const {
  if (label == 0. || dual_variable == 0.) return 0.;
  value_t neg_ratio = -dual_variable / label;
  return neg_ratio * log(neg_ratio) + (1 - neg_ratio) * log(1 - neg_ratio);
}

value_t LogisticLoss::compute_deriative(value_t a_dot_omega,
                                        value_t label) const {
  value_t exp_value = exp(label * a_dot_omega);
  value_t prob = 1 / (1 + exp_value);
  return -label * prob;
}

value_t LogisticLoss::compute_2nd_derivative(value_t a_dot_omega,
                                             value_t label) const {
  value_t exp_value = exp(label * a_dot_omega);
  value_t prob = 1 / (1 + exp_value);
  return label * label * prob * (1 - prob);
}

value_t LogisticLoss::lipschitz_constant() const {
  return 0.25;
}

} // namespace BlitzML


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/AlgorithmUtils.hpp"
#include "../../data/FluidIndex.hpp"
#include <Eigen/Core>
#include <cassert>
#include <cmath>
#include <map>

namespace fluid {
namespace algorithm {

class WindowFuncs
{
public:
  enum class WindowTypes {
    kHann,
    kHannD,
    kHamming,
    kBlackmanHarris,
    kGaussian
  };
  using WindowFuncsMap =
      std::map<WindowTypes,
               std::function<void(index, Eigen::Ref<Eigen::ArrayXd>)>>;

  static WindowFuncsMap& map()
  {
    using namespace std;
    static WindowFuncsMap _funcs = {
        {WindowTypes::kHann,
         [](index size, Eigen::Ref<Eigen::ArrayXd> out) {
           for (index i = 0; i < size; i++)
           { out(i) = 0.5 - 0.5 * cos((pi * 2 * i) / size); }
         }},
        {WindowTypes::kHannD,
         [](index size, Eigen::Ref<Eigen::ArrayXd> out) {
           double norm = pi / size;
           for (index i = 0; i < size; i++)
           { out(i) = norm * sin((2 * pi * i) / size); }
         }},
        {WindowTypes::kHamming,
         [](index size, Eigen::Ref<Eigen::ArrayXd> out) {
           for (index i = 0; i < size; i++)
           { out(i) = 0.54 - 0.46 * cos((pi * 2 * i) / size); }
         }},
        {WindowTypes::kBlackmanHarris,
         [](index size, Eigen::Ref<Eigen::ArrayXd> out) {
           for (index i = 0; i < size; i++)
           {
             out(i) = 0.35875 - 0.48829 * cos((pi * 2 * i) / size) +
                      0.14128 * cos((pi * 2 * i) / size) +
                      0.01168 * cos((pi * 2 * i) / size);
           }
         }},
        {WindowTypes::kGaussian,
         [](index size, Eigen::Ref<Eigen::ArrayXd> out) {
           double sigma = size / 3; // TODO: should be argument
           assert(size % 2);
           index h = (size - 1) / 2;
           for (index i = -h; i <= h; i++)
           { out(i + h) = exp(-i * i / (2 * sigma * sigma)); }
         }}};
    return _funcs;
  }
};
} // namespace algorithm
} // namespace fluid