/*
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).
*/

// Zdenek Prusa and Peter L. Soendergaard,
// Real-time spectrogram inversion using phase gradient heap integration
// Proceedings of DAFX 2016

#pragma once

#include "AlgorithmUtils.hpp"
#include "FluidEigenMappings.hpp"
#include "../public/STFT.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <cmath>

namespace fluid {
namespace algorithm {

class RTPGHI
{

public:
  using ArrayXd = Eigen::ArrayXd;
  using ArrayXcd = Eigen::ArrayXcd;

  void init(index fftSize)
  {
    mBins = fftSize / 2 + 1;
    mPrevMag = ArrayXd::Zero(mBins);
    mPrevLogMag = ArrayXd::Zero(mBins);
    mPrevPrevLogMag = ArrayXd::Zero(mBins);
    mPrevPhase = ArrayXd::Zero(mBins);
    mPrevPrevPhase = ArrayXd::Zero(mBins);
    mPrevPhaseDeltaT = ArrayXd::Zero(mBins);
    mBinIndices = ArrayXd::LinSpaced(mBins, 0, mBins - 1);
  }

  void processFrame(RealVectorView in, ComplexVectorView out, index winSize,
                    index fftSize, index hopSize, double tolerance)
  {
    using namespace Eigen;
    using namespace _impl;
    using namespace std;
    using namespace std::complex_literals;
    double  gamma = 0.25645 * pow(winSize, 2); // assumes Hann window
    ArrayXd mag = asEigen<Array>(in);
    ArrayXd logMag = mag.max(epsilon).log();
    ArrayXd futureLogMag = logMag;
    ArrayXd currentLogMag = mPrevLogMag;
    ArrayXd prevLogMag = mPrevPrevLogMag;

    ArrayXd phaseDeltaT =
        getPhaseDeltaT(currentLogMag, gamma, fftSize, hopSize);
    ArrayXd phaseDeltaF =
        getPhaseDeltaF(prevLogMag, futureLogMag, gamma, fftSize, hopSize);
    double absTol =
        log(tolerance) + max(currentLogMag.maxCoeff(), prevLogMag.maxCoeff());
    ArrayXd todo = (currentLogMag > absTol).cast<double>();
    index   numTodo = todo.sum();
    ArrayXd phaseEst = pi + ArrayXd::Random(mBins) * pi;

    vector<pair<double, index>> heap;

    for (index i = 0; i < mBins; i++)
    {
      if (prevLogMag(i) > absTol) heap.push_back({prevLogMag(i), i});
    }
    make_heap(heap.begin(), heap.end());

    while (numTodo > 0 && heap.size() > 0)
    {
      pop_heap(heap.begin(), heap.end());
      index _m = heap.back().second;
      heap.pop_back();

      // use indices 0..mBins for prev frame
      // mBins ... 2 * mBins  for current frame
      if (_m < mBins && todo[_m] > 0)
      {
        index m = _m;
        phaseEst[m] =
            mPrevPhase[m] + 0.5 * (phaseDeltaT[m] + mPrevPhaseDeltaT[m]);
        heap.push_back({currentLogMag[m], m + mBins});
        push_heap(heap.begin(), heap.end());
        todo[m] = 0;
        numTodo--;
      }
      else if (_m >= mBins)
      {
        index m = _m - mBins;
        if (m < mBins - 1 && todo[m + 1] > 0)
        {
          phaseEst[m + 1] =
              phaseEst[m] + 0.5 * (phaseDeltaF[m] + phaseDeltaF[m + 1]);
          heap.push_back({currentLogMag[m + 1], _m + 1});
          push_heap(heap.begin(), heap.end());
          todo[m + 1] = 0;
          numTodo--;
        }
        if (m > 0 && todo[m - 1] > 0)
        {
          phaseEst[m - 1] =
              phaseEst[m] - 0.5 * (phaseDeltaF[m] + phaseDeltaF[m - 1]);
          heap.push_back({currentLogMag[m - 1], _m - 1});
          push_heap(heap.begin(), heap.end());
          todo[m - 1] = 0;
          numTodo--;
        }
      }
    }

    ArrayXd finalPhase =
        phaseEst - ArrayXd::LinSpaced(mBins, 0, 1) * pi * (winSize - 1) / 2;
    ArrayXcd result = mPrevMag * (1i * finalPhase).exp();
    mPrevPrevLogMag = mPrevLogMag;
    mPrevLogMag = logMag;
    mPrevPhase = phaseEst;
    mPrevPhaseDeltaT = phaseDeltaT;
    mPrevMag = mag;
    out = asFluid(result);
  }

private:
  Eigen::ArrayXd getPhaseDeltaT(Eigen::Ref<ArrayXd> logMag, double gamma,
                                index fftSize, index hopSize)
  {
    ArrayXd deltaT = ArrayXd::Zero(mBins);
    deltaT.segment(1, mBins - 2) =
        logMag.segment(2, mBins - 2) - logMag.segment(0, mBins - 2);
    deltaT = deltaT * 0.5 * hopSize * fftSize / gamma;
    deltaT = deltaT + twoPi * hopSize * mBinIndices / fftSize;
    return deltaT;
  }

  Eigen::ArrayXd getPhaseDeltaF(ArrayXd prevLogMag, ArrayXd nextLogMag,
                                double gamma, index fftSize, index hopSize)
  {
    return 0.5 * (nextLogMag - prevLogMag) * (-gamma / (hopSize * fftSize));
  }

  index   mBins;
  ArrayXd mBinIndices;
  ArrayXd mPrevMag;
  ArrayXd mPrevLogMag;
  ArrayXd mPrevPrevLogMag;
  ArrayXd mPrevPhase;
  ArrayXd mPrevPhaseDeltaT;
  ArrayXd mPrevPrevPhase;
};
} // namespace algorithm
} // namespace fluid


// Filename: eigenvalue_example.cpp (part of MTL4)

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace mtl;

    dense_vector<double>                    eig;

    double array[][4]= {{1,  1,   1,  0},
                        {1, -1,  -2,  0},
                        {1, -2,   1,  0},
                        {0,  0,   0, 10}};
    dense2D<double> A(array);
    std::cout << "A=\n" << A << "\n";

    eig= eigenvalue_symmetric(A,22);
    std::cout<<"eigenvalues  ="<< eig <<"\n";

    eig= 0;
    eig= qr_sym_imp(A);
    std::cout<<"eigenvalues  ="<< eig <<"\n";

    eig= 0;
    eig= qr_algo(A, 5);  // only 5 qr iterations (Q-R-changes)
    std::cout<<"eigenvalues  ="<< eig <<"\n";

    return 0;
}


#include "zmpUtil.h"
#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>

using namespace Eigen;
using namespace std;

MatrixXd expm(const MatrixXd& A) {
  MatrixXd F;
  MatrixExponential<MatrixXd>(A).compute(F);
  return F;
}

ExponentialPlusPiecewisePolynomial<double> s1Trajectory(const TVLQRData &sys, const PiecewisePolynomial<double> &zmp_trajectory,const Ref<const MatrixXd> &S) {
  size_t n = static_cast<size_t>(zmp_trajectory.getNumberOfSegments());
  int d = zmp_trajectory.getSegmentPolynomialDegree(0);
  int k = d + 1;

  for (size_t i = 1; i < n; i++) {
    assert(zmp_trajectory.getSegmentPolynomialDegree(i) == d);
  }

  VectorXd dt(n);
  std::vector<double> breaks = zmp_trajectory.getSegmentTimes();

  for (size_t i = 0; i < n; i++) {
    dt(i) = breaks[i + 1] - breaks[i];
  }

  MatrixXd zmp_tf = zmp_trajectory.value(zmp_trajectory.getEndTime());
  PiecewisePolynomial<double> zbar_pp = zmp_trajectory - zmp_tf;

  Matrix2d R1i = sys.R1.inverse();
  MatrixXd NB = sys.N.transpose() + sys.B.transpose() * S; //2 x 4
  Matrix4d A2 = NB.transpose() * R1i * sys.B.transpose() - sys.A.transpose();
  MatrixXd B2 = 2 * (sys.C.transpose() - NB.transpose() * R1i * sys.D) * sys.Qy; //4 x 2
  Matrix4d A2i = A2.inverse();

  MatrixXd alpha = MatrixXd::Zero(4, n);

  vector<MatrixXd> beta;
  VectorXd s1dt;

  for (size_t i = 0; i < n ; i++) {
    beta.push_back(MatrixXd::Zero(4, k));
  }

  for (int j = static_cast<int>(n) - 1; j >= 0; j--) {

    auto poly_mat = zbar_pp.getPolynomialMatrix(j);
    size_t nq = poly_mat.rows();
    MatrixXd poly_coeffs = MatrixXd::Zero(nq, k);

    for (size_t x = 0; x < nq; x++) {
      poly_coeffs.row(x) = poly_mat(x).getCoefficients().transpose();
    }

    beta[j].col(k - 1) = -A2i * B2 * poly_coeffs.col(k - 1);

    for (int i = k - 2; i >= 0; i--) {
      beta[j].col(i) = A2i * ((i+1) * beta[j].col(i + 1) - B2 * poly_coeffs.col(i));
    }

    if (j == n - 1) {
      s1dt = VectorXd::Zero(4);
    } else {
      s1dt = alpha.col(j+1) + beta[j + 1].col(0);
    }

    VectorXd dtpow(k);
    for (size_t p = 0; p < k; p++) {
      dtpow(p) = pow(dt(j), static_cast<int>(p));
    }

    alpha.col(j) = expm(A2*dt(j)).inverse() * (s1dt - beta[j]*dtpow);
  }

  vector<PiecewisePolynomial<double>::PolynomialMatrix> polynomial_matrices;
  for (int segment = 0; segment < n ; segment++) {
    PiecewisePolynomial<double>::PolynomialMatrix polynomial_matrix(4, 1);
    for(int row = 0; row < 4; row++) {
      polynomial_matrix(row) = Polynomial<double>(beta[segment].row(row));
    }
    polynomial_matrices.push_back(polynomial_matrix);
  }

  PiecewisePolynomial<double> pp_part = PiecewisePolynomial<double>(polynomial_matrices, breaks);
  auto s1traj = ExponentialPlusPiecewisePolynomial<double>(Matrix4d::Identity(), A2, alpha, pp_part);
  return s1traj;
}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_random.h>
#include <OpenTissue/core/math/big/big_prod_row.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

