/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/Numerik/numsoft/kaskade7/                        */
/*                                                                           */
/*  Copyright (C) 2002-2009 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef STRAINTENSORS_HH
#define STRAINTENSORS_HH

#include <utility>
#include <boost/math/constants/constants.hpp>
#include <dune/common/fmatrix.hh>

#include "fem/fixdune.hh"
#include "fem/diffops/elasto.hh"
#include "linalg/wrappedMatrix.hh"

namespace Kaskade
{
  /// \todo docme
  template <class Scalar, int dim, bool byValue = true>
  class CauchyGreenTensor
  {
  public:
    typedef Dune::FieldMatrix<Scalar,dim,dim> Argument;
    typedef Argument ReturnType;

    explicit CauchyGreenTensor(Argument const& F_) : F(F_) {}

    CauchyGreenTensor(CauchyGreenTensor const&) = default;
    CauchyGreenTensor& operator=(CauchyGreenTensor const&) = default;

    ReturnType d0() const { return transpose(F)*F; }

    ReturnType d1(Argument const& dF1) const
    {
      Argument const& G = transpose(dF1)*F;
      return G + transpose(G);
    }

    ReturnType d2(Argument const& dF1, Argument const& dF2) const
    {
      Argument const& G = transpose(dF1)*dF2;
      return G + transpose(G);
    }

    ReturnType d3(Argument const&, Argument const&, Argument const&) const { return zero; }

  private:
    typename std::conditional<byValue,Argument,Argument const&>::type F;
    Argument zero = Argument(0);
  };

  template <class Scalar,bool byValue>
  class CauchyGreenTensor<Scalar,3,byValue>
  {
    static constexpr int dim = 3;
  public:
    typedef Dune::FieldMatrix<Scalar,dim,dim> Argument;
    typedef Argument ReturnType;

    explicit CauchyGreenTensor(Argument const& F_) : F(F_) {}

    CauchyGreenTensor(CauchyGreenTensor const&) = default;
    CauchyGreenTensor& operator=(CauchyGreenTensor const&) = default;

    __attribute__((always_inline)) ReturnType d0() const
    {
      tmp = 0;
      for(size_t i=0; i<dim; ++i)
        for(size_t j=0; j<dim; ++j)
          for(size_t k=0; k<dim; ++k)
            tmp[i][j] += F[k][i]*F[k][j];

      return tmp;// transpose(F)*F;
    }

    __attribute__((always_inline)) ReturnType d1(Argument const& dF1) const
    {
      tmp = 0;
      for(size_t i=0; i<dim; ++i)
        for(size_t j=0; j<dim; ++j)
          for(size_t k=0; k<dim; ++k)
            tmp[i][j] += F[k][i]*dF1[k][j] + dF1[k][i]*F[k][j];

      return tmp;// G + transpose(G);
    }

    __attribute__((always_inline)) ReturnType d2(Argument const& dF1, Argument const& dF2) const
    {
      tmp = 0;
      for(size_t i=0; i<dim; ++i)
        for(size_t j=0; j<dim; ++j)
          for(size_t k=0; k<dim; ++k)
            tmp[i][j] += dF2[k][i]*dF1[k][j] + dF1[k][i]*dF2[k][j];
      return tmp;
    }

    __attribute__((always_inline)) ReturnType d3(Argument const&, Argument const&, Argument const&) const { return zero; }

  private:
    typename std::conditional<byValue,Argument,Argument const&>::type F;
    Argument zero = Argument(0);
    mutable Argument tmp;
  };

//   /// \todo docme
//   template <class Scalar, int dim>
//   class ElasticLinearizedGreenLagrangeTensor
//   {
//   public:
//     ElasticLinearizedGreenLagrangeTensor() = default;
//
//     ElasticLinearizedGreenLagrangeTensor(Dune::FieldMatrix<Scalar,dim,dim> const& du, Dune::FieldMatrix<Scalar,dim,dim> const& inelasticStrain_)
//       : elasticStrain(du), inelasticStrain(inelasticStrain_)
//     {}
//
//     auto d0() const { return elasticStrain.d0() - inelasticStrain; }
//
//     template <class Arg>
//     auto d1(Arg const& arg) const { return elasticStrain.d1(arg); }
//
//     template <class Arg>
//     auto d2(Arg const& arg1, Arg const& arg2) const { return elasticStrain.d2(arg1,arg2); }
//
//     template <class Arg>
//     auto d3(Arg const& arg1, Arg const& arg2, Arg const& arg3) const { return elasticStrain.d3(arg1,arg2,arg3); }
//
//   private:
//     LinearizedGreenLagrangeTensor<Scalar,dim> elasticStrain;
//     Dune::FieldMatrix<Scalar,dim,dim> inelasticStrain;
//   };
//

  /// \todo docme
  template <class Scalar, int dim, class Tensor = WrappedMatrix<Scalar,dim> >
  class Deviator
  {
  public:
    typedef typename Tensor::Argument Argument;
    typedef Dune::FieldMatrix<Scalar,dim,dim> ReturnType;

    explicit Deviator(Tensor const& s_) : s(s_), I(unitMatrix<Scalar,dim>()) {}

    ReturnType d0() const
    {
      return s.d0() - boost::math::constants::third<Scalar>() * trace(s.d0()) * I;
    }

    ReturnType d1(Argument const& dF) const
    {
      return s.d1(dF) - boost::math::constants::third<Scalar>() * trace(s.d1(dF)) * I;
    }

    ReturnType d2(Tensor const& dF1, Tensor const& dF2) const
    {
      return s.d2(dF1,dF2) - boost::math::constants::third<Scalar>() * trace(s.d2(dF1,dF2)) * I;
    }

    ReturnType d3(Tensor const& dF1, Tensor const& dF2, Tensor const& dF3) const
    {
      return s.d3(dF1,dF2,dF3) - boost::math::constants::third<Scalar>() * trace(s.d3s(dF1,dF2,dF3)) * I;
    }

  private:
    Tensor s;
    ReturnType zero = ReturnType(0), I;
  };
}

#endif


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  typedef Matrix<float,1,Dynamic> MatrixType;
typedef Map<MatrixType> MapType;
typedef Map<const MatrixType> MapTypeConst;   // a read-only map
const int n_dims = 5;

MatrixType m1(n_dims), m2(n_dims);
m1.setRandom();
m2.setRandom();
float *p = &m2(0);  // get the address storing the data for m2
MapType m2map(p,m2.size());   // m2map shares data with m2
MapTypeConst m2mapconst(p,m2.size());  // a read-only accessor for m2

cout << "m1: " << m1 << endl;
cout << "m2: " << m2 << endl;
cout << "Squared euclidean distance: " << (m1-m2).squaredNorm() << endl;
cout << "Squared euclidean distance, using map: " <<
  (m1-m2map).squaredNorm() << endl;
m2map(3) = 7;   // this will change m2, since they share the same array
cout << "Updated m2: " << m2 << endl;
cout << "m2 coefficient 2, constant accessor: " << m2mapconst(2) << endl;
/* m2mapconst(2) = 5; */   // this yields a compile-time error

  return 0;
}


/*
 * File:   Activation.hpp
 * Author: heshan
 *
 * Created on June 7, 2018, 11:17 AM
 */

#ifndef ACTIVATION_HPP
#define ACTIVATION_HPP

#include <cmath>
#include <Eigen>

class Activation {
public:
    Activation();
    Activation(const Activation& orig);
    virtual ~Activation();

    static double sigmoid(double);
    static Eigen::MatrixXd sigmoid(Eigen::MatrixXd);
    static double sigmoidDeriv(double);
    static Eigen::MatrixXd sigmoidDeriv(Eigen::MatrixXd);
    static Eigen::MatrixXd maxPoolDelta(double, double, Eigen::MatrixXd, int ,int);

private:

};

#endif /* ACTIVATION_HPP */


/**
 * @ file avgvalboundary.cc
 * @ brief NPDE homework AvgValBoundary code
 * @ author Simon Meierhans, edited by Oliver Rietmann
 * @ date 11.03.2019
 * @ copyright Developed at ETH Zurich
 */

#include "avgvalboundary.h"

#include <lf/assemble/assemble.h>
#include <lf/mesh/test_utils/test_meshes.h>
#include <lf/mesh/utils/utils.h>
#include <lf/refinement/refinement.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <Eigen/SparseLU>
#include <cmath>
#include <memory>
#include <utility>
#include <vector>

namespace AvgValBoundary {

/**
 * @brief computes H1 seminorm over the computational domain
 * @param dofh DofHandler of FEspace.
 *        u coefficient vector
 */
/* SAM_LISTING_BEGIN_1 */
double compH1seminorm(const lf::assemble::DofHandler &dofh,
                      const Eigen::VectorXd &u) {
  double result = 0.0;
  //====================

  auto alpha = [] (Eigen::Vector2d x) { return 1.0; };
  auto beta = [] (Eigen::Vector2d x) { return 0.0; };
  auto gamma = [] (Eigen::Vector2d x) { return 0.0; };

  Eigen::SparseMatrix<double> A = compGalerkinMatrix(dofh, alpha, gamma, beta);

  result = std::sqrt( u.transpose()*A*u );

  //====================
  return result;
}
/* SAM_LISTING_END_1 */

/**
 * @brief solves pde for some simple test problem with
 *        alpha = beta = gamma := 1.0 and load f := 1.0
 * @param dofh DofHandler of FEspace.
 */
/* SAM_LISTING_BEGIN_2 */
Eigen::VectorXd solveTestProblem(const lf::assemble::DofHandler &dofh) {
  // Obtain Galerkin matrix for alpha = beta = gamma := 1.0
  auto const_one = [](Eigen::Vector2d x) -> double { return 1.0; };
  auto A = compGalerkinMatrix(dofh, const_one, const_one, const_one);

  // Set up load vector
  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(dofh.Mesh());
  lf::mesh::utils::MeshFunctionConstant mf_identity{1.0};
  lf::uscalfe::ScalarLoadElementVectorProvider elvec_builder(fe_space,
                                                             mf_identity);
  Eigen::VectorXd phi(dofh.NumDofs());
  phi.setZero();
  AssembleVectorLocally(0, dofh, elvec_builder, phi);

  // Solve system of linear equations
  Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
  solver.compute(A);
  Eigen::VectorXd mu = solver.solve(phi);

  return mu;
}
/* SAM_LISTING_END_2 */

/** @brief generate sequence of nested triangular meshes with L+1 levels */
/* SAM_LISTING_BEGIN_3 */
std::shared_ptr<lf::refinement::MeshHierarchy> generateTestMeshSequence(
    unsigned int L) {
  auto mesh = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3, 1.0 / 3.0);
  std::shared_ptr<lf::refinement::MeshHierarchy> meshes =
      lf::refinement::GenerateMeshHierarchyByUniformRefinemnt(mesh, L);
  return meshes;
}
/* SAM_LISTING_END_3 */

/**
 * @brief Compute the boundary functional values for a sequence of L meshes
 * @param L The number of meshes in the sequence
 * @returns A vector of pairs containing the number of DOFs and value of the
 *	    boundary functional for each level
 */
/* SAM_LISTING_BEGIN_5 */
std::vector<std::pair<unsigned int, double>> approxBoundaryFunctionalValues(
    unsigned int L) {
  std::vector<std::pair<unsigned int, double>> result;
  //====================
  // Your code goes here
  //====================
  return result;
}
/* SAM_LISTING_END_5 */

}  // namespace AvgValBoundary


// Copyright (c) 2017 Evan S Weinberg
// Test code for a real operator.

#include <iostream>
#include <iomanip>
#include <cmath>
#include <string>
#include <sstream>
#include <complex>
#include <random>

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

#include "blas/generic_vector.h"
#include "inverters/generic_cg.h"
#include "inverters/generic_ca_cg.h"
#include "inverters/generic_cheby_ca_cg.h"

using namespace std;

// Define a function that just has varying values along the diagonal.
// Makes it easy to control the condition number.
struct scale_info {
  int size;
  double* scale_vec;
  double lambda_min; // minimum eigenvalue
  double lambda_max; // maximum eigenvalues
  bool log_distribute; // randomly distribute evenly or logarithmically?

  scale_info(int size, double lambda_min, double lambda_max, bool log_distribute, std::mt19937& generator)
   : size(size), lambda_min(lambda_min), lambda_max(lambda_max), log_distribute(log_distribute) {

    scale_vec = allocate_vector<double>(size);

    // First: gurantee lambda_min and lambda_max exist.
    scale_vec[0] = lambda_min;
    scale_vec[1] = lambda_max;

    if (log_distribute) {
      double log_lambda_min = log(lambda_min);
      double log_lambda_max = log(lambda_max);
      random_uniform(scale_vec+2, size-2, generator, log_lambda_min, log_lambda_max);
      exp_vector(scale_vec+2, size-2);
    } else {
      random_uniform(scale_vec+2, size-2, generator, lambda_min, lambda_max);
    }
  }

  scale_info(int size, std::mt19937& generator)
   : scale_info(size, 1., 10., true, generator) { ; }

  ~scale_info() {
    deallocate_vector(&scale_vec);
  }

};

// Define a function that just has varying values along the diagonal.
void scale_function(complex<double>* rhs, complex<double>* lhs, void* extra_data) {
  scale_info& info = *((scale_info*)(extra_data));
  cxtyz(info.scale_vec, lhs, rhs, info.size);
}

// Prepare vectors for various tests.
void reset_vectors(complex<double>* rhs, complex<double>* lhs, complex<double>* check, complex<double>* rhs_backup, int size)
{
  zero_vector(lhs, size);
  copy_vector(rhs, rhs_backup, size);
  zero_vector(check, size);
}

int main(int argc, char** argv)
{
    //double *lattice; // Holds the gauge field.
  complex<double> *lhs, *rhs, *rhs_backup, *check; // For some Kinetic terms.
  double explicit_resid = 0.0;
  double bnorm = 0.0;
  inversion_info invif;
  std::mt19937 generator (1337u); // RNG, 1337u is the seed.

  // Basic information about the linop
  int size = 8192;
  double lambda_min = 1e-6;
  double lambda_max = 10;

  // Parameters related to solve.
  double tol = 1e-8;
  int max_iter = 1000;

  // Vectors.
  lhs = allocate_vector<complex<double>>(size);
  rhs = allocate_vector<complex<double>>(size);
  rhs_backup = allocate_vector<complex<double>>(size);
  check = allocate_vector<complex<double>>(size);

  // Set up a default rhs.
  random_uniform(rhs_backup, size, generator, 0.5, 1.5);

  // Zero out vectors, set rhs to standard backed up version
  reset_vectors(rhs, lhs, check, rhs_backup, size);

  // Set up operator.
  scale_info scinf(size, lambda_min, lambda_max, false, generator);

  // Get norm for rhs.
  bnorm = sqrt(norm2sq(rhs, size));

  printf("Solving A [lhs] = [rhs] for lhs, using a random source. Operator has eigenvalues distributed between %e and %e \n\n", lambda_min, lambda_max);

  /****************
  * NORMAL SOLVES *
  ****************/

  // lhs = A^(-1) rhs
  // Arguments:
  // 1: lhs
  // 2: rhs
  // 3: size of vector
  // 4: maximum iterations
  // 5: residual
  // 5a for gcr_restart: how often to restart.
  // 5b for richardson, minres: overrelaxation parameter (can leave this out, assumes 1)
  // 5c for richardson: how often to check the residual
  // 6: function pointer
  // 7: "extra data":
  // 8: optional, verbosity information.

  inversion_verbose_struct verb(VERB_DETAIL, "Details: ");

  /* CG */
  reset_vectors(rhs, lhs, check, rhs_backup, size);
  invif = minv_vector_cg(lhs, rhs, size, max_iter, tol, scale_function, &scinf, &verb);
  if (invif.success == true)
  {
    printf("Algorithm %s took %d iterations to reach a tolerance of %.8e.\n", invif.name.c_str(), invif.iter, sqrt(invif.resSq)/bnorm);
  }
  else // failed, maybe.
  {
    printf("Potential error! Algorithm %s took %d iterations to reach a tolerance of %.8e.\n", invif.name.c_str(), invif.iter, sqrt(invif.resSq)/bnorm);
  }
  printf("Computing [check] = A [lhs] as a confirmation.\n");
  // Check and make sure we get the right answer.
  scale_function(check, lhs, &scinf);
  explicit_resid = sqrt(diffnorm2sq(rhs, check, size))/bnorm; // sqrt(|rhs - check|^2)/bnorm
  printf("[check] should equal [rhs]. The residual is %15.20e.\n\n", explicit_resid);

  /* CA-CG */
  for (int ca_s = 2; ca_s <= 30; ca_s++)
  {
    reset_vectors(rhs, lhs, check, rhs_backup, size);
    invif = minv_vector_ca_cg(lhs, rhs, size, max_iter, tol, ca_s, scale_function, &scinf, &verb);
    if (invif.success == true)
    {
      printf("Algorithm %s took %d iterations to reach a tolerance of %.8e.\n", invif.name.c_str(), invif.iter, sqrt(invif.resSq)/bnorm);
    }
    else // failed, maybe.
    {
      printf("Potential error! Algorithm %s took %d iterations to reach a tolerance of %.8e.\n", invif.name.c_str(), invif.iter, sqrt(invif.resSq)/bnorm);
    }
    printf("Computing [check] = A [lhs] as a confirmation.\n");
    // Check and make sure we get the right answer.
    scale_function(check, lhs, &scinf);
    explicit_resid = sqrt(diffnorm2sq(rhs, check, size))/bnorm; // sqrt(|rhs - check|^2)/bnorm
    printf("[check] should equal [rhs]. The residual is %15.20e.\n\n", explicit_resid);

    reset_vectors(rhs, lhs, check, rhs_backup, size);
    // invif = minv_vector_cheby_ca_cg(lhs, rhs, size, max_iter, tol, 1.01*lambda_max, ca_s, scale_function, &scinf, &verb); // assume lambda_min = 0
    invif = minv_vector_cheby_ca_cg(lhs, rhs, size, max_iter, tol, lambda_min, lambda_max, ca_s, scale_function, &scinf, &verb);
    if (invif.success == true)
    {
      printf("Algorithm %s took %d iterations to reach a tolerance of %.8e.\n", invif.name.c_str(), invif.iter, sqrt(invif.resSq)/bnorm);
    }
    else // failed, maybe.
    {
      printf("Potential error! Algorithm %s took %d iterations to reach a tolerance of %.8e.\n", invif.name.c_str(), invif.iter, sqrt(invif.resSq)/bnorm);
    }
    printf("Computing [check] = A [lhs] as a confirmation.\n");
    // Check and make sure we get the right answer.
    scale_function(check, lhs, &scinf);
    explicit_resid = sqrt(diffnorm2sq(rhs, check, size))/bnorm; // sqrt(|rhs - check|^2)/bnorm
    printf("[check] should equal [rhs]. The residual is %15.20e.\n\n", explicit_resid);
  }

  //////////////
  // CLEAN UP //
  //////////////

  // Free the lattice.
  //delete[] lattice;
  deallocate_vector(&lhs);
  deallocate_vector(&rhs);
  deallocate_vector(&rhs_backup);
  deallocate_vector(&check);
  return 0;
}


/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <cmath>
using std::log;
using std::sqrt;
using std::pow;

#include <fstream>
using std::ofstream;

#include <streambuf>
using std::streambuf;

#include <utility>
using std::pair;

#include <armadillo>
using arma::Mat;
using arma::Col;
using arma::uword;
using arma::abs;
using arma::eps;
using arma::exp;
using arma::exp2;
using arma::exp10;
using arma::trunc_exp;
using arma::log;
using arma::log2;
using arma::log10;
using arma::trunc_log;
using arma::sqrt;
using arma::square;
using arma::floor;
using arma::ceil;
using arma::round;
using arma::sign;
using arma::sin;
using arma::asin;
using arma::sinh;
using arma::asinh;
using arma::cos;
using arma::acos;
using arma::cosh;
using arma::acosh;
using arma::tan;
using arma::atan;
using arma::tanh;
using arma::atanh;
using arma::cumsum;
using arma::hist;
using arma::sort;
using arma::sort_index;
using arma::stable_sort_index;
using arma::trans;
using arma::unique;
using arma::toeplitz;
using arma::circ_toeplitz;
using arma::accu;
using arma::min;
using arma::max;
using arma::prod;
using arma::sum;
using arma::mean;
using arma::median;
using arma::stddev;
using arma::var;
using arma::cor;
using arma::cov;
using arma::diagmat;
using arma::is_finite;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