BOOST_AUTO_TEST_SUITE(opentissue_math_big_prod_row);

BOOST_AUTO_TEST_CASE(random_test_case)
{
  typedef ublas::compressed_matrix<double> matrix_type;
  typedef ublas::vector<double>            vector_type;

  matrix_type A;
  vector_type x;
  vector_type y;
  vector_type tst;

  A.resize(10,10,false);
  x.resize(10,false);
  y.resize(10,false);
  tst.resize(10,false);

  OpenTissue::math::Random<double> value(0.0,1.0);

  for(size_t iteration = 0;iteration<100;++iteration)
  {
    for(size_t i=0;i<A.size1();++i)
    {
      x(i) = value();
      y(i) = value();
      for(size_t j=0;j<A.size2();++j)
        A(i,j) = value();
    }
    ublas::noalias(tst) = ublas::prod(A,x);
    double tol = 0.01;
    for(size_t i=0;i<A.size1();++i)
    {
      y(i) = OpenTissue::math::big::prod_row( A,x,i);
      BOOST_CHECK_CLOSE( double( y(i) ), double( tst(i) ), tol );
    }
  }
}

BOOST_AUTO_TEST_SUITE_END();


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

TEST(MathFunctions, acosh) {
  using stan::math::acosh;
  EXPECT_FLOAT_EQ(0, acosh(1));
  EXPECT_FLOAT_EQ(0.96242365, acosh(1.5));
  EXPECT_FLOAT_EQ(3.0797991, acosh(10.9));
}

TEST(MathFunctions, acosh_exception) {
  using stan::math::acosh;
  EXPECT_THROW(acosh(0.5), std::domain_error);
}

TEST(MathFunctions, acosh_inf_return) {
  EXPECT_EQ(std::numeric_limits<double>::infinity(),
            stan::math::acosh(std::numeric_limits<double>::infinity()));
}

TEST(MathFunctions, acosh_nan) {
  using stan::math::acosh;
  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::acosh(std::numeric_limits<double>::quiet_NaN()));
}


#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include<Eigen/IterativeLinearSolvers>
#include <Eigen/SparseCholesky>
#include<Eigen/SparseQR>
#include <Eigen/OrderingMethods>

#include <chrono>
#include <time.h>

#include "parameters.h"
#include "Set_2DAV_diags.h"

using Eigen::VectorXd;
using Eigen::MatrixXd;
using Eigen::SparseMatrix;
using Eigen::BiCGSTAB;
using Eigen::MatrixXd;
using Eigen::SimplicialCholeskyLDLT;

typedef Eigen::Triplet<double> Trp;  //TrpripleTrps are objecTrp s of form: row index, column index, value.  used to fill sparse matrices

int main()
{

    std::chrono::high_resolution_clock::time_point start = std::chrono::high_resolution_clock::now();  //start clock timer

    VectorXd x(num_elements), b(num_elements);
    SparseMatrix<double> A(num_elements,num_elements);
    //The creation of these 2 matrices is super slow!So
    MatrixXd epsilon = MatrixXd::Ones(num_cell+1,num_cell+1);  //NOTE: this is num_cell+1*num_cell+1 (NOT num_elements^2), 1 element per each physical grid pt. make matrix of ones
    MatrixXd netcharge = MatrixXd::Zero(num_cell+1,num_cell+1);  //make matrix of zeroes

    std::vector<double> main_diag (num_elements+1);
    std::vector<double> upper_diag(num_elements);
    std::vector<double> lower_diag(num_elements);
    std::vector<double> far_lower_diag(num_elements-N+1);
    std::vector<double> far_upper_diag(num_elements-N+1);

    std::vector<double> V(num_elements+1); //vector for solution, electric potential
    std::vector<double> rhs(num_elements+1);

    //fill A and b
    //Define boundary conditions and initial conditions
    double Va = 1.;
    double V_bottomBC = 0.;
    double V_topBC= Va/Vt;

    //Initial conditions
    double diff = (V_topBC - V_bottomBC)/num_cell;
    int index = 0;
    for (int j = 1;j<=N;j++){//  %corresponds to z coord
        index++;
        V[index] = diff*j;
        for (int i = 2;i<=N;i++){//  %elements along the x direction assumed to have same V
            index++;
            V[index] = V[index-1];
        }
    }

    //side BCs, insulating BC's
    std::vector<double> V_leftBC(N+1);
    std::vector<double> V_rightBC(N+1);

    for(int i = 1;i<=N;i++){
        V_leftBC[i] = V[(i-1)*N +  1];
        V_rightBC[i] = V[i*N];
    }

    //Set up matrix equation
    main_diag = set_main_Vdiag(epsilon, main_diag);
    upper_diag = set_upper_Vdiag(epsilon, upper_diag);
    lower_diag = set_lower_Vdiag(epsilon, lower_diag);
    far_upper_diag = set_far_upper_Vdiag(epsilon, far_upper_diag);
    far_lower_diag = set_far_lower_Vdiag(epsilon, far_lower_diag);

    //setup rhs of Poisson eqn.
    int index2 = 0;
    for(int j = 1;j<=N;j++){
        if(j==1){
            for(int i = 1;i<=N;i++){
                index2++;                //THIS COULD BE MODIFIES TO a switch ,case statement--> might be cleaner
                if(i==1){
                    rhs[index2] = netcharge(i,j) + epsilon(i,j)*(V_leftBC[1] + V_bottomBC);
                }else if(i == N)
                    rhs[index2] = netcharge(i,j) + epsilon(i,j)*(V_rightBC[1] + V_bottomBC);
                else
                    rhs[index2] = netcharge(i,j) + epsilon(i,j)*V_bottomBC;
            }
        }else if(j==N){
            for(int i = 1; i<=N;i++){
                index2++;
                if(i==1)
                    rhs[index2] = netcharge(i,j) + epsilon(i,j)*(V_leftBC[N] + V_topBC);
                else if(i == N)
                    rhs[index2] = netcharge(i,j) + epsilon(i,j)*(V_rightBC[N] + V_topBC);
                else
                    rhs[index2] = netcharge(i,j) + epsilon(i,j)*V_topBC;
            }
        }else{
            for(int i = 1;i<=N;i++){
                index2++;
                if(i==1)
                    rhs[index2] = netcharge(i,j) + epsilon(i,j)*V_leftBC[j];
                else if(i == N)
                    rhs[index2] = netcharge(i,j) + epsilon(i,j)*V_rightBC[j];
                else
                    rhs[index2] = netcharge(i,j);
            }
        }
    }

   //setup the triplet list for sparse matrix

    std::vector<Trp> triplet_list(5*num_elements);   //approximate the size that need         // list of non-zeros coefficients in triplet form(row index, column index, value)
    int trp_cnt = 0;
    for(int i = 1; i< main_diag.size(); i++){
        b(i-1) = rhs[i];   //fill VectorXd  rhs of the equation
          triplet_list[trp_cnt] = {i-1, i-1, main_diag[i]};
          trp_cnt++;
        //triplet_list.push_back(Trp(i-1,i-1,main_diag[i]));;   //triplets for main diag
    }
    for(int i = 1;i< upper_diag.size();i++){
        triplet_list[trp_cnt] = {i-1, i, upper_diag[i]};
        trp_cnt++;
        //triplet_list.push_back(Trp(i-1, i, upper_diag[i]));  //triplets for upper diagonal
     }
     for(int i = 1;i< lower_diag.size();i++){
         triplet_list[trp_cnt] = {i, i-1, lower_diag[i]};
         trp_cnt++;
        // triplet_list.push_back(Trp(i, i-1, lower_diag[i]));
     }
     for(int i = 1;i< far_upper_diag.size();i++){
         triplet_list[trp_cnt] = {i-1, i-1+N, far_upper_diag[i]};
         trp_cnt++;
         //triplet_list.push_back(Trp(i-1, i-1+N, far_upper_diag[i]));
         triplet_list[trp_cnt] = {i-1+N, i-1, far_lower_diag[i]};
         trp_cnt++;
         //triplet_list.push_back(Trp(i-1+N, i-1, far_lower_diag[i]));
      }

    A.setFromTriplets(triplet_list.begin(), triplet_list.end());    //A is our sparse matrix

    //using conjugate gradient--> this is faster than BiCGSTAB
/*
    Eigen::ConjugateGradient<SparseMatrix<double>, Eigen::UpLoType::Lower|Eigen::UpLoType::Upper > cg;
    cg.compute(A);
   x = cg.solve(b);
    std::cout << "#iterations:     " << cg.iterations() << std::endl;
     std::cout << "estimated error: " << cg.error()      << std::endl;

*/

  /*  //using sparse LU
     Eigen::SparseLU<SparseMatrix<double>>  sLU;
    sLU.analyzePattern(A);
    sLU.factorize(A);
    x = sLU.solve(b);
    */

   //using Sparse Cholesky  (THIS SEEMS TO BE THE FASTEST for the 2D Poisson problem)
    Eigen::SimplicialLDLT<SparseMatrix<double>, Eigen::UpLoType::Lower, Eigen::AMDOrdering<int>> SCholesky; //Note using NaturalOrdering is much much slower
    SCholesky.analyzePattern(A);
    SCholesky.factorize(A);
    x = SCholesky.solve(b);

    //NOTE, for repeat solves, I might not need to keep factorizing! Can just subtitute in the new rhs every time, and use solve()

    /*
    //using SparseQR  (is very slow, compared to others for large matrices).
    Eigen::SparseQR<SparseMatrix<double>, Eigen::COLAMDOrdering<int>> SQR;  //NOTE: this ordering parameter is needed for this to work
    SQR.analyzePattern(A);
    SQR.factorize(A);
    x = SQR.solve(b);
    */

    //using BiCGSTAB: this is probably useful for non-symmetric matrices, i.e. not Poisson equation which is symmetric. In that case, Cholesky might not work.

    BiCGSTAB<SparseMatrix<double>, Eigen::IncompleteLUT<double>> solver;  //BiCGStab solver object
    solver.compute(A);  //this computes the preconditioner
    x = solver.solve(b);
    std::cout << "#iterations:     " << solver.iterations() << std::endl;
    std::cout << "estimated error: " << solver.error()      << std::endl;

    std::cout << x <<std::endl;  //they have an overloaded << for seeing result!

    std::chrono::high_resolution_clock::time_point finish = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> time = std::chrono::duration_cast<std::chrono::duration<double>>(finish-start);
    std::cout << "1 Va CPU time = " << time.count() << std::endl;

    return 0;
}


/**
 * @file semimprk.cc
 * @brief NPDE homework SemImpRK code
 * @author Unknown, Oliver Rietmann
 * @date 04.04.2021
 * @copyright Developed at ETH Zurich
 */

#include "semimprk.h"

#include <Eigen/Core>
#include <algorithm>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <vector>

#include "../../../lecturecodes/helperfiles/polyfit.h"

namespace SemImpRK {

/* SAM_LISTING_BEGIN_0 */
double CvgRosenbrock() {
  double cvgRate = 0.0;
  // Use polyfit to estimate the rate of convergence
  // for SolveRosenbrock.
#if SOLUTION
  // Final time
  const double T = 10.0;
  // Timestep sizes h=2^{-k} for which discretization erorrs should be computed
  const Eigen::ArrayXd K = Eigen::ArrayXd::LinSpaced(7, 4, 10);
  // Initial state vector
  Eigen::Vector2d y0(1., 1.);

  // Parameter and useful matrix for f
  const double lambda = 1;
  Eigen::Matrix2d R;
  R << 0.0, -1.0, 1.0, 0.0;
  // Lambda functions giving the right-hand side function f
  // and its Jacobian df
  auto f = [&R, &lambda](Eigen::Vector2d y) {
    return R * y + lambda * (1.0 - y.squaredNorm()) * y;
  };
  auto df = [&lambda](Eigen::Vector2d y) {
    double x = 1 - y.squaredNorm();
    Eigen::Matrix2d J;
    J << lambda * x - 2 * lambda * y(0) * y(0), -1 - 2 * lambda * y(1) * y(0),
        1 - 2 * lambda * y(1) * y(0), lambda * x - 2 * lambda * y(1) * y(1);
    return J;
  };

  // timestep size h=2^{-12} (=> M = T*2^{12}) used to compute a reference
  // solution
  const int M_ref = T * std::pow(2, 12);
  // Reference solution
  std::vector<Eigen::VectorXd> solref = SolveRosenbrock(f, df, y0, M_ref, T);
  // Arrays for recording error norms and numbers of timesteps
  Eigen::ArrayXd Error(K.size());
  Eigen::ArrayXd M(K.size());
  // Loop over all temporal meshes
  for (unsigned int i = 0; i < K.size(); ++i) {
    // h = 2^{-k} => M = T*h = T*2^k
    M(i) = T * std::pow(2, K[i]);

    // Compute solution
    std::vector<Eigen::VectorXd> sol = SolveRosenbrock(f, df, y0, M(i), T);

    // Compute error
    double maxerr = 0;
    for (unsigned int j = 0; j < sol.size(); ++j) {
      maxerr = std::max(maxerr, (sol[j] - solref[j * M_ref / M(i)]).norm());
    }
    Error(i) = maxerr;
  }
  // Print Error table
  std::cout << std::setw(15) << "M" << std::setw(16) << "maxerr \n";
  for (int i = 0; i < M.size(); ++i) {
    std::cout << std::setw(15) << M(i) << std::setw(15) << Error(i)
              << std::endl;
  }
  // Estimate convergence rate using linear regression provided by the auxiliary
  // function polyfit()
  cvgRate = -polyfit(M.log(), Error.log(), 1)(0);
#else
  //====================
  // Your code goes here
  //====================
#endif
  return cvgRate;
}
/* SAM_LISTING_END_0 */

}  // namespace SemImpRK


// inverting A
// using getrf() & getri()

//#define BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <cstddef>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/atlas/cblas3.hpp>
#include <boost/numeric/bindings/atlas/clapack.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/std_vector.hpp>
#include "utils.h"

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

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

typedef std::complex<double> cmpx;

#ifndef F_ROW_MAJOR
typedef ublas::matrix<double, ublas::column_major> m_t;
typedef ublas::matrix<cmpx, ublas::column_major> cm_t;
#else
typedef ublas::matrix<double, ublas::row_major> m_t;
typedef ublas::matrix<cmpx, ublas::row_major> cm_t;
#endif

int main() {

  cout << endl;
  cout << "real matrix:" << endl << endl;

  size_t n = 5;
  m_t a (n, n);
  init_symm (a);
  //     [n   n-1 n-2  ... 1]
  //     [n-1 n   n-1  ... 2]
  // a = [n-2 n-1 n    ... 3]
  //     [        ...       ]
  //     [1   2   ...  n-1 n]
  print_m (a, "A");
  cout << endl;

  m_t aa (a);  // copy of a, for later use

  std::vector<int> ipiv (n);   // pivot vector
  atlas::lu_factor (a, ipiv);  // alias for getrf()
  atlas::lu_invert (a, ipiv);  // alias for getri()

  m_t i1 (n, n), i2 (n, n);
  atlas::gemm (a, aa, i1);   // i1 should be (almost) identity matrix
  atlas::gemm (aa, a, i2);   // i2 should be (almost) identity matrix

  print_m (i1, "I = A^(-1) * A");
  cout << endl;
  print_m (i2, "I = A * A^(-1)");
  cout << endl;

  cout << endl;

  //////////////////////////////////////////////////////

  cout << "complex matrix:" << endl << endl;
  cm_t ca (3, 3);

  ca (0, 0) = cmpx (3, 0);
  ca (0, 1) = cmpx (4, 2);
  ca (0, 2) = cmpx (-7, 5);
  ca (1, 0) = cmpx (4, -2);
  ca (1, 1) = cmpx (-5, 0);
  ca (1, 2) = cmpx (0, -3);
  ca (2, 0) = cmpx (-7, -5);
  ca (2, 1) = cmpx (0, 3);
  ca (2, 2) = cmpx (2, 0);
  print_m (ca, "CA");
  cout << endl;

  cm_t caa (ca);

  std::vector<int> ipiv2 (3);

  int ierr = atlas::getrf (ca, ipiv2);
  if (ierr == 0) {
    atlas::getri (ca, ipiv2);
    cm_t ii (3, 3);
    atlas::gemm (ca, caa, ii);
    print_m (ii, "I = CA^(-1) * CA");
    cout << endl;
    atlas::gemm (caa, ca, ii);
    print_m (ii, "I = CA * CA^(-1)");
    cout << endl;
  }
  else
    cout << "matrix is singular" << endl;

  cout << endl;

}


#ifndef __CAD_UTIL_HH__
#define __CAD_UTIL_HH__
/********************************* TRICK HEADER *******************************
PURPOSE:
      (CAD Utility functions from Rocket6G)
LIBRARY DEPENDENCY:
      ((../src/cad_utility.cpp))
*******************************************************************************/
#include <tuple>
#include "math_utility.hh"
#include <armadillo>

namespace cad {
    /**
     * @return great circle distance between two point on a spherical Earth
     * @brief Refrence: Bate et al. "Fundamentals of Astrodynamics", Dover 1971, p. 310
     *
     * @param[in] lon1 longitude of first  point - rad
     * @param[in] lat1 latitude  of first  point - rad
     * @param[in] lon2 longitude of second point - rad
     * @param[in] lat2 latitude  of second point - rad
     *
     * @author 030414 Created from FORTRAN by Peter H Zipfel
     */

    double distance(const double &lon1,
                    const double &lat1,
                    const double &lon2,
                    const double &lat2);

    /**
     * @brief Calculates geodetic longitude, latitude, and altitude from inertial
     *        displacement vector
     *        using the WGS 84 reference ellipsoid
     *        Reference: Britting,K.R."Inertial Navigation Systems Analysis", Wiley. 1971
     *
     * @return std::tuple(lon, lat, alt)
     *                    lon geodetic longitude - rad
     *                    lat geodetic latitude - rad
     *                    alt altitude above ellipsoid - m
     *
     * @param[in] SBII(3x1) = Inertial position - m
     *
     * @author 030414 Created from FORTRAN by Peter H Zipfel
     * @author 170121 Create Armadillo Version by soncyang
     */
    std::tuple<double, double, double> geo84_in(arma::vec3 SBII,
                                                arma::mat33 TEI);

    /**
     * @return geodetic velocity vector information from inertial postion and
     * velocity
     * using the WGS 84 reference ellipsoid
     *
     * @brief Calls utilities
     *    geo84_in(...), tdi84(...)
     *
     * @param[out] dvbe geodetic velocity - m/s
     * @param[out] psivdx geodetic heading angle - deg
     * @param[out] thtvdx geodetic flight path angle - deg
     *
     * @param[in] SBII(3x1) Inertial position - m
     * @param[in] VBII(3x1) Inertial velocity - m
     *
     * @author 040710 created by Peter H Zipfel
     */
    std::tuple<double, double, double> geo84vel_in(arma::vec3 SBII,
                                                   arma::vec3 VBII,
                                                   arma::mat33 TEI);
    /**
     * @return geocentric lon, lat, alt from inertial displacement vector
     * for spherical Earth
     *
     * @param[out] lonc = geocentric longitude - rad
     * @param[out] latc = geocentric latitude - rad
     * @param[out] altc = geocentric latitude - m
     *
     * @param[in] SBII = Inertial position - m
     * @param[in] time = simulation time - sec
     *
     * @author 010628 Created by Peter H Zipfel
     * @author 030416 Modified for SBII (not SBIE) input, PZi
     */
    std::tuple<double, double, double> geoc_in(arma::vec3 SBII,
                                               const double &time);