namespace armadilloJava {
  class ExpectedGenColVec : public Expected {
    public:
      ExpectedGenColVec() {
        cout << "Compute ExpectedGenColVec(): " << endl;

        vector<vector<pair<string, void*>>> inputs = Input::getTestParameters({
          InputClass::GenColVec
        });

        for (vector<pair<string, void*>> input : inputs) {
          _fileSuffix = "";

          int n = 0;
          for (pair<string, void*> value : input) {
            switch (n) {
              case 0:
                _fileSuffix += value.first;
                _genColVec = *static_cast<Col<double>*>(value.second);
                break;
            }
            ++n;
          }

          cout << "Using input: " << _fileSuffix << endl;

          expectedArmaAbs();
          expectedArmaEps();
          expectedArmaExp();
          expectedArmaExp2();
          expectedArmaExp10();
          expectedArmaTrunc_exp();
          expectedArmaLog();
          expectedArmaLog2();
          expectedArmaLog10();
          expectedArmaTrunc_log();
          expectedArmaSqrt();
          expectedArmaSquare();
          expectedArmaFloor();
          expectedArmaCeil();
          expectedArmaRound();
          expectedArmaSign();
          expectedArmaSin();
          expectedArmaAsin();
          expectedArmaSinh();
          expectedArmaAsinh();
          expectedArmaCos();
          expectedArmaAcos();
          expectedArmaCosh();
          expectedArmaAcosh();
          expectedArmaTan();
          expectedArmaAtan();
          expectedArmaTanh();
          expectedArmaAtanh();
          expectedArmaCumsum();
          expectedArmaHist();
          expectedArmaSort();
          expectedArmaSort_index();
          expectedArmaStable_sort_index();
          expectedArmaTrans();
          expectedArmaUnique();
          expectedArmaNegate();
          expectedArmaReciprocal();
          expectedArmaToeplitz();
          expectedArmaCirc_toeplitz();
          expectedArmaAccu();
          expectedArmaMin();
          expectedArmaMax();
          expectedArmaProd();
          expectedArmaSum();
          expectedArmaMean();
          expectedArmaMedian();
          expectedArmaStddev();
          expectedArmaVar();
          expectedArmaCor();
          expectedArmaCov();
          expectedArmaDiagmat();
          expectedArmaIs_finite();
          expectedMat();
		  expectedColVecSize();
		  expectedColVecT();
		  expectedColVecPrint();
		  expectedColVecRaw_print();
		  expectedColIs_finite();
		  expectedColMinA();
		  expectedColMaxA();
		  expectedColMinB();
		  expectedColMaxB();
		  expectedColIs_empty();

        }

        cout << "done." << endl;
      }

    protected:
      Col<double> _genColVec;