    /**
     * @return lon, lat, alt from displacement vector in Earth coord for spherical
     * earth
     * RETURN[0]=lon
     * RETURN[1]=lat
     * RETURN[2]=alt
     *
     * @param[in] SBIE = displacement of vehicle wrt Earth center in Earth coordinates
     *
     * @author 010628 Created by Peter H Zipfel
     */
    std::tuple<double, double, double>  geoc_ine(arma::vec3 SBIE);

    /**
     * @brief Earth gravitational acceleration, using the WGS 84 ellipsoid
     *      Ref: Chatfield, A.B.,"Fundamentals of High Accuracy Inertial
     *      Navigation",p.10, Prog.Astro and Aeronautics, Vol 174, AIAA, 1997.
     *
     * @return GRAVG(3x1) = gravitational acceleration in geocentric coord - m/s^2
     *
     * @param[in] SBII = inertial displacement vector - m
     * @param[in] time = simulation time - sec
     *
     * @author 030417 Created from FORTRAN by Peter H Zipfel
     */
    arma::vec3 grav84(arma::vec3 SBII, const double &time);

    /**
     * @brief Returns the inertial displacement vector from longitude, latitude and
     *      altitude
     *      using the WGS 84 reference ellipsoid
     *      Reference: Britting,K.R."Inertial Navigation Systems Analysis"
     *      pp.45-49, Wiley, 1971
     *
     * @return SBII(3x1) = Inertial vehicle position - m
     *
     * @param[in] lon = geodetic longitude - rad
     * @param[in] lat = geodetic latitude - rad
     * @param[in] alt = altitude above ellipsoid - m
     * @param[in] time = simulation time - sec
     *
     * @author 030411 Created from FORTRAN by Peter H Zipfel
     * @author 170121 Create Armadillo Version by soncyanga
     */
    arma::vec3 in_geo84(const double lon,
                        const double lat,
                        const double alt,
                        arma::mat33 TEI);

    /**
     * @brief Returns the inertial displacement vector from geocentric longitude, latitude
     *      and altitude
     *      for spherical Earth
     *
     * @return SBII = position of vehicle wrt center of Earth, in inertial coord
     *
     * @param[in] lon = geographic longitude - rad
     * @param[in] lat = geocentric latitude - rad
     * @param[in] alt = altitude above spherical Earth = m
     *
     * @author 010405 Created by Peter H Zipfel
     */
    arma::vec3 in_geoc(const double &lon,
                       const double &lat,
                       const double &alt,
                       const double &time);

    /**
     * @brief Calculates inertial displacement and velocity vectors from orbital elements
     *      Reference: Bate et al. "Fundamentals of Astrodynamics", Dover 1971, p.71
     *
     * @return parabola_flag = 0 ok
     *                       = 1 not suitable (divide by zero), because parabolic
     *                           trajectory
     *
     * @param[out] SBII = Inertial position - m
     * @param[out] SBII = Inertial velocity - m/s
     *
     * @param[in] semi = semi-major axis of orbital ellipsoid - m
     * @param[in] ecc = eccentricity of elliptical orbit - ND
     * @param[in] inclx = inclination of orbital wrt equatorial plane - deg
     * @param[in] lon_anodex = celestial longitude of the ascending node - deg
     * @param[in] arg_perix = argument of periapsis (ascending node to periapsis) - deg
     * @param[in] true_anomx = true anomaly (periapsis to satellite) - deg
     *
     * @author 040510 Created by Peter H Zipfel
     */
    int in_orb(arma::vec3 &SBII,
               arma::vec3 &VBII,
               const double &semi,
               const double &ecc,
               const double &inclx,
               const double &lon_anodex,
               const double &arg_perix,
               const double &true_anomx);

    /**
     * @brief Projects initial state through 'tgo' to final state along a Keplerian
     *      trajectory
     *      Based on Ray Morth, unpublished utility
     *
     * @return kepler_flan = 0: good Kepler projection;
     *                     = 1: bad (# of iterations>20, or neg. sqrt), no new proj cal,
     *                          use prev value;  - ND
     * @param[out] SPII = projected inertial position after tgo - m
     * @param[out] VPII = projected inertial velocity after tgo - m/s
     *
     * @param[in] SBII = current inertial position - m
     * @param[in] VBII = current inertial velocity - m/s
     * @param[in] tgo = time-to-go to projected point - sec
     *
     * @author 040319 Created from FORTRAN by Peter H Zipfel
     */
    int kepler(arma::vec3 &SPII,
               arma::vec3 &VPII,
               arma::vec3 SBII,
               arma::vec3 VBII,
               const double &tgo);

    /**
     * @brief Projects initial state through 'tgo' to final state along a Keplerian
     *      trajectory
     *      Based on: Bate, Mueller, White, "Fundamentals of Astrodynamics", Dover 1971
     *
     * @return iter_flan = 0: # of iterations < 20;
     *                   = 1: # of iterations > 20;  - ND
     *
     * @param[out] SPII = projected inertial position after tgo - m
     * @param[out] VPII = projected inertial velocity after tgo - m/s
     *
     * @param[in] SBII = current inertial position - m
     * @param[in] VBII = current inertial velocity - m/s
     * @param[in] tgo = time-to-go to projected point - sec
     *
     * @author 040318 Created from ASTRO_KEP by Peter H Zipfel
     */
    int kepler1(arma::vec3 &SPII,
                arma::vec3 &VPII,
                arma::vec3 SBII,
                arma::vec3 VBII,
                const double &tgo);

    /**
     * @brief Calculates utility functions c(z) and s(z) for kepler(...)
     *      Reference: Bate, Mueller, White, "Fundamentals of Astrodynamics", Dover 1971,
     *      p.196
     *
     * @param[out] c = c(z) utility function
     * @param[out] s = s(z) utility function
     *
     * @param[in] z = z-variable
     *
     * @author 040318 Created from ASTRO_UCS by Peter H Zipfel
     */
    std::tuple<double, double> kepler1_ucs(const double &z);

    /**
     * @brief Calculates the orbital elements from inertial displacement and velocity
     *      Reference: Bate et al. "Fundamentals of Astrodynamics", Dover 1971, p.58
     *
     * @return cadorbin_flag = 0 ok
     *                         1 'true_anomx' not calculated, because of circular orbit
     *                         2 'semi' not calculated, because parabolic orbit
     *                         3 'lon_anodex' not calculated, because equatorial orbit
     *                         13 'arg_perix' not calculated, because equatorialand/or
     *                            circular orbit
     * @param[out] semi = semi-major axis of orbital ellipsoid - m
     * @param[out] ecc = eccentricity of elliptical orbit - ND
     * @param[out] inclx = inclination of orbital wrt equatorial plane - deg
     * @param[out] lon_anodex = celestial longitude of the ascending node - deg
     * @param[out] arg_perix = argument of periapsis (ascending node to periapsis) - deg
     * @param[out] true_anomx = true anomaly (periapsis to satellite) - deg
     *
     * @param[in] SBII = Inertial position - m
     * @param[in] VBII = Inertial velocity - m/s
     *
     * @author 040510 Created by Peter H Zipfel
     * @author 170121 Create Armadillo Version by soncyang
     */
    int orb_in(double &semi,
                       double &ecc,
                       double &inclx,
                       double &lon_anodex,
                       double &arg_perix,
                       double &true_anomx,
                       arma::vec3 &SBII,
                       arma::vec3 &VBII);

    /**
     * @brief Returns the T.M. of geodetic wrt inertial coordinates
     *        using the WGS 84 reference ellipsoid
     *
     * @return TDI(3x3) = T.M.of geosetic wrt inertial coord - ND
     *
     * @param[out] lon = geodetic longitude - rad
     * @param[out] lat = geodetic latitude - rad
     * @param[out] alt = altitude above ellipsoid - m
     *
     * @author 030424 Created by Peter H Zipfel
     * @author 170121 Create Armadillo Version by sonicyang
     */
    arma::mat33 tdi84(const double &lon,
                               const double &lat,
                               const double &alt,
                               arma::mat33 TEI);

    arma::mat33 tde84(const double &lon,
                               const double &lat,
                               const double &alt);

    /**
     * @brief Returns the T.M. of earth wrt inertial coordinates
     *
     * @return TEI = T.M. of Earthy wrt inertial coordinates
     *
     * @param[in] time = since start of simulation - s
     *
     * @author 010628 Created by Peter H Zipfel
     */
    arma::mat33 tei(const double &time);

    /**
     * @return TGE = the T.M. of geographic wrt earth coordinates, TGE
     *               spherical Earth only
     *
     * @param[in] lon = geographic longitude - rad
     * @param[in] lat = geographic latitude - rad
     *
     * @author 010628 Created by Peter H Zipfel
     */
    arma::mat33 tge(const double &lon, const double &lat);

    /**
     * @brief Returns the T.M. of geographic (geocentric) wrt inertial
     *        using the WGS 84 reference ellipsoid
     *        Reference: Britting,K.R."Inertial Navigation Systems Analysis",
     *        pp.45-49, Wiley, 1971
     *
     * @return TGI(3x3) = T.M.of geographic wrt inertial coord - ND
     *
     * @param[in] lon = geodetic longitude - rad
     * @param[in] lat = geodetic latitude - rad
     * @param[in] alt = altitude above ellipsoid - m
     *
     * @author 030414 Created from FORTRAN by Peter H Zipfel
     * @author 170121 Create Armadillo Version by soncyang
     */
    arma::mat33 tgi84(const double &lon,
                      const double &lat,
                      const double &alt,
                      arma::mat33 TEI);

    /**
     * @brief Returns the transformation matrix of inertial wrt perifocal coordinates
     *
     * @return TIP = TM of inertial wrt perifocal
     *
     * @param[in] incl = inclination of orbital wrt equatorial plane - rad
     * @param[in] lon_anode = celestial longitude of the ascending node - rad
     * @param[in] arg_peri = argument of periapsis (ascending node to periapsis) - rad
     *
     * @author 040510 Created by Peter H Zipfel
     */
    arma::mat33 tip(const double &incl,
                    const double &lon_anode,
                    const double &arg_peri);
}  // namespace cad
#endif  // __CAD_UTIL_HH__


#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry> //Eigen Geometry module

//----------------------------use Eigen Geometry module-------------------------
// Eigen/Geometry 提供了各种旋转和平移的表示

int main(int argc, char const *argv[])
{
   // set a 3x3 indentity matrix
   Eigen::Matrix3d R = Eigen::Matrix3d::Identity();

   //-------------------get Rotation Matrix given theta,n-----------------------
   // 创建rotation动作
   //   rot(theta,n) rot 45 degree about z-axis
   //   类型 Eigen::AngleAxisd rot(),
   //     括号里面可以是（theta, n）, 也可以是quaternion q，应该还能是很多其他类型的
   //   旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
   Eigen::AngleAxisd rot(M_PI / 4, Eigen::Vector3d(0, 0, 1));
   //   Eigen::AngleAxisd rot_2(q); //q在下面定义这里用不了
   // two ways to see the rotation matrix from rot，都可以返回一个matrix
   // 1. rot.matrix()
   // 2. rot.toRotationMatrix()
   R = rot.matrix();           //等价
   R = rot.toRotationMatrix(); //等价
   cout << "rotation matrix =\n" << R << "\n" << endl;

   // 坐标转换
   // 初始vector p
   Eigen::Vector3d p(1, 0, 0);        //这里用了括号初始化，没有赋值操作
   // p' = rot * p -> p在旋转后的坐标里面的值
   Eigen::Vector3d p_prime = rot * p; //rot.matrix()*p, rot可以直接进行运算（因为重载了运算符）
                                      //也可以用 p_prime = R * p   line23
   cout << "vector p (1,0,0) after rotation = " << p_prime.transpose() << "\n" << endl;

   //-----------------------------Euler Angle-----------------------------------
   // 可以将旋转矩阵直接转换成欧拉角，欧拉角用3个量表示3个自由度
   // 欧拉角将一个一次性的rotation matrix分解成三次旋转，返回每次旋转对应的角度
   // 所有3维matrix都可以使用.eulerAngles(0,1,2)，类型为AngleAxisd的，比如rot不能使用(line19)
   Eigen::Vector3d euler_angles // 返回一个3x1的向量
      = R.eulerAngles(2, 1, 0); // ZYX顺序，即roll pitch yaw顺序
   cout << "yaw pitch roll = " << euler_angles.transpose() << "\n" << endl;

   //------------------------Transformation Matrix------------------------------
   // T = | R | t |
   //     | 0 | 1 |
   // given R，t，create transformation matrix（欧式变换矩阵）
   Eigen::Vector3d   t(1, 2, 4);                        //初始化t，没有赋值操作
   Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); //虽然是3d，但是是4x4的矩阵
   T.rotate(R);                                         //把 R 放到 T 里面
   T.pretranslate(t);                                   //把 t 放到 T 里面
   cout << "Transform matrix = \n" << T.matrix() << "\n" << endl;

   // 坐标转换
   p_prime = T * p; //这里T是4x4，而p是3x1。实际操作是 p'=Rp+t
   cout << "p' after transformation matrix is "
        << p_prime.transpose() << "\n" << endl;

   //-------------------------------Quaternion---------------------------------
   // 将 rotation matrix R 转变为 Quaternion q
   // 用Eigen::Quaternion()
   // 括号里的参数类型可以是Matrix3d,比如R，也可以是AngleAxisd，比如rot
   Eigen::Quaterniond q = Eigen::Quaterniond(rot);
   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
   cout << "quaternion = \n" << q.coeffs() << "\n" << endl;

   // 坐标转换
   p_prime = q * p; //这里的实际操作是 p'=qpq^(-1)
   cout << "p' after Quaternion transformation is "
        << p_prime.transpose() << "\n" << endl;

   //----对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略-----

   return 0;
}


#pragma once

#include <Eigen/Dense>
#include <time.h>
#include <vector>

using namespace Eigen;
using namespace std;

class dpmeans
{

public:
	dpmeans(const Ref<const MatrixXf>& X);  //X is n x d
	~dpmeans() {};

	float kpp_init(const Ref<const MatrixXf>& X, int k);
	VectorXi dpmeans_fit(const Ref<const MatrixXf>& X);
	float compute_nmi(const Ref<const VectorXi>& z1, const Ref<const VectorXi>& z2);
	void display_params();

private:
	int K; //infered automatically
	int K_init; //for lambda k++ initiliazation
	int n; //number of points
	int d; //dimension

	VectorXi z;   //assignments
	MatrixXf mu;  //K x d means
	VectorXf nk;  //number of points in cluster k
	VectorXf pik; //mixture proportions
	float lambda; //dp-means hyper-parameter

	int max_iter;
	vector<double> obj;
	vector<double> em_time;

};


/**
 * license is free
 * \file    ontheline.cpp
 * \author  hiratake26to@gmail.com
 * \date    2017-10-11 JST
 */
#include <iostream>
#include <boost/numeric/ublas/vector.hpp>

/**
 * \brief mouse pointer is on the line
 * \param xa  x of equipment A
 * \param ya  y of equipment A
 * \param xb  x of equipment B
 * \param yb  y of equipment B
 * \param xc  x of mouse pointer
 * \param yc  y of mouse pointer
 * \param r   line width
 */
bool isOnTheLine(int xa, int ya, int xb, int yb, int xc, int yc, int r);

int main() {
  /*    0 1 2 3 4
   * 0  _ _ _ _ _
   * 1  _ _ _ B _
   * 2  _ _ @ _ _
   * 3  _ @ _ _ _
   * 4  A _ _ _ _
   */
  bool result;
  result = isOnTheLine(0, 4, 3, 1, 2, 2, 1);
  std::cout << "result : " << result << std::endl;
  std::cout << "--------------------------" << std::endl;
  result = isOnTheLine(0, 4, 3, 1, 3, 2, 1);
  std::cout << "result : " << result << std::endl;
  std::cout << "--------------------------" << std::endl;
  result = isOnTheLine(0, 4, 3, 1, 4, 3, 1);
  std::cout << "result : " << result << std::endl;
  std::cout << "--------------------------" << std::endl;
  result = isOnTheLine(0, 4, 3, 1, 4, 0, 0);
  std::cout << "result : " << result << std::endl;
  return 0;
}

bool isOnTheLine(int xa, int ya, int xb, int yb, int xc, int yc, int r)
{
  namespace ublas = boost::numeric::ublas;

  // point C is mouse pointer
  // point X is point at the intersection of AB with CX
  // vector CX is perpendicular line from C to line AB
  ublas::vector<double> v_AB(2);
  ublas::vector<double> v_nAB(2); // nAB is unit vector of AB
  ublas::vector<double> v_AC(2);
  ublas::vector<double> v_AX(2);
  ublas::vector<double> v_CX(2);
  double len_AB;                  // length of vector AB
  double len_AX;                  // length of vector AX
  double len_CX;                  // length of vector CX

  // vector AB
  v_AB[0] = xb - xa; // AB.x
  v_AB[1] = yb - ya; // AB.y
  // length of AB
  len_AB = ublas::norm_2(v_AB);

  // unit vector nAB
  v_nAB = v_AB / len_AB;

  // vector AC
  v_AC[0] = xc - xa; // AC.x
  v_AC[1] = yc - ya; // AC.y

  // length of AX
  len_AX = ublas::inner_prod(v_AC, v_nAB); // (AC,nAB)
  // vector AX
  v_AX = v_nAB * len_AX; // AX = nAB*|AX|

  // vector CX
  v_CX = v_AX - v_AC;
  // length of CX
  len_CX = ublas::norm_2(v_CX);

  // debug log
  std::cout << "length of AB    : " << len_AB << std::endl;
  std::cout << "length of AX    : " << len_AX << std::endl;
  std::cout << "dist from C to X: " << len_CX << std::endl;
  std::cout << "check length CX : "
    << ((len_CX < r)?"true":"false") << std::endl;
  std::cout << "check length AX : "
    << ((len_AX < len_AB)?"true":"false") << std::endl;

  bool pointer_isOnLine = (len_CX < r) && (len_AX <= len_AB);

  return pointer_isOnLine;
}


#include <iostream>
#include <cmath>
#include <math.h>
#include <iostream>
#include <vector>
#include <array>
#include "heatEquation.hpp"
#include "TriDiagMatrix.hpp"
#include "MassMatrix.hpp"
#include "StiffnessMatrix.hpp"
#include <fstream>
#include <string>
#include <boost/math/quadrature/gauss.hpp>
using namespace std;
using namespace boost::math::quadrature;

void HeatEquation::SetSpaceTimeMesh( SpaceMesh smesh, TimeMesh tmesh, const std::string outputFileName )
{
    mpsmesh = smesh;
    mptmesh = tmesh;
    mpcurrenTimeStep = 0;
}

void HeatEquation::Solve()
{
AnalyticSolutionVec();
mpPreviousSolution = mpAnalyticSolution;

int m = mptmesh.NumberOfTimeSteps();
for(int j = 0; j<m; j++)
{
mpcurrenTimeStep = j+1;
mpcurrentMeshIndex = j;
stiff.BuildStiffnessMatrix( mpsmesh );
stiff.MultiplyByScalar( mptmesh.ReadTimeMesh(mpcurrentMeshIndex) );
mass.BuildMassMatrix(mpsmesh);
LHS.AddTwoMatrices( mass, stiff );

mass.MatrixVectorMultiplier( mpPreviousSolution, mpRHS );

LHS.MatrixSolver( mpRHS, mpx );

mpPreviousSolution = mpx;

}
}

void HeatEquation::AnalyticSolutionVec( )
{
    mpAnalyticSolution.clear();
     for (int i = 0; i<mpsmesh.meshsize()-1; i++)
{

    mpAnalyticSolution.push_back(ContinuousAnalyticSolution( mpsmesh.ReadSpaceNode(i+1),
                                                                        mptmesh.ReadTimeStep(mpcurrenTimeStep)));
}
}

    //the if statements deal with what happens if you are in the first interval
    //in this case the first value of u is given by the boundary condition
    //all the other elements can be fixed for x = 0
    //this function only works for the static case if the mesh changes everything is fucked