      void expectedArmaAbs() {
        cout << "- Compute expectedArmaAbs() ... ";
        save<double>("Arma.abs", abs(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaEps() {
        cout << "- Compute expectedArmaEps() ... ";
        save<double>("Arma.eps", eps(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaExp() {
        cout << "- Compute expectedArmaExp() ... ";
        save<double>("Arma.exp", exp(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaExp2() {
        cout << "- Compute expectedArmaExp2() ... ";
        save<double>("Arma.exp2", exp2(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaExp10() {
        cout << "- Compute expectedArmaExp10() ... ";
        save<double>("Arma.exp10", exp10(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaTrunc_exp() {
        cout << "- Compute expectedArmaTrunc_exp() ... ";
        save<double>("Arma.trunc_exp", trunc_exp(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaLog() {
        cout << "- Compute expectedArmaLog() ... ";
        save<double>("Arma.log", log(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaLog2() {
        cout << "- Compute expectedArmaLog2() ... ";
        save<double>("Arma.log2", log2(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaLog10() {
        cout << "- Compute expectedArmaLog10() ... ";
        save<double>("Arma.log10", log10(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaTrunc_log() {
        cout << "- Compute expectedArmaTrunc_log() ... ";
        save<double>("Arma.trunc_log", trunc_log(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaSqrt() {
        cout << "- Compute expectedArmaSqrt() ... ";
        save<double>("Arma.sqrt", sqrt(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaSquare() {
        cout << "- Compute expectedArmaSquare() ... ";
        save<double>("Arma.square", square(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaFloor() {
        cout << "- Compute expectedArmaFloor() ... ";
        save<double>("Arma.floor", floor(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaCeil() {
        cout << "- Compute expectedArmaCeil() ... ";
        save<double>("Arma.ceil", ceil(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaRound() {
        cout << "- Compute expectedArmaRound() ... ";
        save<double>("Arma.round", round(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaSign() {
        cout << "- Compute expectedArmaSign() ... ";
        save<double>("Arma.sign", sign(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaSin() {
        cout << "- Compute expectedArmaSin() ... ";
        save<double>("Arma.sin", sin(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaAsin() {
        cout << "- Compute expectedArmaAsin() ... ";
        save<double>("Arma.asin", asin(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaSinh() {
        cout << "- Compute expectedArmaSinh() ... ";
        save<double>("Arma.sinh", sinh(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaAsinh() {
        cout << "- Compute expectedArmaAsinh() ... ";
        save<double>("Arma.asinh", asinh(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaCos() {
        cout << "- Compute expectedArmaCos() ... ";
        save<double>("Arma.cos", cos(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaAcos() {
        cout << "- Compute expectedArmaAcos() ... ";
        save<double>("Arma.acos", acos(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaCosh() {
        cout << "- Compute expectedArmaCosh() ... ";
        save<double>("Arma.cosh", cosh(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaAcosh() {
        cout << "- Compute expectedArmaAcosh() ... ";

        /*
         * acosh behaves buggy on some systems, with acosh(inf) = nan instead of inf
         */
        //save<double>("Arma.acosh", acosh(_genColVec));

        Mat<double> expected = _genColVec;
        expected.transform([](double value) {
          return log(value + sqrt(pow(value, 2) - 1));
        });
        save<double>("Arma.acosh", expected);

        cout << "done." << endl;
      }

      void expectedArmaTan() {
        cout << "- Compute expectedArmaTan() ... ";
        save<double>("Arma.tan", tan(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaAtan() {
        cout << "- Compute expectedArmaAtan() ... ";
        save<double>("Arma.atan", atan(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaTanh() {
        cout << "- Compute expectedArmaTanh() ... ";
        save<double>("Arma.tanh", tanh(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaAtanh() {
        cout << "- Compute expectedArmaAtanh() ... ";
        save<double>("Arma.atanh", atanh(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaCumsum() {
        cout << "- Compute expectedArmaCumsum() ... ";
        save<double>("Arma.cumsum", cumsum(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaHist() {
        cout << "- Compute expectedArmaHist() ... ";
        save<uword>("Arma.hist", hist(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaSort() {
        if(!_genColVec.is_finite()) {
          return;
        }

        cout << "- Compute expectedArmaSort() ... ";
        save<double>("Arma.sort", sort(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaSort_index() {
        if(!_genColVec.is_finite()) {
          return;
        }

        cout << "- Compute expectedArmaSort_index() ... ";
        save<uword>("Arma.sort_index", sort_index(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaStable_sort_index() {
        if(!_genColVec.is_finite()) {
          return;
        }

        cout << "- Compute expectedArmaStable_sort_index() ... ";
        save<uword>("Arma.stable_sort_index", stable_sort_index(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaTrans() {
        cout << "- Compute expectedArmaTrans() ... ";
        save<double>("Arma.trans", trans(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaUnique() {
        cout << "- Compute expectedArmaUnique() ... ";
        save<double>("Arma.unique", unique(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaNegate() {
        cout << "- Compute expectedArmaNegate() ... ";
        save<double>("Arma.negate", -_genColVec);
        cout << "done." << endl;
      }

      void expectedArmaReciprocal() {
        cout << "- Compute expectedArmaReciprocal() ... ";
        save<double>("Arma.reciprocal", 1/_genColVec);
        cout << "done." << endl;
      }

      void expectedArmaToeplitz() {
        cout << "- Compute expectedArmaToeplitz() ... ";
        save<double>("Arma.toeplitz", toeplitz(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaCirc_toeplitz() {
        cout << "- Compute expectedArmaCirc_toeplitz() ... ";
        save<double>("Arma.circ_toeplitz", circ_toeplitz(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaAccu() {
        cout << "- Compute expectedArmaAccu() ... ";
        save<double>("Arma.accu", Mat<double>({accu(_genColVec)}));
        cout << "done." << endl;
      }

      void expectedArmaMin() {
        cout << "- Compute expectedArmaMin() ... ";
        save<double>("Arma.min", Mat<double>({min(_genColVec)}));
        cout << "done." << endl;
      }

      void expectedArmaMax() {
        cout << "- Compute expectedArmaMax() ... ";
        save<double>("Arma.max", Mat<double>({max(_genColVec)}));
        cout << "done." << endl;
      }

      void expectedArmaProd() {
        cout << "- Compute expectedArmaProd() ... ";
        save<double>("Arma.prod", Mat<double>({prod(_genColVec)}));
        cout << "done." << endl;
      }

      void expectedArmaSum() {
        cout << "- Compute expectedArmaSum() ... ";
        save<double>("Arma.sum", Mat<double>({sum(_genColVec)}));
        cout << "done." << endl;
      }

      void expectedArmaMean() {
        cout << "- Compute expectedArmaMean() ... ";
        save<double>("Arma.mean", Mat<double>({mean(_genColVec)}));
        cout << "done." << endl;
      }

      void expectedArmaMedian() {
        cout << "- Compute expectedArmaMedian() ... ";
        save<double>("Arma.median", Mat<double>({median(_genColVec)}));
        cout << "done." << endl;
      }

      void expectedArmaStddev() {
        cout << "- Compute expectedArmaStddev() ... ";
        save<double>("Arma.stddev", Mat<double>({stddev(_genColVec)}));
        cout << "done." << endl;
      }

      void expectedArmaVar() {
        cout << "- Compute expectedArmaVar() ... ";
        save<double>("Arma.var", Mat<double>({var(_genColVec)}));
        cout << "done." << endl;
      }

      void expectedArmaCor() {
        cout << "- Compute expectedArmaCor() ... ";
        save<double>("Arma.cor", Mat<double>({cor(_genColVec)}));
        cout << "done." << endl;
      }

      void expectedArmaCov() {
        cout << "- Compute expectedArmaCov() ... ";
        save<double>("Arma.cov", Mat<double>({cov(_genColVec)}));
        cout << "done." << endl;
      }

      void expectedArmaDiagmat() {
        cout << "- Compute expectedArmaDiagmat() ... ";
        save<double>("Arma.diagmat", diagmat(_genColVec));
        cout << "done." << endl;
      }

      void expectedArmaIs_finite() {
        cout << "- Compute expectedArmaIs_finite() ... ";

        if(is_finite(_genColVec)) {
          save<double>("Arma.is_finite", Mat<double>({1.0}));
        } else {
          save<double>("Arma.is_finite", Mat<double>({0.0}));
        }

        cout << "done." << endl;
      }

      void expectedMat() {
        cout << "- Compute expectedMat() ... ";
        save<double>("Mat", _genColVec);
        cout << "done." << endl;
      }

	  void expectedColVecSize() {
		  cout << "- Compute expectedColVecSize() ... ";
		  save<double>("Col.size", Mat<double>({static_cast<double>(_genColVec.size())}));
		  cout << "done." << endl;
      }

	  void expectedColVecT() {
		  cout << "- Compute expectedColVecT() ... ";
		  save<double>("Col.t", _genColVec.t());
		  cout << "done." << endl;
      }

	  void expectedColVecPrint() {
		  cout << "- Compute expectedColVecPrint() ... ";

		  ofstream expected(_filepath + "Col.print(" + _fileSuffix + ").txt");
		  streambuf* previousBuffer = cout.rdbuf(expected.rdbuf());

		  _genColVec.print();

		  cout.rdbuf(previousBuffer);

		  cout << "done." << endl;
      }

	  void expectedColVecRaw_print() {
		  cout << "- Compute expectedColVecRaw_print() ... ";

		  ofstream expected(_filepath + "Col.raw_print(" + _fileSuffix + ").txt");
		  streambuf* previousBuffer = cout.rdbuf(expected.rdbuf());

		  _genColVec.raw_print();

		  cout.rdbuf(previousBuffer);

		  cout << "done." << endl;
      }

      void expectedColIs_finite() {
        cout << "- Compute expectedColIs_finite() ... ";

        if(_genColVec.is_finite()) {
          save<double>("Col.is_finite", Col<double>({1}));
        } else {
          save<double>("Col.is_finite", Col<double>({0}));
        }

        cout << "done." << endl;
      }

      void expectedColMinA() {
        cout << "- Compute expectedColMinA() ... ";
        double value;
        value = _genColVec.min();
        save<double>("Col.minA", Col<double>({value}));

        cout << "done." << endl;
      }

      void expectedColMaxA() {
        cout << "- Compute expectedColMaxA() ... ";
        double value;
        value = _genColVec.max();
        save<double>("Col.maxA", Col<double>({value}));
        cout << "done." << endl;
      }

      void expectedColMinB() {
        cout << "- Compute expectedColMinB() ... ";
        uword value;
        _genColVec.min(value);
        save<double>("Col.minB", Col<double>({static_cast<double>(value)}));
        cout << "done." << endl;
      }

      void expectedColMaxB() {
        cout << "- Compute expectedColMaxB() ... ";
        uword value;
        _genColVec.max(value);
        save<double>("Col.maxB", Col<double>({static_cast<double>(value)}));
        cout << "done." << endl;
      }

      void expectedColIs_empty() {
        cout << "- Compute expectedColIs_empty() ... ";

        if(_genColVec.is_empty()) {
          save<double>("Col.is_empty", Col<double>({1}));
        } else {
          save<double>("Col.is_empty", Col<double>({0}));
        }

        cout << "done." << endl;
      }

  };
}


// Albert Huang <albert@csail.mit.edu>
//
// Implementation of
//
//    Berthold K. P. Horn,
//    "Closed-form solution of absolute orientation using unit quaternions",
//    Journal of the Optical society of America A, Vol. 4, April 1987

#ifndef __absolute_orientation_horn__
#define __absolute_orientation_horn__

#include <assert.h>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>

/**
 * absolute_orientation_horn:
 * @P1: a matrix of dimension [3 x num_points]
 * @P2: a matrix of dimension [3 x num_points]
 * @result: output parameter
 *
 * Given two point sets P1 and P2, with each point p1_i in set P1 matched with
 * a point p2_i in P2, compute the rigid body transformation (isometry) M that
 * minimizes
 *
 * \sum ||p2_i - M p1_i||
 *
 * returns: 0 on success, -1 on failure
 */
template <typename DerivedA, typename DerivedB>
int absolute_orientation_horn(const Eigen::MatrixBase<DerivedA>& P1,
        const Eigen::MatrixBase<DerivedB>& P2,
        Eigen::Isometry3d* result)
{
    int num_points = P1.cols();
    assert(P1.cols() == P2.cols());
    assert(P1.rows() == 3 && P2.rows() == 3);
    if(num_points < 3)
        return -1;

    // compute centroids of point sets
    Eigen::Vector3d P1_centroid = P1.rowwise().sum() / num_points;
    Eigen::Vector3d P2_centroid = P2.rowwise().sum() / num_points;

    Eigen::MatrixXd R1 = P1;
    R1.colwise() -= P1_centroid;
    Eigen::MatrixXd R2 = P2;
    R2.colwise() -= P2_centroid;

    // compute matrix M
    double Sxx = R1.row(0).dot(R2.row(0));
    double Sxy = R1.row(0).dot(R2.row(1));
    double Sxz = R1.row(0).dot(R2.row(2));
    double Syx = R1.row(1).dot(R2.row(0));
    double Syy = R1.row(1).dot(R2.row(1));
    double Syz = R1.row(1).dot(R2.row(2));
    double Szx = R1.row(2).dot(R2.row(0));
    double Szy = R1.row(2).dot(R2.row(1));
    double Szz = R1.row(2).dot(R2.row(2));

    double A00 = Sxx + Syy + Szz;
    double A01 = Syz - Szy;
    double A02 = Szx - Sxz;
    double A03 = Sxy - Syx;
    double A11 = Sxx - Syy - Szz;
    double A12 = Sxy + Syx;
    double A13 = Szx + Sxz;
    double A22 = -Sxx + Syy - Szz;
    double A23 = Syz + Szy;
    double A33 = -Sxx - Syy + Szz;

    // prepare matrix for eigen analysis
    Eigen::Matrix4d N;
    N << A00, A01, A02, A03,
        A01, A11, A12, A13,
        A02, A12, A22, A23,
        A03, A13, A23, A33;

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> eigensolver(N);

    // rotation quaternion is the eigenvector with greatest eigenvalue
    Eigen::Vector4d eigvals = eigensolver.eigenvalues();
    int max_eigen_ind = 0;
    double max_eigen_val = eigvals(0);
    for(int i=1; i<4; i++) {
        if(eigvals(i) > max_eigen_val) {
            max_eigen_val = eigvals(i);
            max_eigen_ind = i;
        }
    }
    Eigen::Vector4d quat = eigensolver.eigenvectors().col(max_eigen_ind);
    Eigen::Quaterniond rotation(quat[0], quat[1], quat[2], quat[3]);

    // now compute the resulting isometry
    result->setIdentity();
    result->translate(P2_centroid - rotation * P1_centroid);
    result->rotate(rotation);

    return 0;
}
#endif


/*
 * Copyright Nick Thompson, 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 <boost/math/tools/config.hpp>
#ifndef BOOST_MATH_NO_THREAD_LOCAL_WITH_NON_TRIVIAL_TYPES
#include "math_unit_test.hpp"
#include <numeric>
#include <random>
#include <array>
#include <boost/core/demangle.hpp>
#include <boost/math/interpolators/bezier_polynomial.hpp>
#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
using boost::multiprecision::float128;
#endif

using boost::math::interpolators::bezier_polynomial;

template<typename Real>
void test_linear()
{
    std::vector<std::array<Real, 2>> control_points(2);
    control_points[0] = {0.0, 0.0};
    control_points[1] = {1.0, 1.0};
    auto control_points_copy = control_points;
    auto bp = bezier_polynomial(std::move(control_points_copy));

    // P(0) = P_0:
    CHECK_ULP_CLOSE(control_points[0][0], bp(0)[0], 3);
    CHECK_ULP_CLOSE(control_points[0][1], bp(0)[1], 3);

    // P(1) = P_n:
    CHECK_ULP_CLOSE(control_points[1][0], bp(1)[0], 3);
    CHECK_ULP_CLOSE(control_points[1][1], bp(1)[1], 3);

    for (Real t = Real(1)/32; t < 1; t += Real(1)/32) {
        Real expected0 = (1-t)*control_points[0][0] + t*control_points[1][0];
        CHECK_ULP_CLOSE(expected0, bp(t)[0], 3);
    }

    // P(1) = P_n:
    std::array<Real, 2> endpoint{1,2};
    bp.edit_control_point(endpoint, 1);
    CHECK_ULP_CLOSE(endpoint[0], bp(1)[0], 3);
    CHECK_ULP_CLOSE(endpoint[1], bp(1)[1], 3);

}

template<typename Real>
void test_quadratic()
{
    std::vector<std::array<Real, 2>> control_points(3);
    control_points[0] = {0.0, 0.0};
    control_points[1] = {1.0, 1.0};
    control_points[2] = {2.0, 2.0};
    auto control_points_copy = control_points;
    auto bp = bezier_polynomial(std::move(control_points_copy));

    // P(0) = P_0:
    auto computed_point = bp(0);
    CHECK_ULP_CLOSE(control_points[0][0], computed_point[0], 3);
    CHECK_ULP_CLOSE(control_points[0][1], computed_point[1], 3);
    auto computed_dp = bp.prime(0);
    CHECK_ULP_CLOSE(2*(control_points[1][0] - control_points[0][0]), computed_dp[0], 3);
    CHECK_ULP_CLOSE(2*(control_points[1][1] - control_points[0][1]), computed_dp[1], 3);

    // P(1) = P_n:
    computed_point = bp(1);
    CHECK_ULP_CLOSE(control_points[2][0], computed_point[0], 3);
    CHECK_ULP_CLOSE(control_points[2][1], computed_point[1], 3);
}

// All points on a Bezier polynomial fall into the convex hull of the control polygon.
template<typename Real>
void test_convex_hull()
{
    std::vector<std::array<Real, 2>> control_points(4);
    control_points[0] = {0.0, 0.0};
    control_points[1] = {0.0, 1.0};
    control_points[2] = {1.0, 1.0};
    control_points[3] = {1.0, 0.0};
    auto bp = bezier_polynomial(std::move(control_points));

    for (Real t = 0; t <= 1; t += Real(1)/32) {
        auto p = bp(t);
        CHECK_LE(p[0], Real(1));
        CHECK_LE(Real(0), p[0]);
        CHECK_LE(p[1], Real(1));
        CHECK_LE(Real(0), p[1]);
    }
}

// Reversal Symmetry: If q(t) is the Bezier polynomial which consumes the control points in reversed order from p(t),
// then p(t) = q(1-t).
template<typename Real>
void test_reversal_symmetry()
{
    std::vector<std::array<Real, 3>> control_points(10);
    std::uniform_real_distribution<Real> dis(-1,1);
    std::mt19937_64 gen;
    for (size_t i = 0; i < control_points.size(); ++i) {
        for (size_t j = 0; j < 3; ++j) {
            control_points[i][j] = dis(gen);
        }
    }

    auto control_points_copy = control_points;
    auto bp0 = bezier_polynomial(std::move(control_points_copy));

    control_points_copy = control_points;
    std::reverse(control_points_copy.begin(), control_points_copy.end());
    auto bp1 = bezier_polynomial(std::move(control_points_copy));
    auto P0 = bp0(Real(0));
    CHECK_ULP_CLOSE(control_points[0][0], P0[0], 3);
    CHECK_ULP_CLOSE(control_points[0][1], P0[1], 3);
    CHECK_ULP_CLOSE(control_points[0][2], P0[2], 3);
    auto P1 = bp0(Real(1));
    CHECK_ULP_CLOSE(control_points.back()[0], P1[0], 3);
    CHECK_ULP_CLOSE(control_points.back()[1], P1[1], 3);
    CHECK_ULP_CLOSE(control_points.back()[2], P1[2], 3);

    P0 = bp1(Real(1));
    CHECK_ULP_CLOSE(control_points[0][0], P0[0], 3);
    CHECK_ULP_CLOSE(control_points[0][1], P0[1], 3);
    CHECK_ULP_CLOSE(control_points[0][2], P0[2], 3);

    P1 = bp1(Real(0));
    CHECK_ULP_CLOSE(control_points.back()[0], P1[0], 3);
    CHECK_ULP_CLOSE(control_points.back()[1], P1[1], 3);
    CHECK_ULP_CLOSE(control_points.back()[2], P1[2], 3);

    for (Real t = 0; t <= 1; t += 1.0) {
        auto P0 = bp0(t);
        auto P1 = bp1(1.0-t);
        if (!CHECK_ULP_CLOSE(P0[0], P1[0], 3)) {
            std::cerr << "  Error at t = " << t << "\n";
        }
        CHECK_ULP_CLOSE(P0[1], P1[1], 3);
        CHECK_ULP_CLOSE(P0[2], P1[2], 3);
    }
}

// Linear precision: If all control points lie *equidistantly* on a line, then the Bezier curve falls on a line.
// See Bezier and B-spline techniques, Section 2.8, Remark 8.
template<typename Real>
void test_linear_precision()
{
    std::vector<std::array<Real, 3>> control_points(10);
    std::array<Real, 3> P0 = {1,1,1};
    std::array<Real, 3> Pf = {2,2,2};
    control_points[0] = P0;
    control_points[9] = Pf;
    for (size_t i = 1; i < 9; ++i) {
        Real t = Real(i)/(control_points.size()-1);
        control_points[i][0] = (1-t)*P0[0] + t*Pf[0];
        control_points[i][1] = (1-t)*P0[1] + t*Pf[1];
        control_points[i][2] = (1-t)*P0[2] + t*Pf[2];
    }

    auto bp = bezier_polynomial(std::move(control_points));
    for (Real t = 0; t < 1; t += Real(1)/32) {
        std::array<Real, 3> P;
        P[0] = (1-t)*P0[0] + t*Pf[0];
        P[1] = (1-t)*P0[1] + t*Pf[1];
        P[2] = (1-t)*P0[2] + t*Pf[2];

        auto computed = bp(t);
        CHECK_ULP_CLOSE(P[0], computed[0], 3);
        CHECK_ULP_CLOSE(P[1], computed[1], 3);
        CHECK_ULP_CLOSE(P[2], computed[2], 3);

        std::array<Real, 3> dP;
        dP[0] = Pf[0] - P0[0];
        dP[1] = Pf[1] - P0[1];
        dP[2] = Pf[2] - P0[2];
        auto dpComputed = bp.prime(t);
        CHECK_ULP_CLOSE(dP[0], dpComputed[0], 5);
    }
}

int main()
{
    test_linear<float>();
    test_linear<double>();
    test_quadratic<float>();
    test_quadratic<double>();
    test_convex_hull<float>();
    test_convex_hull<double>();
    test_linear_precision<float>();
    test_linear_precision<double>();
    test_reversal_symmetry<float>();
    test_reversal_symmetry<double>();
#ifdef BOOST_HAS_FLOAT128
    test_linear<float128>();
    test_quadratic<float128>();
    test_convex_hull<float128>();
#endif
    return boost::math::test::report_errors();
}

#else
int main() {
    return 0;
}
#endif


// Copyright  (C)  2008  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>

// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl

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

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

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

//implementation of svd according to (Maciejewski and Klein,1989)
//and (Braun, Ulrey, Maciejewski and Siegel,2002)

/**
 * \file svd_eigen_Macie.hpp
 * provides Maciejewski's implementation for SVD.
 */

#ifndef SVD_EIGEN_MACIE
#define SVD_EIGEN_MACIE

#include <Eigen/Core>

namespace KDL
{

	/**
	 * svd_eigen_Macie provides Maciejewski implementation for SVD.
	 *
	 * computes the singular value decomposition of a matrix A, such that
	 * A=U*Sm*V
	 *
	 * (Maciejewski and Klein,1989) and (Braun, Ulrey, Maciejewski and Siegel,2002)
	 *
	 * \param A [INPUT] is an \f$m \times n\f$-matrix, where \f$ m \geq n \f$.
	 * \param S [OUTPUT] is an \f$n\f$-vector, representing the diagonal elements of the diagonal matrix Sm.
	 * \param U [INPUT/OUTPUT] is an \f$m \times m\f$ orthonormal matrix.
	 * \param V [INPUT/OUTPUT] is an \f$n \times n\f$ orthonormal matrix.
	 * \param B [TEMPORARY] is an \f$m \times n\f$ matrix used for temporary storage.
	 * \param tempi [TEMPORARY] is an \f$m\f$ vector used for temporary storage.
	 * \param threshold [INPUT] Threshold to determine orthogonality.
	 * \param toggle [INPUT] toggle this boolean variable on each call of this routine.
	 * \return number of sweeps.
	 */
    int svd_eigen_Macie(const Eigen::MatrixXd& A,Eigen::MatrixXd& U,Eigen::VectorXd& S, Eigen::MatrixXd& V,
                        Eigen::MatrixXd& B, Eigen::VectorXd& tempi,
                        double threshold,bool toggle);

}
#endif


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

//// Body to body/field potential influence coefficient computation
// Compute doublet and source potential influence coefficient between 2 surface panels
// Katz and Plotkin (2001), Low-Speed Aerodynamics. Program 12
//
// Inputs:
// - wakeFlag: defines if wake or body panel
// - idTgt: index of target panel
// - idSrc: index of influencing panel
// - x, y, z: coordinates of target panel center in panel axis
// - x1, y1, x2, y2, x3, y3, x4, y4: coordinates of influencing panel vertices in panel axis
//
// Output:
// - coeff: array of AIC ([0] = mu, [1] = tau)

#include <iostream>
#include <Eigen/Dense>
#include <array>
#include "infcB.h"

#define PI 3.14159

using namespace std;
using namespace Eigen;

array<double,2> infcB(bool wakeFlag,int idTgt, int idSrc, double x, double y, double z,
                      double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4) {

    // Temporary
    RowVector4d xV, yV; // Local vertices
    RowVector4d d, r, e, h, F, G; // coefficients
    double mu = 0, tau = 0; // AIC
    array<double,2> coeff;

    xV(0) = x1; xV(1) = x2; xV(2) = x3; xV(3) = x4;
    yV(0) = y1; yV(1) = y2; yV(2) = y3; yV(3) = y4;

    //// Wake panels
    if (wakeFlag) {

        // Coefficients
        for (int i = 1; i <= 4; ++i) {
            int j = i%4 + 1;

            r(i-1) = sqrt((x - xV(i-1))*(x - xV(i-1)) + (y - yV(i-1))*(y - yV(i-1)) + z * z);
            e(i-1) = (x - xV(i-1))*(x - xV(i-1)) + z*z;
            h(i-1) = (x - xV(i-1)) * (y - yV(i-1));
        }
        for (int i = 1; i <= 4; ++i) {
            int j = i%4 + 1;

            F(i-1) = (yV(j-1) - yV(i-1))*e(i-1) - (xV(j-1) - xV(i-1))*h(i-1);
            G(i-1) = (yV(j-1) - yV(i-1))*e(j-1) - (xV(j-1) - xV(i-1))*h(j-1);
        }

        // Doublet
        for (int i = 1; i <= 4; ++i) {
            int j = i % 4 + 1;

            mu = mu + atan2(z * (xV(j-1) - xV(i-1)) * (F(i-1) * r(j-1) - G(i-1) * r(i-1)),
                            z * z * (xV(j-1) - xV(i-1)) * (xV(j-1) - xV(i-1)) * r(i-1) * r(j-1) + F(i-1) * G(i-1));
        }
        mu = -1 / (4 * PI) * mu;
        tau = 0;
    }

    //// Body panels
    else {
        // Coefficients
        for (int i = 1; i <= 4; ++i) {
            int j = i%4 + 1;

            d(i-1) = sqrt((xV(j-1) - xV(i-1))*(xV(j-1) - xV(i-1)) + (yV(j-1) - yV(i-1))*(yV(j-1) - yV(i-1)));
            r(i-1) = sqrt((x - xV(i-1))*(x - xV(i-1)) + (y - yV(i-1))*(y - yV(i-1)) + z * z);
        }

        if (idTgt == idSrc) {
            // Source
            for (int i = 1; i <= 4; ++i) {
                int j = i % 4 + 1;

                tau = tau + ((x - xV(i-1)) * (yV(j-1) - yV(i-1)) - (y - yV(i-1)) * (xV(j-1) - xV(i-1))) / d(i-1) *
                            log((r(i-1) + r(j-1) + d(i-1)) / (r(i-1) + r(j-1) - d(i-1)));
            }
            mu = 0.5;
            tau = -1 / (4 * PI) * tau;
        }
        else {
            // Coefficients
            for (int i = 1; i <= 4; ++i) {
                e(i-1) = (x - xV(i-1))*(x - xV(i-1)) + z*z;
                h(i-1) = (x - xV(i-1)) * (y - yV(i-1));
            }
            for (int i = 1; i <= 4; ++i) {
                int j = i%4 + 1;

                F(i-1) = (yV(j-1) - yV(i-1))*e(i-1) - (xV(j-1) - xV(i-1))*h(i-1);
                G(i-1) = (yV(j-1) - yV(i-1))*e(j-1) - (xV(j-1) - xV(i-1))*h(j-1);
            }

            // Doublet
            for (int i = 1; i <= 4; ++i) {
                int j = i % 4 + 1;

                mu = mu + atan2(z * (xV(j-1) - xV(i-1)) * (F(i-1) * r(j-1) - G(i-1) * r(i-1)),
                                      z * z * (xV(j-1) - xV(i-1)) * (xV(j-1) - xV(i-1)) * r(i-1) * r(j-1) + F(i-1) * G(i-1));
            }
            mu = -1 / (4 * PI) * mu;
            // Source
            for (int i = 1; i <= 4; ++i) {
                int j = i % 4 + 1;

                tau = tau + ((x - xV(i-1)) * (yV(j-1) - yV(i-1)) - (y - yV(i-1)) * (xV(j-1) - xV(i-1))) / d(i-1) *
                                log((r(i-1) + r(j-1) + d(i-1)) / (r(i-1) + r(j-1) - d(i-1)));
            }
            tau = -1 / (4 * PI) * tau - z * mu;
        }
    }
    coeff[0] = mu;
    coeff[1] = tau;

    return  coeff;
}


#include "utils.hpp"

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/integer.hpp>
using namespace std;
using Bint = boost::multiprecision::cpp_int;

string BruteForceFactorizer_cppfunc(string s){
    Bint n(s);
    Bint sqrt_n = boost::multiprecision::sqrt(n);
    for(Bint i=2;i<=sqrt_n+1;i++){
        if(n%i==0){
            return i.str();
        }
    }
    return "1";
}


#include <catch.hpp>
#include <Eigen/Core>
#include <random>

#include "check_adjoint.h"
#include "test_utils.h"
#include "renderer_adjoint.cuh"

TEST_CASE("Adjoint-Cross", "[adjoint]")
{
	typedef empty TmpStorage_t;
	typedef VectorXr Vector_t;

	auto forward = [](const Vector_t& x, TmpStorage_t* tmp) -> Vector_t
	{
		const real3 a = fromEigen3(x.head(3));
		const real3 b = fromEigen3(x.tail(3));
		const real3 c = cross(a, b);
		return toEigen(c);
	};
	auto adjoint = [](const Vector_t& x, const Vector_t& e, const Vector_t& g,
		Vector_t& z, const TmpStorage_t& tmp)
	{
		const real3 a = fromEigen3(x.head(3));
		const real3 b = fromEigen3(x.tail(3));
		const real3 adj_c = fromEigen3(g);
		real3 adj_a = make_real3(0), adj_b = make_real3(0);
		kernel::adjCross(a, b, adj_c, adj_a, adj_b);
		z.head(3) = toEigen(adj_a);
		z.tail(3) = toEigen(adj_b);
	};

	std::default_random_engine rnd(42);
	std::normal_distribution<real_t> distr;
	int N = 20;
	for (int i=0; i<N; ++i)
	{
		INFO("N=" << i);
		const real3 a = make_real3(
			distr(rnd), distr(rnd), distr(rnd));
		const real3 b = make_real3(
			distr(rnd), distr(rnd), distr(rnd));
		Vector_t x(6);
		x.head(3) = toEigen(a);
		x.tail(3) = toEigen(b);

		checkAdjoint<Vector_t, TmpStorage_t>(x, forward, adjoint);
	}
}

TEST_CASE("Adjoint-Normalize", "[adjoint]")
{
	typedef empty TmpStorage_t;
	typedef Vector3r Vector_t;

	auto forward = [](const Vector_t& x, TmpStorage_t* tmp) -> Vector_t
	{
		const real3 a = fromEigen3(x);
		const real3 c = normalize(a);
		return toEigen(c);
	};
	auto adjoint = [](const Vector_t& x, const Vector_t& e, const Vector_t& g,
		Vector_t& z, const TmpStorage_t& tmp)
	{
		const real3 a = fromEigen3(x);
		const real3 adj_c = fromEigen3(g);
		real3 adj_a = kernel::adjNormalize(a, adj_c);
		z = toEigen(adj_a);
	};

	std::default_random_engine rnd(42);
	std::normal_distribution<real_t> distr;
	int N = 20;
	for (int i = 0; i < N; ++i)
	{
		INFO("N=" << i);
		const real3 a = make_real3(
			distr(rnd), distr(rnd), distr(rnd));
		Vector_t x = toEigen(a);

		checkAdjoint<Vector_t, TmpStorage_t>(x, forward, adjoint,
			1e-6, 1e-3, 1e-5);
	}
}


#include "ps/ps.h"
#include "cmath"
#include "lr.h"
#include <ctime>
#include <iostream>
#include <vector>
#include <fstream>

#include <Eigen/Dense>

using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::VectorXi;
using Eigen::ArrayXd;

namespace lrprox {

  LR::LR(int num_dims)
    : num_dims_(num_dims) {
    initWeight_();
  }

  double LR::cost(const MatrixXd &X, const VectorXi &y) {
    // minimize negative log prob
    VectorXd term_in_sigmoid = - y.cast<double>().cwiseProduct(X * weight_);
    // do not take average
    return term_in_sigmoid.array().exp().log1p().matrix().sum();
  }

  VectorXd LR::grad(const MatrixXd &X, const VectorXi &y) {
    ArrayXd term1 = 1 - ((- y.cast<double>().cwiseProduct(X * weight_)).array().exp() + 1).inverse();
    return -X.transpose() * (term1 * y.cast<double>().array()).matrix();
  }
//
//  VectorXi predict(const MatrixXd &X);
//
  const VectorXd& LR::getWeight() {
    return weight_;
  }
  void LR::updateWeight(const std::vector<double>& weight) {
//    for (int i = 0; i < weight.size(); i++) {
//      weight_(i) = weight[i];
//    }
    weight_ = VectorXd::Map(&weight[0], weight_.size());
  }
  void LR::updateWeight(const VectorXd& weight) {
    weight_ = weight;
  }
//
//  bool saveModel(std::string &filename);

  void LR::initWeight_() {
//    weight_ = VectorXd::Random(num_dims_);
    weight_ = VectorXd::Ones(num_dims_);
  }

//bool LR::SaveModel(std::string& filename) {
//  std::ofstream fout(filename.c_str());
//  fout << num_feature_dim_ << std::endl;
//  for (int i = 0; i < num_feature_dim_; ++i) {
//    fout << weight_[i] << ' ';
//  }
//  fout << std::endl;
//  fout.close();
//  return true;
//}

//std::string LR::DebugInfo() {
//  std::ostringstream out;
//  for (size_t i = 0; i < weight_.size(); ++i) {
//    out << weight_[i] << " ";
//  }
//  return out.str();
//}

//  float LR::Sigmoid_(std::vector<float> feature) {
//    float z = 0;
//    for (size_t j = 0; j < weight_.size(); ++j) {
//      z += weight_[j] * feature[j];
//    }
//    return 1. / (1. + exp(-z));
//  }

} // namespace lrprox


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_LOGSQRT2PI_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_LOGSQRT2PI_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate value \f$\log\sqrt{2\pi}\f$

    @par Header <boost/simd/constant/logsqrt2pi.hpp>

    @par Semantic:

    @code
    T r = Logsqrt2pi<T>();
    @endcode

    is similar to:

    @code
    T r = log(sqrt(2*Pi<T>());
    @endcode

    @return The Logsqrt2pi constant for the proper type
  **/
  template<typename T> T Logsqrt2pi();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generate the  constant logsqrt2pi.

      @return The Logsqrt2pi constant for the proper type
    **/
    Value Logsqrt2pi<Value>();
  }
} }
#endif

#include <boost/simd/constant/scalar/logsqrt2pi.hpp>
#include <boost/simd/constant/simd/logsqrt2pi.hpp>

#endif


#include <iostream>
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;
int main()
{
    MatrixXd m = MatrixXd::Random(3,3);
    m = (m + MatrixXd::Constant(3,3,1.2)) * 50;
    cout << "m =" << endl << m << endl;
    VectorXd v(3);
    v << 1, 2, 3;
    cout << "m * v =" << endl << m * v << endl;

    MatrixXf mat = MatrixXf::Random(2, 3);
    std::cout << mat << std::endl << std::endl;
    mat = (MatrixXf(2,2) << 0, 1, 1, 0).finished() * mat;
    std::cout << mat << std::endl;
}


/* C++ headers */
#include <random>

/* boost test framework. */
#define BOOST_TEST_MAIN
#define BOOST_TEST_ALTERNATIVE_INIT_API
#define BOOST_TEST_MODULE math library test
#include <boost/test/unit_test.hpp>

// enable loading of SIMD types.
#define ML_INCLUDE_SIMD

/* user headers. */
#include "ml/all.h"

/*
 * Helpers.
 */

void random_initialize_real_std_vector(std::size_t n, std::vector<ml::vec4>& v)
{
    std::random_device rnd_device;
    std::mt19937 mersenne_engine{rnd_device()};
    std::uniform_real_distribution<float> dist{-1, 1};

    auto gen = [&dist, &mersenne_engine]()
    {
        return ml::vec4{dist(mersenne_engine), dist(mersenne_engine), dist(mersenne_engine), dist(mersenne_engine)};
    };

    v.resize(n);
    generate(std::begin(v), std::end(v), gen);
}

/*
 * memory alignment.
 */
inline bool
  is_aligned(const void* ptr, std::uintptr_t alignment) noexcept
{
    auto iptr = reinterpret_cast<std::uintptr_t>(ptr);
    return !(iptr % alignment);
}

BOOST_AUTO_TEST_SUITE(math)

/*
 * vec2 tests.
 */

BOOST_AUTO_TEST_CASE(vec2)
{
    ml::vec2 v1{1, 0}, v2{0, 1};
    BOOST_TEST(v1.area(v2) == 1.0f);

    ml::vec2 v3{std::cos(M_PI / 4), std::sin(M_PI / 4)};
    ml::vec2 v4{std::cos(M_PI / 4 + M_PI / 2), std::sin(M_PI / 4 + M_PI / 2)};
    BOOST_TEST(v3.area(v4) == 1.0f, boost::test_tools::tolerance(1e-6f));
}

/*
 * tvec2 tests.
 */

BOOST_AUTO_TEST_CASE(tvec2)
{
    ml::tvec2<ml::fixed_24_8_t> v = {1.23, 2.24};
    BOOST_TEST(ml::to_float(v.x) == 1.23f, boost::test_tools::tolerance(4e-3f));
    BOOST_TEST(ml::to_float(v.y) == 2.24f, boost::test_tools::tolerance(4e-3f));
}

/*
 * vec2_fixed tests.
 */

BOOST_AUTO_TEST_CASE(vec2_fixed)
{
    const ml::vec2_fixed<8> v1{1, 0}, v2{0, 1};
    BOOST_TEST(v1.area(v2) == ml::vec2_fixed<8>::type{1});
    BOOST_TEST(v2.area(v1) == ml::vec2_fixed<8>::type{-1});
    BOOST_TEST(v1.area(v1) == ml::vec2_fixed<8>::type{0});
    BOOST_TEST(v2.area(v2) == ml::vec2_fixed<8>::type{0});
}

/*
 * vec4 tests.
 */

// check that we actually using both non-simd and simd types.
static_assert(!std::is_same<ml::vec4, ml::simd::vec4>::value);
static_assert(!std::is_same<ml::mat4x4, ml::simd::mat4x4>::value);

// paranoid checks.
static_assert(sizeof(ml::vec4) == 4 * sizeof(float));
static_assert(sizeof(ml::vec4) == sizeof(__m128));

BOOST_AUTO_TEST_CASE(vec4_alignment)
{
    std::vector<ml::vec4> v;
    random_initialize_real_std_vector(10, v);

    // check vector alignment while being paranoid.
    BOOST_TEST_MESSAGE("Checking 16-byte vector alignment");
    for(size_t i = 0; i < v.size(); ++i)
    {
        BOOST_CHECK(is_aligned(&v[i], 16));
    }
}

BOOST_AUTO_TEST_CASE(vec4_simd_initialization)
{
    ml::simd::vec4 v1{1, 2, 3, 4};
    ml::simd::vec4 v2{1, 2, 3};
    ml::simd::vec4 v3{ml::vec3{1, 2, 3}, 4};

    BOOST_TEST((v1.x == 1 && v1.y == 2 && v1.z == 3 && v1.w == 4));
    BOOST_TEST((v2.x == 1 && v2.y == 2 && v2.z == 3 && v2.w == 1)); /* w-component defaults to one if not specified. */
    BOOST_TEST((v3.x == 1 && v3.y == 2 && v3.z == 3 && v3.w == 4));

    float v[4] = {1, 2, 3, 4};
    ml::simd::vec4 v4{v};
    ml::vec4 v5{v};

    BOOST_TEST((v4.x == 1 && v4.y == 2 && v4.z == 3 && v4.w == 4));
    BOOST_TEST((v5.x == 1 && v5.y == 2 && v5.z == 3 && v5.w == 4));
}

BOOST_AUTO_TEST_CASE(vec4_simd_comparisons)
{
    BOOST_TEST((ml::simd::vec4(1, 2, 3, 4) == ml::simd::vec4(1, 2, 3, 4)));
    BOOST_TEST(!(ml::simd::vec4(1, 2, 3, 4) == ml::simd::vec4(1, 2, 3, 0)));
    BOOST_TEST(!(ml::simd::vec4(1, 2, 3, 4) != ml::simd::vec4(1, 2, 3, 4)));
    BOOST_TEST((ml::simd::vec4(1, 2, 3, 4) != ml::simd::vec4(1, 2, 3, 0)));
}

BOOST_AUTO_TEST_CASE(vec4_component_product)
{
    BOOST_TEST((ml::vec4(1, 2, 3, 4) * ml::vec4(4, 3, 2, 1) == ml::vec4(4, 6, 6, 4)));
    BOOST_TEST((ml::vec4(0, 1, 0, 1) * ml::vec4(-1, 0, -1, 0) == ml::vec4(0, 0, 0, 0)));
    BOOST_TEST((ml::vec4(-2, -3, 3, 2) * ml::vec4(-1, 0, 4, -4) == ml::vec4(2, 0, 12, -8)));
}

BOOST_AUTO_TEST_CASE(vec4_simd_component_product)
{
    BOOST_TEST((ml::simd::vec4(1, 2, 3, 4) * ml::simd::vec4(4, 3, 2, 1) == ml::simd::vec4(4, 6, 6, 4)));
    BOOST_TEST((ml::simd::vec4(0, 1, 0, 1) * ml::simd::vec4(-1, 0, -1, 0) == ml::simd::vec4(0, 0, 0, 0)));
    BOOST_TEST((ml::simd::vec4(-2, -3, 3, 2) * ml::simd::vec4(-1, 0, 4, -4) == ml::simd::vec4(2, 0, 12, -8)));
}

BOOST_AUTO_TEST_CASE(vec4_dot)
{
    BOOST_TEST(ml::dot(ml::vec4(1, -1, 1, -1), ml::vec4(1, -1, 1, -1)) == 4);
    BOOST_TEST(ml::dot(ml::vec4(1, 2, 3, 4), ml::vec4(4, 3, 2, 1)) == 20);
    BOOST_TEST(ml::dot(ml::vec4(1, 2, 3, 4), ml::vec4(-2, 1, -4, 3)) == 0);
}

BOOST_AUTO_TEST_CASE(vec4_simd_dot)
{
    BOOST_TEST(ml::dot(ml::simd::vec4(1, -1, 1, -1), ml::simd::vec4(1, -1, 1, -1)) == 4);
    BOOST_TEST(ml::dot(ml::simd::vec4(1, 2, 3, 4), ml::simd::vec4(4, 3, 2, 1)) == 20);
    BOOST_TEST(ml::dot(ml::simd::vec4(1, 2, 3, 4), ml::simd::vec4(-2, 1, -4, 3)) == 0);
}

BOOST_AUTO_TEST_CASE(vec4_is_zero)
{
    BOOST_TEST(!ml::vec4(0, 0, 0, 1).is_zero());
    BOOST_TEST(ml::vec4(0, 0, 0, 0).is_zero());
}

BOOST_AUTO_TEST_CASE(vec4_simd_is_zero)
{
    BOOST_TEST(!ml::simd::vec4(0, 0, 0, 1).is_zero());
    BOOST_TEST(ml::simd::vec4(0, 0, 0, 0).is_zero());
}

/*
 * mat4x4 tests.
 */

// compare non-simd and simd vec4
bool operator==(const ml::vec4& v1, const ml::simd::vec4& v2)
{
    return v1.x == v2.x && v1.y == v2.y && v1.z == v2.z && v1.w == v2.w;
}

// compare non-simd and simd mat4x4
bool operator==(const ml::mat4x4& m1, const ml::simd::mat4x4& m2)
{
    return m1.rows[0] == m2.rows[0] && m1.rows[1] == m2.rows[1] && m1.rows[2] == m2.rows[2] && m1.rows[3] == m2.rows[3];
}

// initialize simd vec4 from non-simd vec4
ml::simd::vec4 vec_simd_init(const ml::vec4& v)
{
    return {v.x, v.y, v.z, v.w};
}

// initialize simd mat4x4 from non-simd mat4x4
ml::simd::mat4x4 mat_simd_init(const ml::mat4x4& m)
{
    return {vec_simd_init(m.rows[0]), vec_simd_init(m.rows[1]), vec_simd_init(m.rows[2]), vec_simd_init(m.rows[3])};
}

BOOST_AUTO_TEST_CASE(mat4x4_multiplication)
{
    ml::mat4x4 m1{
      {1, 2, 3, 4},
      {2, 4, 3, 1},
      {3, 1, 4, 2},
      {4, 2, 1, 3}};
    m1 = m1 * m1;

    ml::simd::mat4x4 m2{
      {1, 2, 3, 4},
      {2, 4, 3, 1},
      {3, 1, 4, 2},
      {4, 2, 1, 3}};
    m2 = m2 * m2;

    BOOST_TEST((m1.rows[0] == m2.rows[0]));
    BOOST_TEST((m1.rows[1] == m2.rows[1]));
    BOOST_TEST((m1.rows[2] == m2.rows[2]));
    BOOST_TEST((m1.rows[3] == m2.rows[3]));

    m1.transpose();
    m2.transpose();

    // for better tracking in case of test failure.
    auto m1_t = m1;
    auto m2_t = m2;
    BOOST_TEST((m1_t.rows[0] == m2_t.rows[0]));
    BOOST_TEST((m1_t.rows[1] == m2_t.rows[1]));
    BOOST_TEST((m1_t.rows[2] == m2_t.rows[2]));
    BOOST_TEST((m1_t.rows[3] == m2_t.rows[3]));
}

template<typename T>
T get_random_vec4()
{
    return {static_cast<float>(rand() % 9 - 5), static_cast<float>(rand() % 9 - 5), static_cast<float>(rand() % 9 - 5), static_cast<float>(rand() % 9 + 1)};
}

template<typename M, typename T>
M get_random_mat()
{
    return {
      get_random_vec4<T>(),
      get_random_vec4<T>(),
      get_random_vec4<T>(),
      get_random_vec4<T>()};
}

BOOST_AUTO_TEST_CASE(mat4x4_randomized_multiplication)
{
    // random multiplication tests.
    for(int i = 0; i < 1000; ++i)
    {
        ml::mat4x4 rm1 = get_random_mat<ml::mat4x4, ml::vec4>();
        ml::mat4x4 rm2 = get_random_mat<ml::mat4x4, ml::vec4>();

        ml::simd::mat4x4 rm1_simd = mat_simd_init(rm1), rm2_simd = mat_simd_init(rm2);

        ml::mat4x4 res = rm1 * rm2;
        ml::simd::mat4x4 res_simd = rm1_simd * rm2_simd;

        BOOST_REQUIRE(res.rows[0] == res_simd.rows[0]);
        BOOST_REQUIRE(res.rows[1] == res_simd.rows[1]);
        BOOST_REQUIRE(res.rows[2] == res_simd.rows[2]);
        BOOST_REQUIRE(res.rows[3] == res_simd.rows[3]);
    }
}

/*
 * math functions.
 */

BOOST_AUTO_TEST_CASE(lerp)
{
    BOOST_TEST(ml::lerp<float>(0.3f, 1.f, 2.f) == static_cast<float>(1.f * (1.f - 0.3f) + 2.f * 0.3f), boost::test_tools::tolerance(1e-8f));
    BOOST_TEST(ml::lerp<double>(0.3, 1, 2) == static_cast<double>(1.0 * (1.0 - 0.3) + 2.0 * 0.3), boost::test_tools::tolerance(1e-8));

    ml::vec4 a{1.1, 2.2, 3.3, 4.4};
    ml::vec4 b{-9.3, -10.4, -11.5, -12.6};
    ml::vec4 l{ml::lerp<ml::vec4>(0.4, a, b)};
    BOOST_TEST(l.x == -3.06f, boost::test_tools::tolerance(1e-6f));
    BOOST_TEST(l.y == -2.84f, boost::test_tools::tolerance(1e-6f));
    BOOST_TEST(l.z == -2.62f, boost::test_tools::tolerance(1e-6f));
    BOOST_TEST(l.w == -2.4f, boost::test_tools::tolerance(1e-6f));

    for(int i = 0; i < 100; ++i)
    {
        float t = static_cast<float>(i) / 100.f;

        l = ml::lerp(t, a, b);
        BOOST_TEST(l.x == ml::lerp(t, a.x, b.x), boost::test_tools::tolerance(1e-5f));
        BOOST_TEST(l.y == ml::lerp(t, a.y, b.y), boost::test_tools::tolerance(1e-5f));
        BOOST_TEST(l.z == ml::lerp(t, a.z, b.z), boost::test_tools::tolerance(1e-5f));
        BOOST_TEST(l.w == ml::lerp(t, a.w, b.w), boost::test_tools::tolerance(1e-5f));
    }
}

BOOST_AUTO_TEST_CASE(radians_degrees)
{
    /* these are just linear transformations.... */

    BOOST_TEST(ml::to_radians(0) == 0);
    BOOST_TEST(ml::to_radians(90) == static_cast<float>(M_PI_2), boost::test_tools::tolerance(1e-9f));
    BOOST_TEST(ml::to_radians(180) == static_cast<float>(M_PI), boost::test_tools::tolerance(1e-9f));
    BOOST_TEST(ml::to_radians(270) == static_cast<float>(3 * M_PI_2), boost::test_tools::tolerance(1e-9f));
    BOOST_TEST(ml::to_radians(360) == static_cast<float>(2 * M_PI), boost::test_tools::tolerance(1e-9f));
    BOOST_TEST(ml::to_radians(-90) == static_cast<float>(-M_PI_2), boost::test_tools::tolerance(1e-9f));
    BOOST_TEST(ml::to_radians(-180) == static_cast<float>(-M_PI), boost::test_tools::tolerance(1e-9f));
    BOOST_TEST(ml::to_radians(-270) == static_cast<float>(-3 * M_PI_2), boost::test_tools::tolerance(1e-9f));
    BOOST_TEST(ml::to_radians(-360) == static_cast<float>(-2 * M_PI), boost::test_tools::tolerance(1e-9f));

    BOOST_TEST(ml::to_degrees(0) == 0);
    BOOST_TEST(ml::to_degrees(static_cast<float>(M_PI_2)) == static_cast<float>(90), boost::test_tools::tolerance(1e-9f));
    BOOST_TEST(ml::to_degrees(static_cast<float>(M_PI)) == static_cast<float>(180), boost::test_tools::tolerance(1e-9f));
    BOOST_TEST(ml::to_degrees(static_cast<float>(3 * M_PI_2)) == static_cast<float>(270), boost::test_tools::tolerance(1e-9f));
    BOOST_TEST(ml::to_degrees(static_cast<float>(2 * M_PI)) == static_cast<float>(360), boost::test_tools::tolerance(1e-9f));
    BOOST_TEST(ml::to_degrees(static_cast<float>(-M_PI_2)) == static_cast<float>(-90), boost::test_tools::tolerance(1e-9f));
    BOOST_TEST(ml::to_degrees(static_cast<float>(-M_PI)) == static_cast<float>(-180), boost::test_tools::tolerance(1e-9f));
    BOOST_TEST(ml::to_degrees(static_cast<float>(-3 * M_PI_2)) == static_cast<float>(-270), boost::test_tools::tolerance(1e-9f));
    BOOST_TEST(ml::to_degrees(static_cast<float>(-2 * M_PI)) == static_cast<float>(-360), boost::test_tools::tolerance(1e-9f));

    for(int i = 0; i < 100; ++i)
    {
        BOOST_TEST(ml::to_radians(ml::to_degrees(static_cast<float>(i))) == static_cast<float>(i), boost::test_tools::tolerance(1e-6f));
        BOOST_TEST(ml::to_degrees(ml::to_radians(static_cast<float>(i))) == static_cast<float>(i), boost::test_tools::tolerance(1e-6f));
    }
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/div.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/two.hpp>

STF_CASE_TPL (" divreal",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using bs::div;
  using r_t = decltype(div(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t,T);

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_IEEE_EQUAL(div(bs::Inf<T>(), bs::Inf<T>(), bs::floor), bs::Nan<r_t>());
  STF_IEEE_EQUAL(div(bs::Minf<T>(), bs::Minf<T>(), bs::floor), bs::Nan<r_t>());
  STF_IEEE_EQUAL(div(bs::Nan<T>(), bs::Nan<T>(), bs::floor), bs::Nan<r_t>());
#endif
  STF_EQUAL(div(T(4),T(0), bs::floor), bs::Inf<r_t>());
  STF_EQUAL(div(T(4),T(3), bs::floor), bs::One<r_t>());
  STF_EQUAL(div(bs::Mone<T>(), bs::Mone<T>(), bs::floor), bs::One<r_t>());
  STF_EQUAL(div(bs::One<T>(), bs::One<T>(), bs::floor), bs::One<r_t>());
  STF_EQUAL(div(bs::Mone<T>(),bs::Zero<T>(), bs::floor), bs::Minf<r_t>());
  STF_EQUAL(div(bs::One<T>(), bs::One<T>(), bs::floor), bs::One<r_t>());
  STF_EQUAL(div(bs::One<T>(),bs::Zero<T>(), bs::floor), bs::Inf<r_t>());
  STF_IEEE_EQUAL(div(bs::Zero<T>(),bs::Zero<T>(), bs::floor), bs::Nan<r_t>());
} // end of test for floating_

STF_CASE_TPL (" divunsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using bs::div;
  using r_t = decltype(div(T(), T()));

  STF_TYPE_IS(r_t,T);

  // specific values tests
  STF_EQUAL(div(T(4),T(0), bs::floor), bs::Valmax<r_t>());
  STF_EQUAL(div(T(4),T(3), bs::floor), T(1));
  STF_EQUAL(div(bs::One<T>(), bs::One<T>(), bs::floor), bs::One<r_t>());
  STF_EQUAL(div(bs::Valmax<T>(),  bs::Two<T>(), bs::floor), bs::Valmax<r_t>()/bs::Two<T>());
} // end of test for unsigned_int_

STF_CASE_TPL (" divsigned_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using bs::div;
  using r_t = decltype(div(T(), T()));

  STF_TYPE_IS(r_t,T);

  // specific values tests
  STF_EQUAL(div(T(-4),T(0), bs::floor), bs::Valmin<r_t>());
  STF_EQUAL(div(T(4),T(0), bs::floor), bs::Valmax<r_t>());
  STF_EQUAL(div(T(4),T(3), bs::floor), T(1));
  STF_EQUAL(div(T(-4),T(-3), bs::floor), T(1));
  STF_EQUAL(div(T(4),T(-3), bs::floor), T(-2));
  STF_EQUAL(div(T(-4),T(3), bs::floor), T(-2));
  STF_EQUAL(div(bs::Mone<T>(), bs::Mone<T>(), bs::floor), bs::One<r_t>());
  STF_EQUAL(div(bs::One<T>(), bs::One<T>(), bs::floor), bs::One<r_t>());
} // end of test for signed_int_