double HeatEquation::PiecewiseU( double x, SpaceMesh currentsmesh, std::vector<double> U )
{
    std::array<double, 2> firstpoint;
    std::array<double, 2> secondpoint;

    int upperindex = currentsmesh.IndexAbove( x );

    auto boundaryconditionU0 = 0;
    auto boundarycondition1Un = 0;

    if((upperindex==1)||(upperindex==0))
    {
    firstpoint.at(0)= currentsmesh.ReadSpaceNode(0);
    firstpoint.at(1) = boundaryconditionU0;

    secondpoint[0] = currentsmesh.ReadSpaceNode(1);
    secondpoint.at(1) = U.at(0);
    }
    else if (upperindex == currentsmesh.meshsize())
    {
    firstpoint.at(0)= currentsmesh.ReadSpaceNode(upperindex-1);
    firstpoint.at(1) = U.at(upperindex-2);

    secondpoint[0] = currentsmesh.ReadSpaceNode(upperindex);
    secondpoint.at(1) = boundarycondition1Un;
    }
    else
    {
    firstpoint.at(0)= currentsmesh.ReadSpaceNode(upperindex-1);
    firstpoint.at(1) = U.at(upperindex-2);

    secondpoint[0] = currentsmesh.ReadSpaceNode(upperindex);
    secondpoint.at(1) = U.at(upperindex-1);
    }

    long double m = (firstpoint[1]-secondpoint[1])/(firstpoint[0]-secondpoint[0]);

    return m*(x - firstpoint[0])+firstpoint[1];
}

double HeatEquation::ContinuousAnalyticSolution( double x, double t )
{
     return 6*exp(-t)*sin(M_PI*x);
}

void HeatEquation::BuildErrorMesh()
{
mpErrorMesh.clear();

auto SquaredError = [&](double x)
    { return pow(PiecewiseU(x, mpsmesh, mpx)-ContinuousAnalyticSolution(x, mptmesh.ReadTimeStep(mpcurrenTimeStep)), 2); };

double Q;
for(int i=0; i<mpsmesh.meshsize(); i++)
{
Q = gauss<double, 7>::integrate(SquaredError, mpsmesh.ReadSpaceNode(i), mpsmesh.ReadSpaceNode(i+1));
mpErrorMesh.push_back( Q );
}

}

double HeatEquation::GlobalSpaceError()
{
    BuildErrorMesh();
    double globalError=0;
    for(auto k: mpErrorMesh)
        globalError = globalError + k;

    std::cout << sqrt(globalError);
    std::cout << " \n";
    return sqrt(globalError);
}

void HeatEquation::PrintErrorMesh()
{
    BuildErrorMesh();
    PrintVector(mpErrorMesh);
}

void HeatEquation::PrintSolution( )
{
        BuildErrorMesh();
        AnalyticSolutionVec();

        std::cout << "FEM Approximation:     ";
        PrintVector(mpx);
        std::cout << "Analytic Solution:     ";
        PrintVector(mpAnalyticSolution);
        std::cout << "Error Mesh:            ";
        PrintVector(mpErrorMesh);
        std::cout << "Global Error           ";
        GlobalSpaceError();
}

void HeatEquation::PrintVector( std::vector<double> aVector)
{
        for (auto k: aVector)
        std::cout << k << ", ";
        std::cout << " \n";
}

void HeatEquation::AddVectors(std::vector<double>func1, std::vector<double> func2, std::vector<double>& result)
{
    if(func1.size()==func2.size())
    {
    result.clear();
     for ( int i=0; i<func1.size(); i++ )
     {
         result.push_back(func1.at(i)+func2.at(i));
     }
    }
    else
    {
        std::cout<< " your vectors are different sizes";
        std::cout<<"\n";
    }
}

void HeatEquation::VectorTimesScalar( std::vector<double>& func1, double scalar)
{
         for ( int i=0; i<func1.size(); i++ )
     {
         func1.at(i)= scalar*func1.at(i);
     }
}

Simple Heat Equation solver class/heatEquation.cpp


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Daniele Panozzo <daniele.panozzo@gmail.com>, Olga Diamanti <olga.diam@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.

#include "rotation_matrix_from_directions.h"
#include <Eigen/Geometry>
#include <iostream>

template <typename Scalar>
IGL_INLINE Eigen::Matrix<Scalar, 3, 3> igl::rotation_matrix_from_directions(
  const Eigen::Matrix<Scalar, 3, 1> v0,
  const Eigen::Matrix<Scalar, 3, 1> v1)
{
  Eigen::Matrix<Scalar, 3, 3> rotM;
  const double epsilon=1e-8;
  Scalar dot=v0.normalized().dot(v1.normalized());
  ///control if there is no rotation
  if ((v0-v1).norm()<epsilon)
  {
    rotM = Eigen::Matrix<Scalar, 3, 3>::Identity();
    return rotM;
  }
  if ((v0+v1).norm()<epsilon)
  {
    rotM = -Eigen::Matrix<Scalar, 3, 3>::Identity();
    rotM(0,0) = 1.;
    std::cerr<<"igl::rotation_matrix_from_directions: rotating around x axis by 180o"<<std::endl;
    return rotM;
  }
  ///find the axis of rotation
  Eigen::Matrix<Scalar, 3, 1> axis;
  axis=v0.cross(v1);
  axis.normalize();

  ///construct rotation matrix
  Scalar u=axis(0);
  Scalar v=axis(1);
  Scalar w=axis(2);
  Scalar phi=acos(dot);
  Scalar rcos = cos(phi);
  Scalar rsin = sin(phi);

  rotM(0,0) =      rcos + u*u*(1-rcos);
  rotM(1,0) =  w * rsin + v*u*(1-rcos);
  rotM(2,0) = -v * rsin + w*u*(1-rcos);
  rotM(0,1) = -w * rsin + u*v*(1-rcos);
  rotM(1,1) =      rcos + v*v*(1-rcos);
  rotM(2,1) =  u * rsin + w*v*(1-rcos);
  rotM(0,2) =  v * rsin + u*w*(1-rcos);
  rotM(1,2) = -u * rsin + v*w*(1-rcos);
  rotM(2,2) =      rcos + w*w*(1-rcos);

  return rotM;
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template Eigen::Matrix<double, 3, 3, 0, 3, 3> igl::rotation_matrix_from_directions<double>(const Eigen::Matrix<double, 3, 1, 0, 3, 1>, const Eigen::Matrix<double, 3, 1, 0, 3, 1>);
#endif


#include "src/metric/pnorm_metric.h"

#include <exception>

#include <armadillo>

#include "gtest/gtest.h"
#include "gmock/gmock.h"

namespace ocr {
	class PNormMetricTests : public testing::Test {
	public:
		void SetUp() {

		}

		void TearDown() {

		}
	};

	TEST_F(PNormMetricTests, Constructor_EmptyParam_Valid) {
		EXPECT_NO_THROW({ocr::PNorm();});
	}

	TEST_F(PNormMetricTests, Constructor_PosNumberParam_Valid) {
		EXPECT_NO_THROW({ocr::PNorm(1);});
		EXPECT_NO_THROW({ocr::PNorm(2);});
	}

	TEST_F(PNormMetricTests, Constructor_ZeroParam_Invalid) {
		EXPECT_THROW({ocr::PNorm(0);}, std::invalid_argument);
	}

	TEST_F(PNormMetricTests, Distance_EuclideanParam_Test) {
		ocr::PNorm euclidean_metric = ocr::PNorm(2);
		EXPECT_EQ(0, euclidean_metric.distance({0,0,0},{0,0,0}));
		EXPECT_EQ(0, euclidean_metric.distance({1,1},{1,1}));
		EXPECT_EQ(2, euclidean_metric.distance({0,0,0,0},{1,1,1,1}));
		EXPECT_EQ(2, euclidean_metric.distance({1,0,1,0},{0,1,0,1}));
	}

	TEST_F(PNormMetricTests, Distance_ManhattanParam_Test) {
		ocr::PNorm manhattan_metric = ocr::PNorm(1);
		EXPECT_EQ(0, manhattan_metric.distance({0,0,0},{0,0,0}));
		EXPECT_EQ(0, manhattan_metric.distance({1,1},{1,1}));
		EXPECT_EQ(4, manhattan_metric.distance({0,0,0,0},{1,1,1,1}));
		EXPECT_EQ(4, manhattan_metric.distance({1,0,1,0},{0,1,0,1}));
	}

}


#ifndef BURKARDT_ODE_HPP
#define BURKARDT_ODE_HPP

// ODE test problem set from
//   https://people.sc.fsu.edu/~burkardt/f_src/test_ode/test_ode.html
//   https://people.sc.fsu.edu/~burkardt/f_src/test_ode/test_ode.f90

#include <boost/numeric/ublas/vector.hpp>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>

namespace ub = boost::numeric::ublas;

class P01 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(1);

		y(0) = - x(0);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(1);
		x(0) = 1.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P02 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(1);

		y(0) = - x(0)* x(0)*x(0) / 2.;

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(1);
		x(0) = 1.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P03 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(1);

		y(0) = - cos(t) * x(0);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(1);
		x(0) = 1.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P04 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(1);

		y(0) = x(0) * (20. - x(0)) / 80.;

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(1);
		x(0) = 1.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P05 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(1);

		y(0) = (x(0) - t) / (x(0) + t);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(1);
		x(0) = 4.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P06 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);

		y(0) = 2. * x(0) * (1. - x(1));
		y(1) = - x(1) * (1. - x(0));

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 1.;
		x(1) = 3.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P07 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(3);

		y(0) = -x(0) + x(1);
		y(1) = x(0) - 2. * x(1) + x(2);
		y(2) = x(1) - x(2);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(3);
		x(0) = 2.;
		x(1) = 0.;
		x(2) = 1.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P08 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(3);

		y(0) = -x(0);
		y(1) = x(0) - x(1) * x(1);
		y(2) = x(1) * x(1);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(3);
		x(0) = 1.;
		x(1) = 0.;
		x(2) = 0.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P09 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(3);

		T norm;

		norm = sqrt(x(0)*x(0) + x(1)*x(1));

		y(0) = - x(1) - x(0) * x(2) / norm;
		y(1) = x(0) - x(1) * x(2) / norm;
		y(2) = x(0) / norm;

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(3);
		x(0) = 3.;
		x(1) = 0.;
		x(2) = 0.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P10 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(3);

		y(0) = x(1) * x(2);
		y(1) = - x(0) * x(2);
		y(2) = -0.51 * x(0) * x(1);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(3);
		x(0) = 0.;
		x(1) = 1.;
		x(2) = 1.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P11 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(10);
		int i;

		y(0) = - x(0);
		for (i=1; i<9; i++) {
			y(i) = x(i-1) - x(i);
		}
		y(9) = - x(8);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(10);
		x(0) = 1.;
		for (i=1; i<10; i++) {
			x(i) = 0.;
		}
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P12 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(10);
		int i;

		y(0) = - x(0);
		for (i=1; i<9; i++) {
			y(i) = (double)i * x(i-1) - (double)(i+1) * x(i);
		}
		y(9) = - 9. * x(8);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(10);
		x(0) = 1.;
		for (i=1; i<10; i++) {
			x(i) = 0.;
		}
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P13 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(10);
		int i;

		y(0) = - 2. * x(0) + x(1);
		for (i=1; i<9; i++) {
			y(i) = x(i-1) - 2. * x(i) + x(i+1);
		}
		y(9) = x(8) - 2. * x(9);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(10);
		x(0) = 1.;
		for (i=1; i<10; i++) {
			x(i) = 0.;
		}
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P14 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(51);
		int i;

		y(0) = - 2. * x(0) + x(1);
		for (i=1; i<50; i++) {
			y(i) = x(i-1) - 2. * x(i) + x(i+1);
		}
		y(50) = x(49) - 2. * x(50);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(51);
		x(0) = 1.;
		for (i=1; i<51; i++) {
			x(i) = 0.;
		}
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P15 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(30);
		const double k2 = 2.95912208286;
		const double m[6] = {
			1.00000597682,
			0.954786104043e-03,
			0.285583733151e-03,
			0.437273164546e-04,
			0.517759138449e-04,
			0.277777777778e-05
		};
		int i, i3, j, l, ll, mm;
		T p;
		T q[5][5];
		T r[5];

		i = 0;
		for (l=3; l<=15; l+=3) {
			i++;
			p = x(l-3)*x(l-3) + x(l-2)*x(l-2) + x(l-1)*x(l-1);
			r[i-1] = 1. / (p * sqrt(p));
			j = 0;
			for (ll=3; ll<=15; ll+=3) {
				j++;
				if (ll != l) {
					p = (x(l-3) - x(ll-3)) * (x(l-3) - x(ll-3))
					+ (x(l-2) - x(ll-2)) * (x(l-2) - x(ll-2))
					+ (x(l-1) - x(ll-1)) * (x(l-1) - x(ll-1));
					q[i-1][j-1] = 1. / (p * sqrt(p));
					q[j-1][i-1] = q[i-1][j-1];
				}
			}
		}

		i3 = 0;
		for (i=1; i<=5; i++) {
			i3 += 3;
			for (ll = i3-2; ll <= i3; ll++) {
				mm = ll - i3;
				y(ll-1) = x(ll+14);
				p = 0.;
				for (j=1; j<=5; j++) {
					mm += 3;
					if (j != i) {
						p += m[j-1] * (x(mm-1) * (q[i-1][j-1] - r[j-1]) - x(ll-1) * q[i-1][j-1]);
					}
				}
				y(ll+14) = k2 * ( - (m[0] + m[i]) * x(ll-1) * r[i-1] + p);
			}
		}

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {
		x.resize(30);

		x(0) = 3.42947415189;
		x(1) = 3.35386959711;
		x(2) = 1.35494901715;
		x(3) = 6.64145542550;
		x(4) = 5.97156957878;
		x(5) = 2.18231499728;
		x(6) = 11.2630437207;
		x(7) = 14.6952576794;
		x(8) = 6.27960525067;
		x(9) = -30.1552268759;
		x(10) = 1.65699966404;
		x(11) = 1.43785752721;
		x(12) = -21.1238353380;
		x(13) = 28.4465098142;
		x(14) = 15.3882659679;
		x(15) = -0.557160570446;
		x(16) = 0.505696783289;
		x(17) = 0.230578543901;
		x(18) = -0.415570776342;
		x(19) = 0.365682722812;
		x(20) = 0.169143213293;
		x(21) = -0.325325669158;
		x(22) = 0.189706021964;
		x(23) = 0.0877265322780;
		x(24) = -0.0240476254170;
		x(25) = -0.287659532608;
		x(26) = -0.117219543175;
		x(27) = -0.176860753121;
		x(28) = -0.216393453025;
		x(29) = -0.0148647893090;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P16 {
	public:
	double delta;

	P16(double _delta = 0.1) : delta(_delta) {}

	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(4);
		T tmp;

		tmp = sqrt(x(0) * x(0) + x(1) * x(1));
		y(0) = x(2);
		y(1) = x(3);
		y(2) = - x(0) / (tmp * tmp * tmp);
		y(3) = - x(1) / (tmp * tmp * tmp);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {
		x.resize(4);
		x(0) = 1. - delta;
		x(1) = 0.;
		x(2) = 0.;
		x(3) = sqrt((1. + delta)/(1. - delta));
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

// P17-P20はP16の初期値違い。
// P16, P17は解けるが、P18-P20は厳しい。

class P17 : public P16 {
	public:
	P17(double _delta = 0.3) {delta = _delta;}
};

class P18 : public P16 {
	public:
	P18(double _delta = 0.5) {delta = _delta;}
};

class P19 : public P16 {
	public:
	P19(double _delta = 0.7) {delta = _delta;}
};

class P20 : public P16 {
	public:
	P20(double _delta = 0.9) {delta = _delta;}
};

class P21 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);

		y(0) = x(1);
		y(1) = - ( 1. - 0.25 / ((t + 1.) * (t + 1.))) * x(0) - 1. / (t + 1.) * x(1);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 0.6713967071418030;
		x(1) = 0.09540051444747446;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P22 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);

		y(0) = x(1);
		y(1) = (1. - x(0) * x(0)) * x(1) - x(0);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 2.;
		x(1) = 0.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P23 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);

		y(0) = x(1);
		y(1) = x(0) * x(0) * x(0) / 6. - x(0) + 2. * sin(2.78535 * t);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 0.;
		x(1) = 0.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P24 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);

		y(0) = x(1);
		y(1) = 0.032 - 0.4 * x(1) * x(1);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 30.;
		x(1) = 0.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

class P25 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);

		y(0) = x(1);
		y(1) = sqrt(1. + x(1) * x(1)) / (25. - t);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 0.;
		x(1) = 0.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

// Lotka-Volterra Predator-Prey Equations
class P31 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);
		const double a = 5.;
		const double b = 1.;
		const double c = 0.5;
		const double d = 2.;

		y(0) = (a - b * x(1)) * x(0);
		y(1) = (c * x(0) - d) * x(1);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 2.;
		x(1) = 2.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 10.;
	}
};

// The Lorenz System
class P32 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(3);
		const double beta = 8./3.;
		const double rho = 28.;
		const double sigma = 10.;

		y(0) = sigma * (x(1) - x(0));
		y(1) = rho * x(0) - x(1) - x(0) * x(2);
		y(2) = - beta * x(2) + x(0) * x(1);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(3);
		x(0) = 2.;
		x(1) = 2.;
		x(2) = 21.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

// The Van der Pol equation
class P33 {
	public:

	double delta;

	P33(double _delta = 1.) : delta(_delta) {}

	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);

		y(0) = x(1);
		y(1) = delta * (1. - x(0) * x(0)) * x(1) - x(0);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 2.;
		x(1) = 2.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

// The Linearized Damped Pendulum
class P34 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);
		const double g = 32.;
		const double k = 1.;
		const double l = 1.;
		const double m = 1.;

		y(0) = x(1);
		y(1) = - (g / l) * x(0) - (k / m) * x(1);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 2.;
		x(1) = 2.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

// The Nonlinear Damped Pendulum
class P35 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);
		const double g = 32.;
		const double k = 1.;
		const double l = 1.;
		const double m = 1.;

		y(0) = x(1);
		y(1) = - (g / l) * sin(x(0)) - (k / m) * x(1);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 2.;
		x(1) = 2.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 20.;
	}
};

// Duffing's Equation
class P36 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);

		y(0) = x(1);
		y(1) = x(0) * (1. - x(0) * x(0));

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 0.5;
		x(1) = 0.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 100.;
	}
};

// Duffing's Equation with Damping and Forcing
class P37 {
	public:
	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(2);
		const double a = 0.3;
		const double k = 0.2;
		const double w = 1.;

		y(0) = x(1);
		y(1) = x(0) * (1. - x(0) * x(0)) - k * x(1) + a * cos(w * t);

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(2);
		x(0) = 0.5;
		x(1) = 0.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 100.;
	}
};

// Shampine's Ball of Flame
class P38 {
	public:
	double delta;

	P38(double _delta = 0.01) : delta(_delta) {}

	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(1);

		y(0) = x(0) * x(0) * (1. - x(0));

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(1);
		x(0) = delta;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 2./delta;
	}
};

// Polking's first order ODE
class P39 {
	public:

	double a;
	double b;

	P39(double _a = 1., double _b = 0.) : a(_a), b(_b) {}

	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(1);

		y(0) = x(0) * x(0) - a * t + b;

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(1);
		x(0) = 0.5;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = 0.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = (b + 9.) / a;
	}
};

// the Kee problem
class P40 {
	public:

	double eps;

	P40(double _eps = 0.01) : eps(_eps) {}

	template <class T> ub::vector<T> operator() (ub::vector<T> x, T t){
		ub::vector<T> y(1);

		y(0) = x(0) * (x(0) - t) / eps;

		return y;
	}

	template<class T>
	void initial_value(ub::vector< kv::interval<T> >& x) {

		x.resize(1);
		x(0) = -1.;
	}

	template<class T>
	void start_time(kv::interval<T>& x) {

		x = -1.;
	}

	template<class T>
	void stop_time(kv::interval<T>& x) {

		x = 1.;
	}
};

#endif // BURKARDT_ODE_HPP


///////////////////////////////////
#include <boost/test/unit_test.hpp>
///////////////////////////
#include "distancefixture.h"
/////////////////////////
#include <indivtree.h>
#include <clusterize_kmeans.h>
/////////////////////////////////
#include <boost/foreach.hpp>
//////////////////////
using namespace std;
using namespace info;
/////////////////////////////
BOOST_FIXTURE_TEST_SUITE(IndivTreeTestSuite, DistanceFixture)
;
BOOST_AUTO_TEST_CASE(testIndivTreeMean) {
	INumIndivProvider *pProvider = m_pnumprovider.get();
	BOOST_REQUIRE(pProvider != nullptr);
	size_t nbClusters = 5;
	LinkMode mode = LinkMode::linkMean;
	IndivsTree oTree;
	oTree.process(pProvider, nbClusters, mode);
	ints_size_t_map xMap;
	oTree.get_map(xMap);
	size_t n = xMap.size();
	BOOST_CHECK(n == m_nbrows);
	IndivsTree::valuemaps_vector oCenters;
	oTree.get_centers(oCenters);
	BOOST_CHECK(oCenters.size() == nbClusters);
	//int nx = 0;
	//BOOST_TEST_MESSAGE("LINK MEAN");
	BOOST_FOREACH(const DbValueMap &oMap, oCenters)
	{
		std::string ss;
		this->write_point(oMap, ss);
		//BOOST_TEST_MESSAGE("Cluster " << nx++ << ":\t" << ss);
	}
	/*
	std::string sx;
	this->write_indis_map(xMap,sx);
	BOOST_TEST_MESSAGE(sx);
	*/
} //testInfoGlobalClusterize
BOOST_AUTO_TEST_CASE(testIndivTreeMin) {
	INumIndivProvider *pProvider = m_pnumprovider.get();
	BOOST_REQUIRE(pProvider != nullptr);
	size_t nbClusters = 5;
	LinkMode mode = LinkMode::linkMin;
	IndivsTree oTree;
	oTree.process(pProvider, nbClusters, mode);
	ints_size_t_map xMap;
	oTree.get_map(xMap);
	size_t n = xMap.size();
	BOOST_CHECK(n == m_nbrows);
	IndivsTree::valuemaps_vector oCenters;
	oTree.get_centers(oCenters);
	BOOST_CHECK(oCenters.size() == nbClusters);
	//int nx = 0;
	//BOOST_TEST_MESSAGE("LINK MAX");
	BOOST_FOREACH(const DbValueMap &oMap, oCenters)
	{
		std::string ss;
		this->write_point(oMap, ss);
		//BOOST_TEST_MESSAGE("Cluster " << nx++ << ":\t" << ss);
	}
	/*
	std::string sx;
	this->write_indis_map(xMap, sx);
	BOOST_TEST_MESSAGE(sx);
	*/
} //testInfoGlobalClusterize
BOOST_AUTO_TEST_CASE(testIndivTreeMax) {
	INumIndivProvider *pProvider = m_pnumprovider.get();
	BOOST_REQUIRE(pProvider != nullptr);
	size_t nbClusters = 5;
	LinkMode mode = LinkMode::linkMax;
	IndivsTree oTree;
	oTree.process(pProvider, nbClusters, mode);
	ints_size_t_map xMap;
	oTree.get_map(xMap);
	size_t n = xMap.size();
	BOOST_CHECK(n == m_nbrows);
	IndivsTree::valuemaps_vector oCenters;
	oTree.get_centers(oCenters);
	BOOST_CHECK(oCenters.size() == nbClusters);
	//int nx = 0;
	//BOOST_TEST_MESSAGE("LINK MIN");
	BOOST_FOREACH(const DbValueMap &oMap, oCenters)
	{
		std::string ss;
		this->write_point(oMap, ss);
		//BOOST_TEST_MESSAGE("Cluster " << nx++ << ":\t" << ss);
	}
	/*
	std::string sx;
	this->write_indis_map(xMap, sx);
	BOOST_TEST_MESSAGE(sx);
	*/
} //testInfoGlobalClusterize
BOOST_AUTO_TEST_CASE(testClusterizeKMeansWithSeed) {
	//BOOST_TEST_MESSAGE("ClusterizeKMeansSeed");
	INumIndivProvider *pProvider = m_pnumprovider.get();
	BOOST_REQUIRE(pProvider != nullptr);
	LinkMode mode = LinkMode::linkMin;
	size_t nbClusters = 5;
	IndivsTree oTree;
	oTree.process(pProvider, nbClusters, mode);
	IndivsTree::valuemaps_vector oCenters;
	oTree.get_centers(oCenters);
	BOOST_CHECK(oCenters.size() == nbClusters);
	size_t nbMaxIters = 100;
	ClusterizeKMeans oMeans(pProvider, nbClusters);
	BOOST_REQUIRE(oMeans.is_valid());
	bool bRet = oMeans.compute(oCenters,nbMaxIters);
	BOOST_REQUIRE(bRet);
	double fIntra = 0, fInter = 0, fCrit = 0;
	bRet = oMeans.intra_inertia(fIntra);
	BOOST_CHECK(bRet);
	BOOST_CHECK(fIntra > 0);
	bRet = oMeans.inter_inertia(fInter);
	BOOST_CHECK(bRet);
	BOOST_CHECK(fInter > 0);
	bRet = oMeans.criteria(fCrit);
	BOOST_CHECK(bRet);
	BOOST_CHECK(fCrit > 0);
	//BOOST_TEST_MESSAGE("Criteria: " << fCrit << "\tIntra: " << fIntra << "\tInter: " << fInter);
	const indivclusters_vector & oClusters = oMeans.clusters();
	BOOST_FOREACH(const IndivCluster &oCluster, oClusters)
	{
		std::string ss;
		this->write_point(oCluster.center(), ss);
	  //BOOST_TEST_MESSAGE("Cluster: " << oCluster.index() << "\t" << ss);
	}// oCluster
	ints_size_t_map xMap = oMeans.get_map();
	std::string sx;
	this->write_indis_map(xMap, sx);
	//BOOST_TEST_MESSAGE(sx);
} //testClusterizeKMeans
BOOST_AUTO_TEST_SUITE_END();


//
// Copyright (c) 2019-2020 CNRS INRIA
//

#include <boost/python.hpp>

#include "pinocchio/bindings/python/fwd.hpp"
#include "pinocchio/bindings/python/utils/namespace.hpp"
#include "pinocchio/math/rpy.hpp"

namespace pinocchio
{
  namespace python
  {
    namespace bp = boost::python;

    Eigen::Matrix3d rpyToMatrix_proxy(const Eigen::Vector3d & rpy)
    {
      return pinocchio::rpy::rpyToMatrix(rpy);
    }

    Eigen::Vector3d matrixToRpy_proxy(const Eigen::Matrix3d & R)
    {
      return pinocchio::rpy::matrixToRpy(R);
    }

    Eigen::Matrix3d rotate(const std::string & axis, const double ang)
    {
      if(axis.length() != 1U)
          throw std::invalid_argument(std::string("Invalid axis: ").append(axis));
      Eigen::Vector3d u;
      u.setZero();
      const char axis_ = axis[0];
      switch(axis_)
      {
        case 'x': u[0] = 1.; break;
        case 'y': u[1] = 1.; break;
        case 'z': u[2] = 1.; break;
        default: throw std::invalid_argument(std::string("Invalid axis: ").append(1U,axis_));
      }

      return Eigen::AngleAxisd(ang, u).matrix();
    }

    void exposeRpy()
    {
      using namespace Eigen;
      using namespace pinocchio::rpy;

      {
        // using the rpy scope
        bp::scope current_scope = getOrCreatePythonNamespace("rpy");

        bp::def("rpyToMatrix",
                static_cast<Matrix3d (*)(const double, const double, const double)>(&rpyToMatrix),
                bp::args("roll", "pitch", "yaw"),
                "Given (r, p, y), the rotation is given as R = R_z(y)R_y(p)R_x(r),"
                " where R_a(theta) denotes the rotation of theta radians axis a");

        bp::def("rpyToMatrix",
                &rpyToMatrix_proxy,
                bp::arg("rpy"),
                "Given (r, p, y), the rotation is given as R = R_z(y)R_y(p)R_x(r),"
                " where R_a(theta) denotes the rotation of theta radians axis a");

        bp::def("matrixToRpy",
                &matrixToRpy_proxy,
                bp::arg("R"),
                "Given a rotation matrix R, the angles (r, p, y) are given so that R = R_z(y)R_y(p)R_x(r),"
                " where R_a(theta) denotes the rotation of theta radians axis a."
                " The angles are guaranteed to be in the ranges: r in [-pi,pi],"
                " p in[-pi/2,pi/2], y in [-pi,pi]");

        bp::def("rotate",
                &rotate,
                bp::args("axis", "ang"),
                "Rotation matrix corresponding to a rotation about x, y or z"
                " e.g. R = rot('x', pi / 4): rotate pi/4 rad about x axis");
      }

    }

  } // namespace python
} // namespace pinocchio


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

#include "base/fit_plane.h"

#include <boost/test/auto_unit_test.hpp>

using mjmech::base::FitPlane;

BOOST_AUTO_TEST_CASE(BasicFitPlane, * boost::unit_test::tolerance(1e-4)) {
  {
    std::vector<Eigen::Vector3d> points = {
      { -2, -2, -1 },
      { -2, 2, -1 },
      { 2, -2, 1 },
      { 2, 2, 1 },
    };

    const auto result = FitPlane(points);
    BOOST_TEST(std::abs(result.c - 0.0) < 0.001);
    BOOST_TEST(result.a == 0.5);
    BOOST_TEST(result.b == 0.0);
  }

  {
    std::vector<Eigen::Vector3d> points = {
      { -2, -2, 0 },
      { -2, 2, 2 },
      { 2, -2, 0 },
      { 2, 2, 2 },
    };

    const auto result = FitPlane(points);
    BOOST_TEST(result.c == 1.0);
    BOOST_TEST(result.a == 0.0);
    BOOST_TEST(result.b == 0.5);
  }
}