// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2019 Alec Jacobson <alecjacobson@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 "rigid_alignment.h"
#include "polar_svd.h"
#include <Eigen/Sparse>
#include <Eigen/Cholesky>
#include <vector>
#include <iostream>

template <
  typename DerivedX,
  typename DerivedP,
  typename DerivedN,
  typename DerivedR,
  typename Derivedt
>
IGL_INLINE void igl::rigid_alignment(
  const Eigen::MatrixBase<DerivedX> & _X,
  const Eigen::MatrixBase<DerivedP> & P,
  const Eigen::MatrixBase<DerivedN> & N,
  Eigen::PlainObjectBase<DerivedR> & R,
  Eigen::PlainObjectBase<Derivedt> & t)
{
  typedef typename DerivedX::Scalar Scalar;
  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> MatrixXS;
  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1> VectorXS;
  typedef Eigen::Matrix<Scalar,3,3> Matrix3S;
  const int k = _X.rows();
  VectorXS Z = VectorXS::Zero(k,1);
  VectorXS I = VectorXS::Ones(k,1);

  DerivedX X = _X;
  R = DerivedR::Identity(3,3);
  t = Derivedt::Zero(1,3);
  // See gptoolbox, each iter could be O(1) instead of O(k)
  const int max_iters = 5;
  for(int iters = 0;iters<max_iters;iters++)
  {
    MatrixXS A(k*3,6);
    A <<
               Z, X.col(2),-X.col(1),I,Z,Z,
       -X.col(2),        Z, X.col(0),Z,I,Z,
        X.col(1),-X.col(0),        Z,Z,Z,I;
    VectorXS B(k*3,1);
    B<<
      P.col(0)-X.col(0),
      P.col(1)-X.col(1),
      P.col(2)-X.col(2);
    std::vector<Eigen::Triplet<Scalar> > NNIJV;
    for(int i = 0;i<k;i++)
    {
      for(int c = 0;c<3;c++)
      {
        NNIJV.emplace_back(i,i+k*c,N(i,c));
      }
    }
    Eigen::SparseMatrix<Scalar> NN(k,k*3);
    NN.setFromTriplets(NNIJV.begin(),NNIJV.end());
    A = (NN * A).eval();
    B = (NN * B).eval();
    VectorXS u = (A.transpose() * A).ldlt().solve(A.transpose() * B);
    Derivedt ti = u.tail(3).transpose();

    Matrix3S W;
    W<<
          0, u(2),-u(1),
      -u(2),    0, u(0),
       u(1),-u(0),    0;
    // strayed from a perfect rotation. Correct it.
    const double x = u.head(3).stableNorm();
    DerivedR Ri;
    if(x == 0)
    {
      Ri = DerivedR::Identity(3,3);
    }else
    {
      Ri =
        DerivedR::Identity(3,3) +
        sin(x)/x*W +
        (1.0-cos(x))/(x*x)*W*W;
    }

    R = (R*Ri).eval();
    t = (t*Ri + ti).eval();
    X = ((_X*R).rowwise()+t).eval();
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::rigid_alignment<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 3, 3, 0, 3, 3>, Eigen::Matrix<double, 1, 3, 1, 1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 3, 0, 3, 3> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> >&);
#endif


// Copyright 2014 Jonathan Graehl - http://graehl.org/
//
// 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.
#ifndef GRAEHL__SHARED__TIME_SERIES_HPP
#define GRAEHL__SHARED__TIME_SERIES_HPP

//FIXME: curvature does nothing if <=0 for score_t, otherwise results in +INF (bug) if positive

#ifdef SAMPLE
//# define DEBUG_TIME_SERIES
#endif

#ifdef DEBUG_TIME_SERIES
#define GRAEHL__DEBUG_PRINT
# include <graehl/shared/debugprint.hpp>
#endif
#include <boost/config.hpp>
#include <cstddef>
#include <cmath>
#include <cassert>
#include <functional>
#include <ostream>

namespace graehl {

inline double pow(double x, double y)
{
  return std::pow(x, y);  // this isn't found with using namespace std, so ...
}

/*
  template <class V>
  struct time_series
  {
  typedef V value_type;
  typedef std::size_t time_type;
  virtual V series_value_at(time_type t)=0;
  };
*/

/// Computes must support: Computes=pow(Value,double),+, -,*,/.
/// adaptable unary function: time_type(=double) -> Returns
template <class Computes = double, class Returns = Computes>
struct clamped_time_series : public std::unary_function<double, Returns>
{
  typedef Computes value_type;
  typedef Returns result_type;
  typedef double time_type;
  typedef time_type first_argument_type;
  value_type x0;
  value_type k;
  value_type x_origin;
  time_type t_max;
  //    BOOST_STATIC_CONSTANT(double,linear= -1.7976931348623157e308); // bad answers (all 0) for anything near -INF.  fast math lib failure?
  BOOST_STATIC_CONSTANT(int, linear=-100000000);
  BOOST_STATIC_CONSTANT(int, exponential = 0);
  BOOST_STATIC_CONSTANT(int, constant = 0);

  clamped_time_series()
  {
    set(0, 0);
  }

  clamped_time_series(value_type start, value_type end,// start and end should both be positive.  at t<=0, return start, at t>=duration-1, return end.  in between, depends on alpha
                      time_type duration = constant, // varies over t in [0,duration);
                      double curvature = exponential)
      // curvature=0->regular exponential decay (x[n+1]=k*x[n]).  curvature->(-infty) -> (nearly) linear.  curvature>=1 -> impossible. curvature->1 -> quickly drops to end and stays nearly constant
  {
    assert(curvature<=1);
    /* //FIXME: disabled because decoder_filters fixing end to 2 (start at 0)
       if (false && duration && start!=end) {
       assert(start>0);
       assert(end>0);
       }
    */
    set(start, end, duration, curvature);
  }

  // duration <= 0 -> constant series (=start)
  void set(value_type start, value_type end, time_type duration = constant, double curvature = exponential)
  {
#ifdef DEBUG_TIME_SERIES
    DBP4(start, end, duration, curvature);
#endif
    if (duration <= 0 || start == end) { //set constant fn
      k = 1;
      t_max = 1;
      x0 = start;
      x_origin = 0;
      return;
    }
    t_max = duration;
    value_type xN;
    if (curvature) {
      x_origin = (end*static_cast<value_type>(curvature));
      x0 = start-x_origin;
      xN = end-x_origin;
    } else {
      x_origin = 0;
      x0 = start;
      xN = end;
    }
    k = xN/x0;
#ifdef DEBUG_TIME_SERIES
    DBP3(x0, xN, k);
#endif
  }
  bool is_constant() const
  {
    return k==1;
  }
  value_type start() const
  {
    return x0+x_origin;
  }
  value_type end() const
  {
    return x0*k+x_origin; // because k=xN/x0, and xN=end-x_origin
  }
  value_type curvature() const
  {
    return x_origin/end();
  }

  value_type value(time_type t) const
  {
    if (t<=0)
      return start();
    else if (t>=t_max)
      return end();
    else
      return x0*pow(k, t/t_max)+x_origin;
  }

  result_type operator()(time_type t) const
  {
    return static_cast<result_type>(value(t));
  }

  void print(std::ostream &o) const
  {
    if (is_constant())
      o<<"[constant]"<<start();
    else
      o<<"[0.."<<t_max<<"]="<<start()<<".."<<end()<<";c="<<curvature();
  }

  typedef clamped_time_series<Computes, Returns> self_type;
  inline friend std::ostream & operator <<(std::ostream &o, self_type const& s)
  {
    s.print(o);
    return o;
  }
};

} //graehl

#ifdef SAMPLE
# define TIME_SERIES_SAMPLE
#endif

#ifdef TIME_SERIES_SAMPLE
# include <iostream>

using namespace graehl;

typedef clamped_time_series<double> dser;

double s = 8, e = .2, t_max = 3;
double linear = dser::linear;
double curves[] = {
  0, .9, linear
};

dser series[] = {
  dser(s, e, t_max, 0)
};

int main(int argc, char *argv[])
{
  using namespace std;
  for (unsigned i = 0; i<sizeof(curves)/sizeof(curves[0]); ++i) {
    dser d(s, e, t_max, curves[i]);
    cout << d << "\n";
    for (double t = 0; t<=t_max+1; t += .5)
      cout << "t="<<t<<"\t"<<d(t)<<endl;
  }

  cout <<"\n\n";

  {

    clamped_time_series<double>
        s1(s, e, t_max, 0),
        s2(s, e, t_max, .9),
        s3(s, e, t_max, linear);

    for (double t = 0; t<=t_max+1; t += .5) {
      cout << "t="<<t<<"\t"<<s1(t);
      cout <<"\t"<<s2(t)<<"\t"<<s3(t)<<endl;
    }
  }

  cout<<"\n\n";

  {

    clamped_time_series<double, unsigned>
        s1(s, e, t_max, 0),
        s2(s, e, t_max, .9),
        s3(s, e, t_max, linear);
    for (double t = 0; t<=t_max+1; t += .5) {
      cout << "t="<<t<<"\t"<<s1(t);
      cout <<"\t"<<s2(t)<<"\t"<<s3(t)<<endl;
    }

  }

  return 0;
}

#endif

#endif


#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <fstream>
#include <array>
#include <random>
#include <mutex>
#include <iostream>
#include <map>
#include <tuple>
#include <gtest/gtest.h>
#include <mpi.h>

#include "tasktorrent/tasktorrent.hpp"

using namespace std;
using namespace Eigen;
using namespace ttor;

typedef array<int, 2> int2;
typedef array<int, 3> int3;

int VERB = 0;
bool LOG = false;
int n_threads_ = 4;
int n_ = 100;
int N_ = 20;
int p_ = 1;
int q_ = 1;

void cholesky(int n_threads, int n, int N, int p, int q)
{
    // MPI info
    const int rank = comm_rank();
    const int n_ranks = comm_size();
    if(VERB) printf("[%d] Hello from %s\n", comm_rank(), processor_name().c_str());

    assert(p * q == n_ranks);
    assert(p >= 1);
    assert(q >= 1);

    // Form the matrix : let every node have a copy of A for now
    auto gen = [&](int i, int j) {
        if(i == j) {
            return static_cast<double>(N*n+2);
        } else {
            int k = (i+j)%3;
            if(k == 0) {
                return 0.0;
            } else if (k == 1) {
                return 0.5;
            } else {
                return 1.0;
            }
        }
    };
    MatrixXd A = MatrixXd::NullaryExpr(N * n, N * n, gen);
    MatrixXd Aref = A;
    A = A.triangularView<Lower>();

    // Mapper
    auto block2rank = [&](int2 ij){
        int i = ij[0];
        int j = ij[1];
        int ii = i % p;
        int jj = j % q;
        int r = ii + jj * p;
        assert(r <= n_ranks);
        return r;
    };

    // Gives the priority
    auto block2prio = [&](int2 ij) {
        int i = ij[0]+1;
        int j = ij[1]+1;
        assert(i >= j);
        return static_cast<double>(N*N - ((i*(i-1))/2 + j));
    };

    // Block the matrix for every node
    // Store it in a map
    // Later on we can probably optimize this, to avoid storing the whole thing
    // Note: concurrent access to the map should _NOT_ modify it, otherwise we need a lock
    map<int2, MatrixXd> Mat;
    for(int i = 0; i < N; i++) {
        for(int j = 0; j <= i; j++) {
            if(block2rank({i,j}) == rank) {
                Mat[{i,j}] = A.block(i * n, j * n, n, n);
            } else {
                Mat[{i,j}] = MatrixXd::Zero(n, n);
            }
        }
    }

    // Factorize
    {
        // Initialize the communicator structure
        Communicator comm(MPI_COMM_WORLD, VERB);

        // Threadpool
        Threadpool tp(n_threads, &comm, VERB, "[" + to_string(rank) + "]_");
        Taskflow<int> potf_tf(&tp, VERB);
        Taskflow<int2> trsm_tf(&tp, VERB);
        Taskflow<int3> gemm_tf(&tp, VERB);

        // Log
        DepsLogger dlog(1000000);
        Logger log(1000000);
        if(LOG) {
            tp.set_logger(&log);
            comm.set_logger(&log);
        }

        // Active messages
        // Sends a pivot and trigger multiple trsms in one columns
        auto am_trsm = comm.make_active_msg(
            [&](view<double> &Lkk, int& j, view<int>& is) {
                Mat.at({j,j}) = Map<MatrixXd>(Lkk.data(), n, n);
                for(auto& i: is) {
                    trsm_tf.fulfill_promise({i,j});
                }
            });

        // Sends a panel bloc and trigger multiple gemms
        auto am_gemm = comm.make_large_active_msg(
            [&](int&, int& j, view<int2>& ijs) {
                for(auto& ij: ijs) {
                    int gi = ij[0];
                    int gj = ij[1];
                    int gk = j;
                    gemm_tf.fulfill_promise({gi,gj,gk});
                }
            },
            [&](int& i, int& j, view<int2>&) {
                return Mat.at({i,j}).data();
            },
            [&](int&, int&, view<int2>&) {
                return;
            });

        // potf
        potf_tf.set_mapping([&](int j) {
                return (j % n_threads);
            })
            .set_indegree([](int) {
                return 1;
            })
            .set_task([&](int j) {
                LLT<Ref<MatrixXd>> llt(Mat.at({j,j}));
                assert(llt.info() == Eigen::Success);
            })
            .set_fulfill([&](int j) {
                // Dependencies
                map<int,vector<int>> to_fulfill; // rank -> trsm[i,j] on that rank at (i,j)
                for(int i = j+1; i<N; i++) {
                    int r = block2rank({i,j});
                    if(to_fulfill.count(r) == 0) {
                        to_fulfill[r] = {i};
                    } else {
                        to_fulfill[r].push_back(i);
                    }
                }
                // Send data & trigger tasks
                for(auto& p: to_fulfill) {
                    int r = p.first;
                    // Task is local. Just fulfill.
                    if(r == rank) {
                        for(auto& i: p.second) {
                            trsm_tf.fulfill_promise({i,j});
                        }
                    // Task is remote. Send data and fulfill.
                    } else {
                        auto Ljjv = view<double>(Mat.at({j,j}).data(), n*n);
                        auto isv = view<int>(p.second.data(), p.second.size());
                        am_trsm->send(r, Ljjv, j, isv);
                    }
                }
            })
            .set_name([](int j) {
                return "potf_" + to_string(j);
            })
            .set_priority([&](int j) {
                return block2prio({j,j});
            });

        // trsm
        trsm_tf.set_mapping([&](int2 ij) {
                return ((ij[0] + ij[1] * N) % n_threads);
            })
            .set_indegree([](int2 ij) {
                return (ij[1] == 0 ? 0 : 1) + 1;
            })
            .set_task([&](int2 ij) {
                int i = ij[0];
                int j = ij[1];
                auto L = Mat.at({j,j}).triangularView<Lower>().transpose();
                Mat.at({i,j}) = L.solve<OnTheRight>(Mat.at({i,j}));
            })
            .set_fulfill([&](int2 ij) {
                int i = ij[0];
                int j = ij[1];
                // Dependencies
                map<int,vector<int2>> to_fulfill; // rank -> gemm[i,j,k] on that rank at (i,j)
                for (int k = j+1; k<=i; k++) // on the right, row i, cols k=j+1, ..., i
                {
                    int r = block2rank({i,k});
                    if(to_fulfill.count(r) == 0) {
                        to_fulfill[r] = { {i,k} };
                    } else {
                        to_fulfill[r].push_back({i,k});
                    }
                }
                for (int k = i+1; k<N; k++) // below, col i, row k=i+1, ..., N
                {
                    int r = block2rank({k,i});
                    if(to_fulfill.count(r) == 0) {
                        to_fulfill[r] = { {k,i} };
                    } else {
                        to_fulfill[r].push_back({k,i});
                    }
                }
                // Send data & trigger tasks
                for(auto& p: to_fulfill) {
                    int r = p.first;
                    // Task is local. Just fulfill.
                    if(r == rank) {
                        for(auto& ij: p.second) {
                            int gi = ij[0];
                            int gj = ij[1];
                            int gk = j;
                            gemm_tf.fulfill_promise({gi,gj,gk});
                        }
                    // Task is remote. Send data and fulfill.
                    } else {
                        auto Lijv = view<double>(Mat.at({i,j}).data(), n*n);
                        auto ijsv = view<int2>(p.second.data(), p.second.size());
                        am_gemm->send_large(r, Lijv, i, j, ijsv);
                    }
                }
            })
            .set_name([](int2 ij) {
                return "trsm_" + to_string(ij[0]) + "_" + to_string(ij[1]);
            })
            .set_priority([&](int2 ij) {
                return block2prio(ij);
            });

        // gemm
        gemm_tf.set_mapping([&](int3 ijk) {
                return ((ijk[0] + ijk[1] * N + ijk[2] * N * N) % n_threads);
            })
            .set_indegree([](int3 ijk) {
                int i = ijk[0];
                int j = ijk[1];
                int k = ijk[2];
                return (k == 0 ? 0 : 1) + (i == j ? 1 : 2);
            })
            .set_task([&](int3 ijk) {
                int i = ijk[0];
                int j = ijk[1];
                int k = ijk[2];
                Mat.at({i,j}).noalias() -= Mat.at({i,k}) * Mat.at({j,k}).transpose();
                ASSERT_TRUE(k < N - 1);
            })
            .set_fulfill([&](int3 ijk) {
                int i = ijk[0];
                int j = ijk[1];
                int k = ijk[2];
                if (k + 1 == i && k + 1 == j)
                {
                    potf_tf.fulfill_promise(k+1); // same node, no comms
                }
                else if (k + 1 == j)
                {
                    trsm_tf.fulfill_promise({i, k + 1}); // same node, no comms
                }
                else
                {
                    gemm_tf.fulfill_promise({i, j, k + 1}); // same node, no comms
                }
            })
            .set_name([](int3 ijk) {
                return "gemm_" + to_string(ijk[0]) + "_" + to_string(ijk[1]) + "_" + to_string(ijk[2]);
            })
            .set_priority([&](int3 ijk) {
                return block2prio({ijk[0], ijk[1]});
            });

            if(rank == 0) printf("Starting Cholesky\n");
            MPI_Barrier(MPI_COMM_WORLD);
            timer t0 = wctime();
            if (rank == 0){
                potf_tf.fulfill_promise(0);
            }
            tp.join();
            timer t1 = wctime();
            MPI_Barrier(MPI_COMM_WORLD);
            if(rank == 0)
            {
                cout << "Time : " << elapsed(t0, t1) << endl;
            }

            if(LOG) {
                std::ofstream logfile;
                string filename = "cholesky_"+ to_string(n_ranks)+".log."+to_string(rank);
                logfile.open(filename);
                logfile << log;
                logfile.close();
            }
    }

    // Gather everything on rank 0 and test for accuracy
    {
        Communicator comm(MPI_COMM_WORLD, VERB);
        Threadpool tp(n_threads, &comm, VERB);
        Taskflow<int2> gather_tf(&tp, VERB);
        auto am_gather = comm.make_active_msg(
        [&](view<double> &Lij, int& i, int& j) {
        	A.block(i * n, j * n, n, n) = Map<MatrixXd>(Lij.data(), n, n);
        });
        // potf
        gather_tf.set_mapping([&](int2 ij) {
                return ( (ij[0] + ij[1]) % n_threads );
            })
            .set_indegree([](int2) {
                return 1;
            })
            .set_task([&](int2 ij) {
                int i = ij[0];
                int j = ij[1];
                if(rank != 0) {
                    auto Lijv = view<double>(Mat.at({i,j}).data(), n*n);
                    am_gather->send(0, Lijv, i, j);
                } else {
                    A.block(i * n, j * n, n, n) = Mat.at({i,j});
                }
            })
            .set_name([](int2 ij) {
                return "gather_" + to_string(ij[0]) + "_" + to_string(ij[1]);
            });

        for(int i = 0; i < N; i++) {
            for(int j = 0; j <= i; j++) {
                if(block2rank({i,j}) == rank) {
                    gather_tf.fulfill_promise({i,j});
                }
            }
        }
        tp.join();
        MPI_Barrier(MPI_COMM_WORLD);

        if(rank == 0) {
            // Test 1
            {
                auto L = A.triangularView<Lower>();
                VectorXd x = VectorXd::Random(n * N);
                VectorXd b = Aref*x;
                VectorXd bref = b;
                L.solveInPlace(b);
                L.transpose().solveInPlace(b);
                double error = (b - x).norm() / x.norm();
                cout << "Error solve: " << error << endl;
                EXPECT_LE(error, 1e-8);
            }
            // Test 2
            {
                // MatrixXd L = A.triangularView<Lower>();
                // MatrixXd A = L * L.transpose();
                // double error = (A - Aref).norm() / Aref.norm();
                // cout << "Error LLT : " << error << endl;
                // EXPECT_LE(error, 1e-8);
            }
        }
    }
}

TEST(cholesky, one)
{
    int n_threads = n_threads_;
  	int n = n_;
  	int N = N_;
    int p = p_;
    int q = q_;
    cholesky(n_threads, n, N, p, q);
}

class ManyTest : public ::testing::Test, public ::testing::WithParamInterface<tuple<int, int, int>> {};

TEST_P(ManyTest, MixedTwoSteps) {
    int n_threads, n, N;
    std::tie(n_threads, n, N) = GetParam();
    cholesky(n_threads, n, N, p_, q_);
}

INSTANTIATE_TEST_SUITE_P(
    cholesky, ManyTest,
    ::testing::Combine(
        ::testing::Values(1, 4),
        ::testing::Values(1, 5, 10),
        ::testing::Values(1, 16, 64)
    ),
    [](const ::testing::TestParamInfo<ManyTest::ParamType>& info) -> string {
        return "nt" + to_string(get<0>(info.param)) + "n" + to_string(get<1>(info.param)) + "N" + to_string(get<2>(info.param));
    }
);

int main(int argc, char **argv)
{
    int req = MPI_THREAD_FUNNELED;
    int prov = -1;

    MPI_Init_thread(NULL, NULL, req, &prov);

    assert(prov == req);

    ::testing::InitGoogleTest(&argc, argv);

    if (argc >= 2)
    {
        n_threads_ = atoi(argv[1]);
    }

    if (argc >= 3)
    {
        n_ = atoi(argv[2]);
    }

    if (argc >= 4)
    {
        N_ = atoi(argv[3]);
    }

    if (argc >= 5)
    {
        p_ = atoi(argv[4]);
    }

    if (argc >= 6)
    {
        q_ = atoi(argv[5]);
    }

    if (argc >= 7)
    {
        VERB = atoi(argv[6]);
    }

    const int return_flag = RUN_ALL_TESTS();

    MPI_Finalize();

    return return_flag;
}


#ifndef JSPACE_PSEUDO_INVERSE_HPP
#define JSPACE_PSEUDO_INVERSE_HPP

#include <Eigen/Dense>

namespace myUtils {
  void pseudoInverse(Eigen::MatrixXd const & matrix,
                     double sigmaThreshold,
                     Eigen::MatrixXd & invMatrix,
                     Eigen::VectorXd * opt_sigmaOut = 0);

  Eigen::MatrixXd getNullSpace(const Eigen::MatrixXd & J,
                               const double threshold = 0.00001);

  void weightedInverse(const Eigen::MatrixXd & J,
                       const Eigen::MatrixXd & Winv,
                       Eigen::MatrixXd & Jinv);
}

#endif


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015.
//  Copyright Paul Bristow 2015.
//  Distributed under the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt
//  or copy at http://www.boost.org/LICENSE_1_0.txt)

//! \file
//!\brief Tests for the fixed_point functions next, prior, distance, advance.

#define BOOST_TEST_MODULE test_negatable_func_next_prior
#define BOOST_LIB_DIAGNOSTIC

#include <boost/fixed_point/fixed_point.hpp>
#include <boost/test/included/unit_test.hpp>

// Basic tests using any type and any value (except max, lowest or -max).
template<typename T>
void tests(T value)
{
  using namespace boost::fixed_point;

  const T x(value);
  const T xn = fixed_next(x);
  const T xp = fixed_prior(x);

  BOOST_CHECK_EQUAL(fixed_prior(xn),  x); // next then prior should be x.
  BOOST_CHECK_EQUAL(fixed_next(xp), x);   // prior then next should be x.

  const T xx = fixed_next(xp);

  const bool b = (xx == x);

  BOOST_CHECK_EQUAL(b, true);

  const T np = fixed_next(xp);

  BOOST_CHECK_EQUAL(np.bit_pattern(), x.bit_pattern()); // prior then next should be x.
  BOOST_CHECK_EQUAL(np, x);
  BOOST_CHECK_EQUAL(fixed_distance(xp, xn), 2);
  BOOST_CHECK_EQUAL(fixed_advance(xp, 2), xn);
} // void tests.

BOOST_AUTO_TEST_CASE(test_negatable_func_next_prior)
{
  typedef boost::fixed_point::negatable< 0,   -7> fixed_point_type_0m7;     //  8-bit fixed_point type using all 7 bits for resolution.
  typedef boost::fixed_point::negatable< 2,   -5> fixed_point_type_2m5;     //  8-bit fixed_point type 2 range 5 resolution.
  typedef boost::fixed_point::negatable< 7,   -8> fixed_point_type_7m8;     //  16-bit fixed_point type with even split.
  typedef boost::fixed_point::negatable< 0,  -63> fixed_point_type_0m63;   //  64-bit fixed_point type using all 63 bits for resolution.
  typedef boost::fixed_point::negatable<15,  -48> fixed_point_type_15m48;  //  64-bit fixed_point type.
  typedef boost::fixed_point::negatable<15, -240> fixed_point_type_15m240; // 256-bit using multiprecision
  typedef boost::fixed_point::negatable<55, -200> big_fixed_point_type;    // 256-bit using multiprecision

  using boost::fixed_point::fixed_next;
  using boost::fixed_point::fixed_prior;
  using boost::fixed_point::fixed_advance;
  using boost::fixed_point::fixed_distance;

  // Simple test using unity and a single fixed_point type.
  const fixed_point_type_7m8 x(1);
  BOOST_CHECK_EQUAL(x.bit_pattern(), "0000000100000000");

  const fixed_point_type_7m8 xn = fixed_next(x);
  BOOST_CHECK_EQUAL(xn.bit_pattern(), "0000000100000001");

  const fixed_point_type_7m8 xp = fixed_prior(x);
  BOOST_CHECK_EQUAL(xp.bit_pattern(), "0000000011111111");
  BOOST_CHECK_EQUAL(fixed_prior(xn), x);
  BOOST_CHECK_EQUAL(fixed_next(xp), x);
  BOOST_CHECK_EQUAL(fixed_distance(xp, xn), 2);
  BOOST_CHECK_EQUAL(fixed_advance(xp, 2), xn);

  // Check that nextafter gives same results.
  // (Useful in case fixed_next, etc. are not implemented using nextafter).
  BOOST_CHECK_EQUAL(fixed_prior(x), nextafter(x, x - 1));
  BOOST_CHECK_EQUAL(fixed_next(x),  nextafter(x, x + 1));

  // Use 8-bit all solution bit type fixed_point_type_0m7
  // tests(fixed_point_type_0m7(-1)); // Cannot represent unity with this type.
  tests(fixed_point_type_0m7( 0));
  tests((std::numeric_limits<fixed_point_type_0m7>::min)()); // small value.

  // Repeat above tests using multiple types and multiple values.
  tests(fixed_point_type_7m8( 1));
  tests(fixed_point_type_7m8(-1));
  tests(fixed_point_type_7m8( 0));
  tests((std::numeric_limits<fixed_point_type_7m8>::min)()); // small value.

  tests(fixed_prior((std::numeric_limits<fixed_point_type_7m8>::max)()));   // penultimate value.
  tests(fixed_next ( std::numeric_limits<fixed_point_type_7m8>::lowest())); // penultimate value.

  //tests((std::numeric_limits<fixed_point_type_7m8>::max)());    // Expected to fail!
  //tests((std::numeric_limits<fixed_point_type_7m8>::lowest)()); // Expected to fail!

  // Use a 64-bit types:  fixed_point_type_15m48
  tests(fixed_point_type_15m48( 1));
  tests(fixed_point_type_15m48(-1));
  tests(fixed_point_type_15m48( 0));
  tests((std::numeric_limits<fixed_point_type_7m8>::min)()); // small value.
  // fixed_point_type_0m63 all resolution bits.
  tests((std::numeric_limits<fixed_point_type_0m63>::min)()); // small value.

  tests(fixed_prior((std::numeric_limits<fixed_point_type_15m48>::max)()));   // penultimate value.
  tests(fixed_next ( std::numeric_limits<fixed_point_type_15m48>::lowest())); // penultimate value.

    // Test using a big 256-bit type using multiprecision.
  tests(fixed_point_type_15m240( 1));
  tests(fixed_point_type_15m240(-1));
  tests(fixed_point_type_15m240( 0));
  tests((std::numeric_limits<fixed_point_type_15m240>::min)()); // small value.

  tests(fixed_prior((std::numeric_limits<fixed_point_type_15m240>::max)()));   // penultimate value.
  tests(fixed_next ( std::numeric_limits<fixed_point_type_15m240>::lowest())); // penultimate value.

  // Test using a big 256-bit type using multiprecision.
  tests(big_fixed_point_type( 1));
  tests(big_fixed_point_type(-1));
  tests(big_fixed_point_type( 0));
  tests((std::numeric_limits<big_fixed_point_type>::min)()); // small value.

  tests(fixed_prior((std::numeric_limits<big_fixed_point_type>::max)()));   // penultimate value.
  tests(fixed_next ( std::numeric_limits<big_fixed_point_type>::lowest())); // penultimate value.

  tests(fixed_point_type_2m5(0));

} // BOOST_AUTO_TEST_CASE(test_negatable_func_next_prior)


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <stdint.h>
#include <Eigen/Dense>

#include <boost/random/mersenne_twister.hpp>
#include <boost/math/special_functions/gamma.hpp>

#include <dpMM/global.hpp>

#ifndef PI
#  define PI 3.141592653589793
#endif
#define LOG_PI 1.1447298858494002
#define LOG_2 0.69314718055994529
#define LOG_2PI 1.8378770664093453

using namespace Eigen;

template<typename T>
class Distribution
{
public:
  Distribution(boost::mt19937* pRndGen) : pRndGen_(pRndGen)
  {};
  virtual ~Distribution()
  {};

//  virtual logProb()
  boost::mt19937* pRndGen_;
private:
};

template<typename T, typename T2>
inline Matrix<T,Dynamic,1> counts(const Matrix<T2,Dynamic,1> & z, T2 K)
{
  Matrix<T,Dynamic,1> N(K);
  N.setZero(K);
  for (T2 i=0; i<z.size(); ++i)
    N(z(i))++;
  return N;
};

//inline VectorXd counts(const VectorXu& z, uint32_t K)
//{
//  VectorXd N(K);
//  N.setZero(K);
//  for (uint32_t i=0; i<z.size(); ++i)
//    N(z(i))++;
//  return N;
//};

/* multivariate gamma function of dimension p */
inline double lgamma_mult(double x,uint32_t p)
{
  assert(x+0.5*(1.-p) > 0.);
  double lgam_p = p*(p-1.)*0.25*LOG_PI;
  for (uint32_t i=1; i<p+1; ++i)
  {
//    cout<<"digamma_mult of "<<(x + (1.0-double(i))/2)<<" = "<<digamma(x + (1.0-double(i))/2)<<endl;
    lgam_p += boost::math::lgamma(x + 0.5*(1.0-double(i)));
  }
  return lgam_p;
}

template<typename T>
inline T logsumexp(T x1, T x2)
{
   if (x1>x2)
      return x1 + log(1.+exp(x2-x1));
   else
      return x2 + log(1.+exp(x1-x2));
}


#include <Eigen/Core>
#include <iostream>
using namespace Eigen;
using namespace std;

template<typename Derived>
Eigen::VectorBlock<Derived>
segmentFromRange(MatrixBase<Derived>& v, int start, int end)
{
  return Eigen::VectorBlock<Derived>(v.derived(), start, end-start);
}

template<typename Derived>
const Eigen::VectorBlock<const Derived>
segmentFromRange(const MatrixBase<Derived>& v, int start, int end)
{
  return Eigen::VectorBlock<const Derived>(v.derived(), start, end-start);
}

int main(int, char**)
{
  Matrix<int,1,6> v; v << 1,2,3,4,5,6;
  cout << segmentFromRange(2*v, 2, 4) << endl; // calls the const version
  segmentFromRange(v, 1, 3) *= 5;              // calls the non-const version
  cout << "Now the vector v is:" << endl << v << endl;
  return 0;
}


///////////////////////////////////////////////////////////////
//  Copyright 2011 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_

#include <boost/multiprecision/gmp.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <iostream>

void t1()
{
   //[mpz_eg
   //=#include <boost/multiprecision/gmp.hpp>

   using namespace boost::multiprecision;

   mpz_int v = 1;

   // Do some arithmetic:
   for(unsigned i = 1; i <= 1000; ++i)
      v *= i;

   std::cout << v << std::endl; // prints 1000!

   // Access the underlying representation:
   mpz_t z;
   mpz_init(z);
   mpz_set(z, v.backend().data());
   //]
   mpz_clear(z);
}

void t2()
{
   //[mpf_eg
   //=#include <boost/multiprecision/gmp.hpp>

   using namespace boost::multiprecision;

   // Operations at variable precision and limited standard library support:
   mpf_float a = 2;
   mpf_float::default_precision(1000);
   std::cout << mpf_float::default_precision() << std::endl;
   std::cout << sqrt(a) << std::endl; // print root-2

   // Operations at fixed precision and full standard library support:
   mpf_float_100 b = 2;
   std::cout << std::numeric_limits<mpf_float_100>::digits << std::endl;
   // We can use any C++ std lib function:
   std::cout << log(b) << std::endl; // print log(2)
   // We can also use any function from Boost.Math:
   std::cout << boost::math::tgamma(b) << std::endl;
   // These even work when the argument is an expression template:
   std::cout << boost::math::tgamma(b * b) << std::endl;

   // Access the underlying representation:
   mpf_t f;
   mpf_init(f);
   mpf_set(f, a.backend().data());
   //]
   mpf_clear(f);
}

void t3()
{
   //[mpq_eg
   //=#include <boost/multiprecision/gmp.hpp>

   using namespace boost::multiprecision;

   mpq_rational v = 1;

   // Do some arithmetic:
   for(unsigned i = 1; i <= 1000; ++i)
      v *= i;
   v /= 10;

   std::cout << v << std::endl; // prints 1000! / 10
   std::cout << numerator(v) << std::endl;
   std::cout << denominator(v) << std::endl;

   mpq_rational w(2, 3);  // component wise constructor
   std::cout << w << std::endl; // prints 2/3

   // Access the underlying data:
   mpq_t q;
   mpq_init(q);
   mpq_set(q, v.backend().data());
   //]
   mpq_clear(q);
}

int main()
{
   t1();
   t2();
   t3();
   return 0;
}


/**
 * @file
 * @brief NPDE homework "Handling degrees of freedom (DOFs) in LehrFEM++"
 * @author Julien Gacon
 * @date March 1st, 2019
 * @copyright Developed at ETH Zurich
 */

#include "lfppdofhandling.h"

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

#include "lf/assemble/assemble.h"
#include "lf/base/base.h"
#include "lf/geometry/geometry.h"
#include "lf/mesh/mesh.h"
#include "lf/mesh/utils/utils.h"

namespace LFPPDofHandling {

/* SAM_LISTING_BEGIN_1 */
std::array<std::size_t, 3> countEntityDofs(
    const lf::assemble::DofHandler &dofhandler) {
  std::array<std::size_t, 3> entityDofs;
  //====================
  // Your code goes here
  // returns the number of global shape functions (managed by the local-> global index ing mapping encoded
  // in the dofhander) associated with mesh entities of co-domensioon 0, 1, and 2 respectively
  // the co-dimension also serves as index for the returned array

  // loop over all mesh entitesm get the inices of the global shape functions
  // associated with them via DofHandker::InteriorGlobalDofIndices()
  // and count them
  // this number is alo available through DofHandler::NumInteriorDofs()

  // iterate over entities in the mesh and get interior number of dofs for each
  std::shared_ptr<lf::mesh::Mesh> mesh = dofhandler.Mesh();
  for (int co_dim =0 ; co_dim <=2; co_dim++){
    entityDofs[co_dim] =0;
    for (auto el : mesh->Entities(co_dim)){
      entity_Dofs[co_dim] += dofhandler.interiorGlobalDofIndices(el);
    }
  }

  //====================
  return entityDofs;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
std::size_t countBoundaryDofs(const lf::assemble::DofHandler &dofhandler) {
  std::shared_ptr<const lf::mesh::Mesh> mesh = dofhandler.Mesh();
  // given an entity, bd\_flags(entity) == true, if the entity is on the
  // boundary
  lf::mesh::utils::AllCodimMeshDataSet<bool> bd_flags(
      lf::mesh::utils::flagEntitiesOnBoundary(mesh));
  std::size_t no_dofs_on_bd = 0;
  //====================
  // Your code goes here
  // tells the number of global shape functions associated with mesh entities located on the
  // boundary
  // use the function flagEntitiesonBoundary to obtain the array of flags whether an entity is located on the boundary
  // edges and nodes can be on the boundary;
  for (auto edge: mesh->Entity(1)){
    if (bd_flags(edge) == TRUE){
      no_dofs_on_bd += dofhandler.InteriorGlobalDofIndices(edge);
    }
  }

  for (auto node: mesh->Entity(2)){
    if (bd_flags(node) == TRUE){
      no_dofs_on_bd += dofhandler.InteriorGlobalDofIndices(node);
    }
  }

  //====================
  return no_dofs_on_bd;
}
/* SAM_LISTING_END_2 */

// clang-format off
/* SAM_LISTING_BEGIN_3 */
double integrateLinearFEFunction(
    const lf::assemble::DofHandler& dofhandler,
    const Eigen::VectorXd& mu) {
  double I = 0;
  //====================
  // Your code goes here
  // computes the ...
  // whose nodal bases coefficients are passed in mu
  // the dofhandler arguments provides the local-> global mapping
  // check whether the lf::assemble::DofHandler object dofhandler really fits the
  // lagrangian finite element space
  // run over all cells
  // get the indices of the tent functions covering them via
  //DofHandler::GlobalDofIndices,
  // sum the corresponding entries of the coefficient vectors
  // in linear Lagarangian FE, the integral over the basis functions
  // over a triangle K is 1/3*vol(k)

  std::shared_ptr<lf::mesh::Mesh> mesh = dofhandler.Mesh();

  for(auto cell: mesh->Entities(0)){
    // lf::assemble::DofHandler::GlobalDofIndices
    // access to indices of global dof's belonging to an entity
    auto glob_dof_indices = dofhandler.GlobalDofIndices(*cell);

    double I_dof =0;
    for(auto dof_indice = glob_dof_indices.start(); dof_indice =glob_dof_indices.end(); dof_indice++){
      I_dof += mu(dof_indice);
    }
    lf::geometry::Geometry *cell->Geometry();
    I_dof *= 1/3*lf::geometry::Volume(cell);
    I+=I_dof;
  }

  //====================
  return I;
}
/* SAM_LISTING_END_3 */
// clang-format on

/* SAM_LISTING_BEGIN_4 */
double integrateQuadraticFEFunction(const lf::assemble::DofHandler &dofhandler,
                                    const Eigen::VectorXd &mu) {
  double I = 0;
  //====================
  // Your code goes here
  //====================
  return I;
}
/* SAM_LISTING_END_4 */

/* SAM_LISTING_BEGIN_5 */
Eigen::VectorXd convertDOFsLinearQuadratic(
    const lf::assemble::DofHandler &dofh_Linear_FE,
    const lf::assemble::DofHandler &dofh_Quadratic_FE,
    const Eigen::VectorXd &mu) {
  if (dofh_Linear_FE.Mesh() != dofh_Quadratic_FE.Mesh()) {
    throw "Underlying meshes must be the same for both DOF handlers!";
  }
  std::shared_ptr<const lf::mesh::Mesh> mesh =
      dofh_Linear_FE.Mesh();                          // get the mesh
  Eigen::VectorXd zeta(dofh_Quadratic_FE.NumDofs());  // initialise empty zeta
  // safety guard: always set zero if you're not sure to set every entry later
  // on for us this shouldn't be a problem, but just to be sure
  zeta.setZero();

  for (const auto *cell : mesh->Entities(0)) {
    // check if the spaces are actually linear and quadratic
    //====================
    // Your code goes here
    //====================
    // get the global dof indices of the linear and quadratic FE spaces, note
    // that the vectors obey the LehrFEM++ numbering, which we will make use of
    // lin\_dofs will have size 3 for the 3 dofs on the nodes and
    // quad\_dofs will have size 6, the first 3 entries being the nodes and
    // the last 3 the edges
    //====================
    // Your code goes here
    // assign the coefficients of mu to the correct entries of zeta, use
    // the previous subproblem 2-9.a
    //====================
  }
  return zeta;
}
/* SAM_LISTING_END_5 */

}  // namespace LFPPDofHandling


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

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

template<typename Scalar,int Size> void hessenberg(int size = Size)
{
  typedef Matrix<Scalar,Size,Size> MatrixType;

  // Test basic functionality: A = U H U* and H is Hessenberg
  for(int counter = 0; counter < g_repeat; ++counter) {
    MatrixType m = MatrixType::Random(size,size);
    HessenbergDecomposition<MatrixType> hess(m);
    MatrixType Q = hess.matrixQ();
    MatrixType H = hess.matrixH();
    VERIFY_IS_APPROX(m, Q * H * Q.adjoint());
    for(int row = 2; row < size; ++row) {
      for(int col = 0; col < row-1; ++col) {
	VERIFY(H(row,col) == (typename MatrixType::Scalar)0);
      }
    }
  }

  // Test whether compute() and constructor returns same result
  MatrixType A = MatrixType::Random(size, size);
  HessenbergDecomposition<MatrixType> cs1;
  cs1.compute(A);
  HessenbergDecomposition<MatrixType> cs2(A);
  VERIFY_IS_EQUAL(cs1.matrixH().eval(), cs2.matrixH().eval());
  MatrixType cs1Q = cs1.matrixQ();
  MatrixType cs2Q = cs2.matrixQ();
  VERIFY_IS_EQUAL(cs1Q, cs2Q);

  // Test assertions for when used uninitialized
  HessenbergDecomposition<MatrixType> hessUninitialized;
  VERIFY_RAISES_ASSERT( hessUninitialized.matrixH() );
  VERIFY_RAISES_ASSERT( hessUninitialized.matrixQ() );
  VERIFY_RAISES_ASSERT( hessUninitialized.householderCoefficients() );
  VERIFY_RAISES_ASSERT( hessUninitialized.packedMatrix() );

  // TODO: Add tests for packedMatrix() and householderCoefficients()
}

EIGEN_DECLARE_TEST(hessenberg)
{
  CALL_SUBTEST_1(( hessenberg<std::complex<double>,1>() ));
  CALL_SUBTEST_2(( hessenberg<std::complex<double>,2>() ));
  CALL_SUBTEST_3(( hessenberg<std::complex<float>,4>() ));
  CALL_SUBTEST_4(( hessenberg<float,Dynamic>(internal::random<int>(1,EIGEN_TEST_MAX_SIZE)) ));
  CALL_SUBTEST_5(( hessenberg<std::complex<double>,Dynamic>(internal::random<int>(1,EIGEN_TEST_MAX_SIZE)) ));

  // Test problem size constructors
  CALL_SUBTEST_6(HessenbergDecomposition<MatrixXf>(10));
}


//######################################################################
//#   SDF_Fusion Module
//#
//#   Copyright (C) 2020 Siemens AG
//#   SPDX-License-Identifier: MIT
//#   Author 2020: This module has been developed by
//#                or under supervision of Slobodan Ilic
//#######################################################################

#ifndef UTILS_HPP
#define UTILS_HPP

#include <iostream>
#include <fstream>
#include <Eigen/Core>

template<typename T>
inline Eigen::Matrix<T, 3, 3> read_intrinsics_from_file(const std::string &in_file)
{
	std::vector<T> intrinsics_vector = read_matrix_one_line<T>(in_file);
	return matrix_from_vector_rowwise<T, 3, 3>(intrinsics_vector);
}

template<typename T>
inline Eigen::Matrix<T, 2, 1> project_point(const Eigen::Matrix<T, 3, 1> &point, const Eigen::Matrix<T, 3, 3> &intrinsics)
{
	Eigen::Matrix<T, 3, 1> projected = point / point(2);
	projected = intrinsics * projected;
	return { projected(0), projected(1) };
}

template<typename T, int R, int C>
inline Eigen::Matrix<T, R, C> matrix_from_vector_rowwise(std::vector<T> v)
{
	if (v.size() != R * C)
	{
		std::cerr << "Invalid input vector of size " << v.size() << " for " << R << "x" << C << " matrix" << std::endl;
		throw std::invalid_argument("Invalid vector size");
	}

	return Eigen::Map<Eigen::Matrix<T, R, C, Eigen::RowMajor>>(&v[0]);
}

template<typename T>
inline std::vector<T> read_matrix_one_line(const std::string &in_file)
{
	std::string line;
	std::vector<T> elements;

	std::ifstream f(in_file);

	while (std::getline(f, line))
	{
		stringstream ss(line);
		T current_value;

		while (ss >> current_value)
		{
			elements.push_back(current_value);
		}

	}

	return elements;
}

#endif


#include <iostream>
#include <boost/numeric/ublas/matrix.hpp>

typedef float                                RealNumber;
typedef boost::numeric::ublas::matrix<float> Matrix44;

Matrix44 mtxMulTranslate(Matrix44 matrix, RealNumber x, RealNumber y, RealNumber z) throw()
{
	matrix(0, 3) += matrix(0, 0)*x + matrix(0, 1)*y + matrix(0, 2)*z;
	matrix(1, 3) += matrix(1, 0)*x + matrix(1, 1)*y + matrix(1, 2)*z;
	matrix(2, 3) += matrix(2, 0)*x + matrix(2, 1)*y + matrix(2, 2)*z;
	matrix(3, 3) += matrix(3, 0)*x + matrix(3, 1)*y + matrix(3, 2)*z;
	return matrix;
}

int main()
{
	Matrix44 matrix = boost::numeric::ublas::identity_matrix<float> (4);

	mtxMulTranslate(matrix, 2.0f, 2.0f, 2.0f);

	return 0;
}


#include <Eigen/Core>
#include <chrono>
#include <fstream>
#include <iostream>

int main() {
  using namespace std;
  using namespace std::chrono;
  using namespace Eigen;
  int height = 1080;
  int width = 1920;
  int sv_count = 30;

  system_clock::time_point start, stop;
  double elapsed_time;

  int u_buffer_size = 3 * height * sv_count * sizeof(float);
  float *u_buffer = (float *)malloc(u_buffer_size);
  int v_buffer_size = 3 * sv_count * width * sizeof(float);
  float *v_buffer = (float *)malloc(v_buffer_size);
  int sv_buffer_size = 3 * sv_count * sizeof(float);
  float *sv_buffer = (float *)malloc(sv_buffer_size);
  int svd_buffer_size = 3 * width * height * sizeof(float);
  float *svd_buffer = (float *)malloc(svd_buffer_size);

  start = high_resolution_clock::now();
  std::ifstream svd_file("SVD_metadata_10/1.bin", std::ios::binary);
  svd_file.read((char *)sv_buffer, sv_buffer_size);
  svd_file.read((char *)u_buffer, u_buffer_size);
  svd_file.read((char *)v_buffer, v_buffer_size);
  svd_file.close();
  stop = high_resolution_clock::now();
  elapsed_time = duration<double, std::milli>(stop - start).count();
  cout << "Time to read file: " << elapsed_time << endl;

  Map<Matrix<float, Dynamic, Dynamic, RowMajor>> eigen_u_buffer(
      u_buffer, 3 * height, sv_count);
  Map<Matrix<float, Dynamic, Dynamic, RowMajor>> eigen_v_buffer(
      v_buffer, 3 * sv_count, width);
  Map<Matrix<float, Dynamic, 1>> eigen_svd_buffer(svd_buffer, 3 * sv_count);
  start = high_resolution_clock::now();
  for (int color = 0; color < 3; color++) {
    Map<Matrix<float, Dynamic, Dynamic, RowMajor>, 0, InnerStride<3>> eigen_svd(
        svd_buffer + color, height, width);
    eigen_svd =
        eigen_u_buffer.block(color * height, 0, height, sv_count) *
        eigen_svd_buffer.segment(color * sv_count, sv_count).asDiagonal() *
        eigen_v_buffer.block(color * sv_count, 0, sv_count, width);
  }
  stop = high_resolution_clock::now();
  elapsed_time = duration<double, std::milli>(stop - start).count();
  cout << "Time to recover SAT: " << elapsed_time << endl;

  free(u_buffer);
  free(v_buffer);
  free(sv_buffer);
  free(svd_buffer);
}


#pragma once

#include <ros/ros.h>

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

#include <algorithm>

namespace dr {

	/// A pose convention.
	struct PoseHeader {
		std::string parent_frame;
		std::string child_frame;
	};

	/// A pose with source and target frame information.
	struct Pose {
		PoseHeader header;
		Eigen::Isometry3d isometry;
	};

	/// Elementary axes.
	namespace axes {
		/// Get a vector representing the X axis.
		inline Eigen::Vector3d x() { return Eigen::Vector3d::UnitX(); }

		/// Get a vector representing the Y axis.
		inline Eigen::Vector3d y() { return Eigen::Vector3d::UnitY(); }

		/// Get a vector representing the Z axis.
		inline Eigen::Vector3d z() { return Eigen::Vector3d::UnitZ(); }
	}

	/// Create an aligned box with a center and dimensions.
	inline Eigen::AlignedBox3d makeCenteredBox(Eigen::Vector3d const & center, Eigen::Vector3d const & size) {
		return Eigen::AlignedBox3d(center - size / 2, center + size / 2);
	}

	/// Create a hyperplane with a position and normal.
	inline Eigen::Hyperplane<double, 3> makePlane(Eigen::Vector3d const & normal, Eigen::Vector3d const & point) {
		return Eigen::Hyperplane<double, 3>(normal, point);
	}

	/// Create a hyperplane from a pose.
	inline Eigen::Hyperplane<double, 3> makeXyPlane(Eigen::Isometry3d const & pose) {
		return Eigen::Hyperplane<double, 3>(pose.rotation() * axes::z(), pose.translation());
	}

	/// Create a translation from a vector.
	inline Eigen::Translation3d translate(Eigen::Vector3d translation) {
		return Eigen::Translation3d{translation};
	}

	/// Create a translation from X, Y and Z components.
	inline Eigen::Translation3d translate(double x, double y, double z) {
		return translate(Eigen::Vector3d{x, y, z});
	}

	/// Create a rotation with a given angle and axis.
	inline Eigen::AngleAxisd rotate(double angle, Eigen::Vector3d const & axis) {
		return Eigen::AngleAxisd{angle, axis};
	}

	/// Create a rotation with a given angle and axis and center of rotation.
	inline Eigen::Isometry3d rotate(double angle, Eigen::Vector3d const & axis, Eigen::Vector3d const & pivot_point) {
		return dr::translate(pivot_point) * Eigen::AngleAxisd{angle, axis} * dr::translate(-pivot_point);
	}

	/// Create a rotation around the X axis with a given angle.
	inline Eigen::AngleAxisd rotateX(double angle) {
		return dr::rotate(angle, axes::x());
	}

	/// Create a rotation around the X axis with a given angle and center of rotation.
	inline Eigen::Isometry3d rotateX(double angle, Eigen::Vector3d const & pivot_point) {
		return dr::rotate(angle, axes::x(), pivot_point);
	}

	/// Create a rotation around the Y axis with a given angle.
	inline Eigen::AngleAxisd rotateY(double angle) {
		return dr::rotate(angle, axes::y());
	}

	/// Create a rotation around the Y axis with a given angle and center of rotation.
	inline Eigen::Isometry3d rotateY(double angle, Eigen::Vector3d const & pivot_point) {
		return dr::rotate(angle, axes::y(), pivot_point);
	}

	/// Create a rotation around the Z axis with a given angle.
	inline Eigen::AngleAxisd rotateZ(double angle) {
		return dr::rotate(angle, axes::z());
	}

	/// Create a rotation around the Z axis with a given angle and center of rotation.
	inline Eigen::Isometry3d rotateZ(double angle, Eigen::Vector3d const & pivot_point) {
		return dr::rotate(angle, axes::z(), pivot_point);
	}

	/// Convert rpy to quaternions. Convention here is z-y-x.
	inline Eigen::Quaterniond rpyToQuaternion(double r, double p, double y) {
		return rotateZ(y) * rotateY(p) * rotateX(r);
	}

	/// Convert rpy to quaternions. Convention here is z-y-x.
	inline Eigen::Quaterniond rpyToQuaternion(Eigen::Vector3d const & rpy) {
		return rpyToQuaternion(rpy[0], rpy[1], rpy[2]);
	}

	/// Convert a quaternion to rpy. Convention here is z-y-x.
	inline Eigen::Vector3d quaternionToRpy(Eigen::Quaterniond const & quaternion) {
		Eigen::Vector3d rpy = quaternion.matrix().eulerAngles(2, 1, 0);

		std::swap(rpy[0], rpy[2]);

		return rpy;
	}

	/// Project vector a onto b.
	/**
	 * \return The projection of a onto b.
	 */
	template<typename A, typename B>
	auto projection(
		A const & a, ///< Vector a.
		B const & b  ///< Vector b.
	) -> decltype((a.dot(b) / b.dot(b)) * b) {
		return (a.dot(b) / b.dot(b)) * b;
	}

	/// Get the rejection of vector a onto b.
	/**
	 * \return The rejection of a onto b.
	 */
	template<typename A, typename B>
	auto rejection(
		A const & a, ///< Vector a.
		B const & b  ///< Vector b.
	) -> decltype(a - projection(a, b)) {
		return a - projection(a, b);
	}
}


#include <iostream>

#include <deal.II/base/tensor.h>

int main(int argc, char* argv[])
{
    const unsigned int dim = 2;

    dealii::Tensor<2, dim> T;

    std::cout << "T = ";

    for (unsigned int i = 0; i < T.n_independent_components; ++i)
    {
        auto indices = T.unrolled_to_component_indices(i);

        std::cout << "T_{" << indices[0] + 1 << indices[1] + 1 << "}" << " ";

    }

    for (unsigned int i = 1; i < (dim + 1); ++i)
    {
        for (unsigned int j = 1; j < (dim + 1); ++j)
        {
            T[i - 1][j - 1] = 10*i + j;
        }
    }

    std::cout << std::endl << std::endl << "T = " << T << std::endl;

    return 0;
}


// TestNonCentralChiSquared.cpp
//
// Test of noncentral chi^2 distribution (boost 1.36!!!). Once you understand
// the member functions and global functions for 1 kind of distribitions it becomes
// very easy to apply them to other kinds of distributions.
//
// Contents:
//
//	1. Statistcal properties of some distributions
//	2. Create a table of power of a test using non central chi^2, similar to boost except
//  we store all the data in an 1) normal matrix, 2) associative matrix. The advantage of approach
//  2 is that we can index using specific values of the degrees of freedom and non centrality parameter.
//
//	DJD, based on adaption of code in Chapter 3.
//
//	2009-1-8 DD creating look up tables
//
// (C) Datasim Education BV 2009
//

#include <boost/math/distributions/non_central_chi_squared.hpp> // for NC chi_squared_distribution
#include <boost/math/special_functions/cbrt.hpp>				// for chi_squared_distribution
#include <boost/math/distributions.hpp>							// For non-member functions of distributions

#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/distributions/poisson.hpp>

#include "UtilitiesDJD/AssociativeStructures/AssocArray.cpp"
#include "UtilitiesDJD/AssociativeStructures/AssocMatrix.cpp"

#include "UtilitiesDJD/CompileTimeVectorsAndMatrices/VectorSpace.cpp"
#include "UtilitiesDJD/CompileTimeVectorsAndMatrices/VectorSpaceMechanisms.cpp"
#include "UtilitiesDJD/VectorsAndMatrices/Vector.cpp"
#include "UtilitiesDJD/VectorsAndMatrices/NumericMatrix.cpp"
#include "UtilitiesDJD/VectorsAndMatrices/ArrayMechanisms.cpp"
#include "UtilitiesDJD/ExcelDriver/ExcelMechanisms.hpp"
#include "UtilitiesDJD/Geometry/Range.cpp"
#include "UtilitiesDJD/BitsAndPieces/StringConversions.hpp"
#include "UtilitiesDJD/ExceptionClasses/DatasimException.hpp"
#include "UtilitiesDJD/ExceptionClasses/DatasimException.hpp"
#include "UtilitiesDJD/RNG/UniformGenerator.hpp"
#include "UtilitiesDJD/RNG/NormalGenerator.hpp"

#include <iostream>
using namespace std;

template <typename Key1, typename Key2, typename Value>
	void print(const AssocMatrix<Key1, Key2, Value>& assMat)
{

	// Iterating in the map
	AssocMatrix<Key1, Key2, Value>::const_iterator iter = assMat.begin();

	for (long i = assMat.mat->MinRowIndex(); i <= assMat.mat->MaxRowIndex(); ++i)
	{
		cout << "Row " << (*iter).first << ":  ";
		for (long j = assMat.mat->MinColumnIndex(); j <= assMat.mat->MaxColumnIndex(); ++j)
		{
			cout << std::setprecision(3) << (*assMat.mat)(i,j) << ", ";
		}
		cout << endl;iter++;
	}

	cout << endl;
}

int main()
{

	// Significance level
	double level = 0.05;
	double cs;			// Temporary variable for chi^2 distribution

	// Now create the row and column indices
	VectorCollectionGenerator<double, double> dofRows; // Degrees of freedom
	dofRows.Start = 2.0;
	dofRows.Increment = 1.0;
	dofRows.Size = 9;
	Set<double> dofSet = createSet<double>(dofRows);

	VectorCollectionGenerator<double, double> nonCentralParameterColumns; // non cen parameter
	nonCentralParameterColumns.Start = 2.0;
	nonCentralParameterColumns.Increment = 2.0;
	nonCentralParameterColumns.Size = 5;
	Set<double> nonCentralParameterSet = createSet<double>(nonCentralParameterColumns);

	// Start values for rows and columns
	double r1 = dofRows.Start;
	double c1 = nonCentralParameterColumns.Start;

	// Lookup table dimensions
	long NRows = dofRows.Size;								// Degrees of freedom
	long NColumns = nonCentralParameterColumns.Size;		// Non-centrality parameter
	double incrementRow = dofRows.Increment;
	double incrementColumn = nonCentralParameterColumns.Increment;

	NumericMatrix<double, long> mat(NRows, NColumns);
	using namespace boost::math; // For convenience
	// Basic case, no associativity
	for (long r = mat.MinRowIndex(); r <= mat.MaxRowIndex(); ++r)
	{
		c1 = nonCentralParameterColumns.Start;
		for (long c = mat.MinColumnIndex(); c <= mat.MaxColumnIndex(); ++c)
		{
//		 cs = quantile(complement(chi_squared(r1), 0.05));
//		 mat(r,c)=cdf(complement(non_central_chi_squared(r1,c1),cs));
//		 c1 += incrementColumn;
		 cs = quantile(chi_squared(r1), 0.05);
		 mat(r,c)=cdf(non_central_chi_squared(r1,c1),cs);
		 c1 += incrementColumn;
		}

		r1 += incrementRow;
	}

	// Now create the associative matrix
	AssocMatrix<double, double, double> myAssocMat(dofSet, nonCentralParameterSet, mat);
	print(myAssocMat);

	printAssocMatrixInExcel(myAssocMat, string("NCCQT"));

    return 0;
}

windows/CsForFinancialMarketsPart2/Chapters20+21+22+23/Demos - CLI-CS Interop with Excel/CLI Interop Test (Chi-Squared)/CPPVersion2/Main.cpp


//
//  main.cpp
//  CameraCalibration
//
//  Created by Michael Schwendeman on 8/11/15.
//  Copyright (c) 2015 MSS. All rights reserved.
//

#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
#include <boost/filesystem.hpp>

using namespace cv;
using namespace std;
using namespace boost::filesystem;

int main() {
    // Set plot options
    bool plotUndistort = true;
    bool plotCheckerboard = false;
    if (plotCheckerboard || plotUndistort)
        namedWindow("Image", CV_WINDOW_AUTOSIZE); //create window for left image

    // Set path
    path p = "/Volumes/Data/PAPA/TGTcruise2015/StereoSystem/15Jan2015/1740UTC_flea35";
    //path p = "/Volumes/Data/PAPA/TGTcruise2015/StereoSystem/15Jan2015/1804UTC_flea34";
    //path p = "/Volumes/Data/PAPA/TGTcruise2015/StereoSystem/15Jan2015/1750UTC_flea65";
    //path p = "/Volumes/Data/PAPA/TGTcruise2015/StereoSystem/15Jan2015/1834UTC_flea18";
    //path p = "/Volumes/Data/PAPA/TGTcruise2015/StereoSystem/15Jan2015/1811UTC_flea21";
    //path p = "/Volumes/Data/PAPA/TGTcruise2015/StereoSystem/15Jan2015/1825UTC_flea83";

    vector<path> imageVec;
    copy(directory_iterator(p),directory_iterator(),back_inserter(imageVec));

    // Setup checkerboard
    int numCornersHor = 7;
    int numCornersVer = 4;
    int numSquares = numCornersHor * numCornersVer;
    Size board_sz = Size(numCornersHor, numCornersVer);
    vector<vector<Point3f>> object_points;
    vector<vector<Point2f>> image_points;
    vector<Point2f> corners;
    vector<Point3f> obj;
    for(int j=0;j<numSquares;j++)
        obj.push_back(Point3f((j/numCornersHor)*2*25.4, (j%numCornersHor)*2*25.4, 0.0f));

    // Detect checkerboard
    Mat image;
    string filename;
    int skip = 100;
    int iframe = 0;
    for ( vector<path>::const_iterator it = imageVec.begin(); it != imageVec.end(); ++it )
    {
        if ( !(it->extension().empty()) && (it->extension().string() == ".pgm") && (iframe % skip == 0) )
        {
            filename = it->string();
            image = imread(filename,CV_LOAD_IMAGE_GRAYSCALE);
            bool found = findChessboardCorners(image, board_sz, corners, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);

            if(found)
            {
                cornerSubPix(image, corners, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1));
                if (plotCheckerboard)
                {
                    drawChessboardCorners(image, board_sz, corners, found);
                    imshow("Image",image);
                    waitKey(0);
                }
                image_points.push_back(corners);
                object_points.push_back(obj);
            }
        }
        iframe++;
    }

    // Calculate calibration
    Mat intrinsic = Mat(3, 3, CV_32FC1);
    Mat distCoeffs;
    vector<Mat> rvecs;
    vector<Mat> tvecs;
    calibrateCamera(object_points, image_points, image.size(), intrinsic, distCoeffs, rvecs, tvecs, CV_CALIB_ZERO_TANGENT_DIST, TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 1000, DBL_EPSILON));

    // Check undistort
    iframe = 0;
    if (plotUndistort)
    {
        namedWindow("Undistorted", CV_WINDOW_AUTOSIZE); //create window for left image
        Mat imageUndistorted;
        for (vector<path>::const_iterator it (imageVec.begin()); it != imageVec.end(); ++it)
        {
            if ( !(it->extension().empty()) && (it->extension().string() == ".pgm") && (iframe % skip == 0) )
            {
                filename = it->string();
                image = imread(filename);
                undistort(image, imageUndistorted, intrinsic, distCoeffs);
                imshow("Image", image);
                imshow("Undistorted", imageUndistorted);
                waitKey(0);
            }
            iframe++;
        }
        destroyWindow("Undistorted"); //create window for left image
    }

    // Close windows
    if (plotCheckerboard || plotUndistort)
        destroyWindow("Image"); //create window for left image

    // Save intrinsic matrix and distortion coefficients
    string outputFile;
    outputFile = p.string() + "/OpenCVCalibrationResults.yml";
    FileStorage fs(outputFile, FileStorage::WRITE);
    fs << "Intrinsic Matrix" << intrinsic;
    fs << "Distortion Coefficients" << distCoeffs;
    fs.release();
    return 0;
}


#include "truncated_svd.hpp"

#include <gtest/gtest.h>

#include <armadillo>
#include <cmath>
#include <limits>
#include <random>
#include <stdexcept>
#include <vector>

#include "thin_lq.hpp"

template <typename Real>
void CreateTestMatrix(long m, long n, long r, arma::Mat<Real> &A,
                      long &rank_cutoff, Real &absolute_tolerance,
                      Real &relative_tolerance) {
    if (m < 1 || n < 1) {
        throw std::invalid_argument(
            "Number of rows and columns must be positive");
    }

    // Create Ut factor
    arma::Mat<Real> Ut;
    {
        arma::Mat<Real> temp(r, m, arma::fill::randn);

        arma::Mat<Real> L;
        ThinLq<Real>(temp, L, Ut);
    }

    // Create s factor; s[i] ~ Uniform([(r - 1 - i), (r - i)])
    arma::Col<Real> s(r, arma::fill::randu);
    for (long i = 0; i < r; ++i) {
        s(i) += r - i - 1;
    }

    // Create Vt factor
    arma::Mat<Real> Vt;
    {
        arma::Mat<Real> temp(r, n, arma::fill::randn);

        arma::Mat<Real> L;
        ThinLq<Real>(temp, L, Vt);
    }

    // Compute A = Ut.conjugate() * diagmat(s) * Vt
    A = Ut.t() * arma::diagmat(s) * Vt;

    // Compute absolute and relative errors given specified maximum rank
    std::vector<Real> absolute_error(r);

    absolute_error[r - 1] = static_cast<Real>(0);
    for (long i = r - 1; i > 0; --i) {
        absolute_error[i - 1] = absolute_error[i] + std::pow(s(i), 2);
    }
    Real frobenius_norm = absolute_error[0] + std::pow(s(0), 2);

    for (long i = 0; i < r; ++i) {
        absolute_error[i] = std::sqrt(absolute_error[i]);
    }
    frobenius_norm = std::sqrt(frobenius_norm);

    std::vector<Real> relative_error(r);

    for (long i = 0; i < r; ++i) {
        relative_error[i] = absolute_error[i] / frobenius_norm;
    }

    // Determine 75% Frobenius norm cutoff rank
    rank_cutoff = 1;
    while (rank_cutoff <= r) {
        if (relative_error[rank_cutoff - 1] <= static_cast<Real>(0.25)) {
            break;
        }
        ++rank_cutoff;
    }

    // Compute accuracy levels based on cutoff rank
    absolute_tolerance =
        (absolute_error[rank_cutoff - 2] + absolute_error[rank_cutoff - 1]) / 2;
    relative_tolerance =
        (relative_error[rank_cutoff - 2] + relative_error[rank_cutoff - 1]) / 2;
}

template <typename Real>
void SvdFactorQualityTest(const arma::Mat<Real> &U, const arma::Col<Real> &s,
                          const arma::Mat<Real> &V) {
    const long m = U.n_rows;
    const long n = V.n_rows;
    const long r = s.n_rows;

    for (long j1 = 0; j1 < r; ++j1) {
        for (long j2 = 0; j2 < j1; ++j2) {
            Real dot_product = static_cast<Real>(0);
            for (long i = 0; i < m; ++i) {
                dot_product += U(i, j1) * U(i, j2);
            }

            ASSERT_NEAR(dot_product, static_cast<Real>(0),
                        100 * std::numeric_limits<Real>::epsilon());
        }

        Real norm_squared = static_cast<Real>(0);
        for (long i = 0; i < m; ++i) {
            norm_squared += std::pow(U(i, j1), 2);
        }

        ASSERT_NEAR(norm_squared, static_cast<Real>(1),
                    100 * std::numeric_limits<Real>::epsilon());
    }

    for (long i = 0; i < r; ++i) {
        if (i < r - 1) {
            ASSERT_GE(s(i), s(i + 1));
        } else {
            ASSERT_GT(s(i), static_cast<Real>(0));
        }
    }

    for (long j1 = 0; j1 < r; ++j1) {
        for (long j2 = 0; j2 < j1; ++j2) {
            Real dot_product = static_cast<Real>(0);
            for (long i = 0; i < n; ++i) {
                dot_product += V(i, j1) * V(i, j2);
            }

            ASSERT_NEAR(dot_product, static_cast<Real>(0),
                        100 * std::numeric_limits<Real>::epsilon());
        }

        Real norm_squared = static_cast<Real>(0);
        for (long i = 0; i < n; ++i) {
            norm_squared += std::pow(V(i, j1), 2);
        }

        ASSERT_NEAR(norm_squared, static_cast<Real>(1),
                    100 * std::numeric_limits<Real>::epsilon());
    }
}

template <typename Real>
void ThinSvdTest(long m, long n, long r) {
    if (m < 1 || n < 1 || r < 1) {
        throw std::invalid_argument("Matrix sizes and rank must be positive");
    }

    if (r > m || r > n) {
        throw std::invalid_argument("Rank must be smaller than matrix size");
    }

    // Construct appropriate matrix
    arma::Mat<Real> A;
    long rank_cutoff;
    Real absolute_tolerance;
    Real relative_tolerance;
    CreateTestMatrix<Real>(m, n, r, A, rank_cutoff, absolute_tolerance,
                           relative_tolerance);

    // Test
    arma::Mat<Real> U;
    arma::Col<Real> s;
    arma::Mat<Real> V;
    long rank;
    TruncatedSvd<Real>(A, 0, false, U, s, V, rank);

    const Real frobenius_error =
        arma::norm(A - U * arma::diagmat(s) * V.t(), "fro");

    ASSERT_EQ(rank, std::min(m, n));
    SvdFactorQualityTest<Real>(U, s, V);
    ASSERT_LE(frobenius_error, 1000 * std::numeric_limits<Real>::epsilon());
}

template <typename Real>
void TruncatedSvdAbsoluteToleranceTest(long m, long n, long r) {
    if (m < 1 || n < 1 || r < 1) {
        throw std::invalid_argument("Matrix sizes and rank must be positive");
    }

    if (r > m || r > n) {
        throw std::invalid_argument("Rank must be smaller than matrix size");
    }

    // Construct appropriate matrix
    arma::Mat<Real> A;
    long rank_cutoff;
    Real absolute_tolerance;
    Real relative_tolerance;
    CreateTestMatrix<Real>(m, n, r, A, rank_cutoff, absolute_tolerance,
                           relative_tolerance);

    // Test
    arma::Mat<Real> U;
    arma::Col<Real> s;
    arma::Mat<Real> V;
    long rank;
    TruncatedSvd<Real>(A, absolute_tolerance, false, U, s, V, rank);

    const Real frobenius_error =
        arma::norm(A - U * arma::diagmat(s) * V.t(), "fro");

    ASSERT_EQ(rank, rank_cutoff);
    SvdFactorQualityTest<Real>(U, s, V);
    ASSERT_LE(frobenius_error, absolute_tolerance);
}

template <typename Real>
void TruncatedSvdRelativeToleranceTest(long m, long n, long r) {
    if (m < 1 || n < 1 || r < 1) {
        throw std::invalid_argument("Matrix sizes and rank must be positive");
    }

    if (r > m || r > n) {
        throw std::invalid_argument("Rank must be smaller than matrix size");
    }

    // Construct appropriate matrix
    arma::Mat<Real> A;
    long rank_cutoff;
    Real absolute_tolerance;
    Real relative_tolerance;
    CreateTestMatrix<Real>(m, n, r, A, rank_cutoff, absolute_tolerance,
                           relative_tolerance);

    // Test
    arma::Mat<Real> U;
    arma::Col<Real> s;
    arma::Mat<Real> V;
    long rank;
    TruncatedSvd<Real>(A, relative_tolerance, true, U, s, V, rank);

    const Real frobenius_norm = arma::norm(A, "fro");
    const Real frobenius_error =
        arma::norm(A - U * arma::diagmat(s) * V.t(), "fro");

    ASSERT_EQ(rank, rank_cutoff);
    SvdFactorQualityTest<Real>(U, s, V);
    ASSERT_LE(frobenius_error, relative_tolerance * frobenius_norm);
}

TEST(TruncatedSvd, ThinSvd_ShortMatrix) {
    ThinSvdTest<float>(16, 64, 8);
    ThinSvdTest<double>(16, 64, 8);
}

TEST(TruncatedSvd, ThinSvd_SquareMatrix) {
    ThinSvdTest<float>(32, 32, 8);
    ThinSvdTest<double>(32, 32, 8);
}

TEST(TruncatedSvd, ThinSvd_TallMatrix) {
    ThinSvdTest<float>(64, 16, 8);
    ThinSvdTest<double>(64, 16, 8);
}

TEST(TruncatedSvd, TruncatedSvdAbsoluteTolerance_ShortMatrix) {
    TruncatedSvdAbsoluteToleranceTest<float>(16, 64, 8);
    TruncatedSvdAbsoluteToleranceTest<double>(16, 64, 8);
}

TEST(TruncatedSvd, TruncatedSvdAbsoluteTolerance_SquareMatrix) {
    TruncatedSvdAbsoluteToleranceTest<float>(32, 32, 8);
    TruncatedSvdAbsoluteToleranceTest<double>(32, 32, 8);
}

TEST(TruncatedSvd, TruncatedSvdAbsoluteTolerance_TallMatrix) {
    TruncatedSvdAbsoluteToleranceTest<float>(64, 16, 8);
    TruncatedSvdAbsoluteToleranceTest<double>(64, 16, 8);
}

TEST(TruncatedSvd, TruncatedSvdRelativeTolerance_ShortMatrix) {
    TruncatedSvdRelativeToleranceTest<float>(16, 64, 8);
    TruncatedSvdRelativeToleranceTest<double>(16, 64, 8);
}

TEST(TruncatedSvd, TruncatedSvdRelativeTolerance_SquareMatrix) {
    TruncatedSvdRelativeToleranceTest<float>(32, 32, 8);
    TruncatedSvdRelativeToleranceTest<double>(32, 32, 8);
}

TEST(TruncatedSvd, TruncatedSvdRelativeTolerance_TallMatrix) {
    TruncatedSvdRelativeToleranceTest<float>(64, 16, 8);
    TruncatedSvdRelativeToleranceTest<double>(64, 16, 8);
}

int main(int argc, char **argv) {
    ::testing::InitGoogleTest(&argc, argv);

    return RUN_ALL_TESTS();
}


/**
 * This code has been downloaded from the website of Bojan Nikolic
 * see http://www.bnikolic.co.uk/blog/cpp-khachiyan-min-cov-ellipsoid.html
 * the code implements Khachiyan's algorithm to compute the Minimum volume
 * enclosing ellipsoid approximately.
 *
 * Some parts were written inefficiently so have been changed.
 */

#include "MVE.h"
#include "RMSUtils.h"
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include "cholesky.h"

namespace ublas=boost::numeric::ublas;

template<class T>
bool InvertMatrix(const ublas::matrix<T> &input, ublas::matrix<T> &inverse) {
    using namespace boost::numeric::ublas;

    typedef permutation_matrix<std::size_t> pmatrix;
    matrix<T> A(input);
    pmatrix pm(A.size1());
    int res = lu_factorize(A, pm);
    if (res != 0) return false;
    inverse.assign(identity_matrix<T>(A.size1()));
    lu_substitute(A, pm, inverse);
    return true;
}

void InvertLP(const ublas::matrix<double> &Lambdap, ublas::matrix<double> &LpInv) {
    bool res = InvertMatrix(Lambdap, LpInv);
    if (not res) {
        throw std::runtime_error("Could not invert Matrix");
    }
}

void Lift(const ublas::matrix<double> &A, ublas::matrix<double> &Ap) {
    Ap.resize(A.size1() + 1,
              A.size2());
    ublas::matrix_range<ublas::matrix<double> >
            sub(Ap,
                ublas::range(0, A.size1()),
                ublas::range(0, A.size2()));
    sub.assign(A);
    ublas::row(Ap, Ap.size1() - 1) = ublas::scalar_vector<double>(A.size2(), 1.0);

}

void genDiag(const ublas::vector<double> &p,
             ublas::matrix<double> &res) {
    res.assign(ublas::zero_matrix<double>(p.size(),
                                          p.size()));
    for (size_t i = 0; i < p.size(); ++i) {
        res(i, i) = p(i);
    }
}

void KaLambda(const ublas::matrix<double> &Ap,
              const ublas::vector<double> &p,
              ublas::matrix<double> &Lambdap) {
    //cout << "KaLambda: Ap.size1() = " << Ap.size1() << ", Ap.size2() = "
    //	 << Ap.size2() << ", p.size() = " << p.size() << endl;
    assert(p.size() == Ap.size2());

    /**
        This code is very inefficient (It is trying to allocate a matrix
        of size m x m where m = # of points which is simply infeasible
        for large data sets

        It is basically trying to compute the sum of p_i q_i q_i^t for
        i in 1 to m where m is the number of points and p is a m dimensional
        vector p = (p_1, ...., p_m)^t. Since each q_i is a n x 1 vector
        the return value of this is a n x n matrix.
     **/
    /*
    ublas::matrix<double> dp(p.size(), p.size());
    genDiag(p, dp);

    dp=ublas::prod(dp, ublas::trans(Ap));
    Lambdap=ublas::prod(Ap,
                        dp);
    */

    /* Recall that Lambda(p) is a matrix of size n x n where it is simply
       defined as Lambda(p) = sum of p_i q_i q_i^t for i from 1 to m
       and q_i are the n x 1 vectors representing the (lifted points)
       The matrix Ap is (q_i .... q_m). It is a n x m matrix
    */
    size_t n = Ap.size1();
    ublas::matrix<double> sum(n, n);
    sum.assign(ublas::zero_matrix<double>(n, n));

    /* directly invoke the correct formula for matrix product */
    for (size_t i = 0; i < p.size(); i++) {
        for (size_t j = 0; j < n; j++)
            for (size_t k = 0; k < n; k++)
                sum(j, k) += p(i) * Ap(j, i) * Ap(k, i);
    }
    Lambdap = sum;
}

double KhachiyanIter(const ublas::matrix<double> &Ap,
                     ublas::vector<double> &p) {
    /// Dimensionality of the problem
    const size_t d = Ap.size1() - 1;

    ublas::matrix<double> Lp;
    KaLambda(Ap, p, Lp);
    ublas::matrix<double> ILp(Lp.size1(), Lp.size2());
    InvertLP(Lp, ILp);

    /**
        * This code is very inefficient as it is trying to allocate
        * a matrix of size m x m where m is the number of data
        * points simply to get the diagonal elements of M !!

        * Basically it is trying to do the following:
	* M = ILp * Ap where ILp is a n x n matrix and
	* Ap is a n x m matrix (n is dimension, m is number
	* of points). The needed output is a set of m numbers
	* where the i-th number is basically <Ap_i^t, M_i>
	* i.e. the dot product of the transpose of the i-th
	* column of Ap and the i-th column of M.

    ublas::matrix<double> M;
    M=ublas::prod(ILp, Ap);
    M=ublas::prod(ublas::trans(Ap), M);

    double maxval=0;
    size_t maxi=0;
    for(size_t i=0; i<M.size1(); ++i)
    {
        if (M(i,i) > maxval)
        {
            maxval=M(i,i);
            maxi=i;
        }
    }
    **/

    double maxval = 0;
    size_t maxi = 0;
    ublas::matrix<double> M = ublas::prod(ILp, Ap);
    ublas::matrix<double> Apt = ublas::trans(Ap);
    assert(M.size1() == Apt.size2());

    for (size_t i = 0; i < Ap.size2(); i++) {
        //evaluate the product c_i^t (ILp) c_i
        ublas::matrix_column<ublas::matrix<double> > mc(M, i);
        ublas::matrix_row<ublas::matrix<double> > mr(Apt, i);

        //evaluate the left product of row and column
        double prod = 0;
        for (size_t j = 0; j < mc.size(); j++)
            prod += mr(j) * mc(j);

        if (prod > maxval) {
            maxval = prod;
            maxi = i;
        }

    }

    const double step_size = (maxval - d - 1) / ((d + 1) * (maxval - 1));
    ublas::vector<double> newp = p * (1 - step_size);
    newp(maxi) += step_size;

    const double err = ublas::norm_2(newp - p);
    p = newp;
    return err;

}

void KaInvertDual(const ublas::matrix<double> &A,
                  const ublas::vector<double> &p,
                  ublas::matrix<double> &Q,
                  ublas::vector<double> &c
) {
    const size_t d = A.size1();
    /** This part of the code is not efficient. It is trying
        to allocate a matrix dp of size m x m where m is the
        number of points

	Basically it is trying to do the following:
	Here A is a n x m matrix (where n is dimension
	and m is number of points)

	The output of this code is PN a m x n matrix where
	the row i is the transpose of the i-th column of A
	multiplied by p_i where p is a m-vector.
    **/
/*
    ublas::matrix<double> dp(p.size(), p.size());
    genDiag(p, dp);

    ublas::matrix<double> PN=ublas::prod(dp, ublas::trans(A));
*/
    //========= Begin replacement code ===============
    ublas::matrix<double> PN = ublas::trans(A);
    assert(p.size() == PN.size1());
    for (size_t i = 0; i < PN.size1(); i++)
        for (size_t j = 0; j < PN.size2(); j++)
            PN(i, j) *= p(i);
    //======== End replacement code ===================

    PN = ublas::prod(A, PN);

    ublas::vector<double> M2 = ublas::prod(A, p);
    ublas::matrix<double> M3 = ublas::outer_prod(M2, M2);

    ublas::matrix<double> invert(PN.size1(), PN.size2());
    InvertLP(PN - M3, invert);

    Q.assign(1.0 / d * invert);
    c = ublas::prod(A, p);
}

double KhachiyanAlgo(const ublas::matrix<double> &A,
                     double eps,
                     size_t maxiter,
                     ublas::matrix<double> &Q,
                     ublas::vector<double> &c) {
    ublas::vector<double> p = ublas::scalar_vector<double>(A.size2(), 1.0) * (1.0 / A.size2());

    ublas::matrix<double> Ap;
    Lift(A, Ap);

    double ceps = eps * 2;
    for (size_t i = 0; i < maxiter && ceps > eps; ++i) {
        ceps = KhachiyanIter(Ap, p);
    }

    KaInvertDual(A, p, Q, c);

    return ceps;
}

void print_matrix(const ublas::matrix<double> &A) {
    for (size_t i = 0; i < A.size1(); i++) {
        for (size_t j = 0; j < A.size2(); j++)
            cout << A(i, j) << "  ";
        cout << endl;
    }
}

void print_vector(const ublas::vector<double> &v) {
    if (v.size() == 1) {
        cout << v(0) << endl;
        return;
    }

    cout << "---     ---" << endl;
    for (size_t i = 0; i < v.size(); i++) {
        cout << "||" << " " << v(i) << " " << "||" << endl;
    }

    cout << "---     ---" << endl;
}

void MVEUtil::GetNormalizedMVE(const vector<Point> &dataP,
                               float epsilon,
                               vector<Point> &normalizedP,
                               double &outer_rad,
                               double &inner_rad
) {
    using namespace boost::numeric::ublas;

    if (dataP.size() == 0)
        return;

    size_t d = dataP[0].get_dimension();

    ublas::matrix<double> A(d, dataP.size());

    size_t j = 0;

    for (size_t i = 0; i < dataP.size(); i++) {
        for (size_t k = 0; k < d; k++)
            A(k, j) = dataP[i].get_coordinate(k);
        ++j;
    }

    //try Khachiyans algorithm
    //cout << "Running Khachiyan algorithm " << endl;

    ublas::matrix<double> Q(d, d);
    ublas::vector<double> c(d);

    size_t maxiter = 1024 * dataP.size() * d;
    double ceps = KhachiyanAlgo(A, epsilon, maxiter, Q, c);
    if (ceps > epsilon) {
        throw std::runtime_error("Khachiyan failed ... ");
    }

    /** Only for debug
    cout << "Khachiyan returned " << ceps << endl;
    cout << "Printing the matrix Q " << endl;
    print_matrix(Q);
    **/

    /**
        The equation of the ellipse that is returned
	by Khachiyan algorithm is (x - c)^T Q (x - c) <= 1
        Unfortunately all the points of the data set
	do not satisfy it (but they do satisfy it
	approximately). Now we can put (1+epsilon) * dim
	on the right hand side and the points will
	still satisfy it, but this may be too loose
	an approximation. Instead, we find the maximum
	of (x-c)^T Q (x-c) over all the data points,
	add 0.01, and take that as the RHS.
	The square root of this gives the radius of
	the outer sphere, when we apply a linear
	transform to turn the ellipsoid into a ball
    **/
    double max_val = 0;
    for (size_t i = 0; i < dataP.size(); i++) {
        ublas::vector<double> ublasp = Point::to_ublas(dataP[i]);

        //compute p - c
        ublasp = ublasp - c;

        //compute Q (p - c)
        ublas::vector<double> prod1 = ublas::prod(Q, ublasp);

        //compute (p - c)^T
        ublas::matrix<double> ublasptr(1, d);
        for (size_t j = 0; j < d; j++)
            ublasptr(0, j) = ublasp[j];

        //compute (p-c)^T Q (p-c)
        ublas::vector<double> x = ublas::prod(ublasptr, prod1);

        if (x(0) > max_val) max_val = x(0);

        /** for debug
        cout << "Printing (p-c)^T Q (p -c ) " ;
        print_vector(x);
        cout << endl;
        **/
    }

    //the radius of the outer sphere
    outer_rad = sqrt(max_val + 0.01);

    //now compute the linear transformation
    ublas::matrix<double> L(d, d);
    //compute a Cholesky factorization
    int res = cholesky_decompose(Q, L);

    if (res != 0) {
        cout << "Cholesky decomposition failed " << res << endl;
        exit(1);
    }

    /** debug
    cout << "Printing cholesky decomposition " << endl;
    print_matrix(L);
    **/

    //get the transpose of L
    ublas::matrix<double> LTr(d, d);
    LTr = ublas::trans(L);

    //ublas::matrix<double> chprod = ublas::prod(L, LTr);
    //print_matrix(chprod);

    normalizedP = dataP;

    for (size_t i = 0; i < dataP.size(); i++) {
        ublas::vector<double> ublasp = Point::to_ublas(normalizedP[i]);

        //compute the linear transformation
        ublasp = ublasp - c;
        ublasp = ublas::prod(LTr, ublasp);
        normalizedP[i] = Point::from_ublas(ublasp);
    }

    inner_rad = 1 / ((1 + epsilon) * d);

    /* save this matrix into RandomUtil class for further use */
    /* the use requires the inverse of (matrix transpose) of Ltr */
    ublas::matrix<double> IL(d, d);
    InvertLP(L, IL);
    RMSUtils::dimension = d;
    RMSUtils::transformation_matrix = IL;
    RMSUtils::center = c;

    return;
}


/**
 * @brief Tests for GeoLib::Surface::isPntInSfc()
 *
 * @copyright
 * Copyright (c) 2012-2019, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/LICENSE.txt
 */

#include <array>
#include <memory>
#include <random>
#include <vector>

#include "gtest/gtest.h"

#include <boost/math/constants/constants.hpp>

#include "GeoLib/GEOObjects.h"
#include "GeoLib/Point.h"
#include "GeoLib/Surface.h"
#include "GeoLib/Triangle.h"
#include "GeoLib/AnalyticalGeometry.h"

#include "MathLib/LinAlg/Dense/DenseMatrix.h"
#include "MathLib/Point3d.h"

#include "MeshLib/Mesh.h"
#include "MeshLib/Node.h"
#include "MeshLib/convertMeshToGeo.h"
#include "MeshLib/MeshGenerators/MeshGenerator.h"

inline double constant(double , double )
{
    return 0.0;
}

inline double coscos(double x, double y)
{
    return std::cos(x) * std::cos(y);
}

inline MathLib::Point3d
getEdgeMiddlePoint(MathLib::Point3d const&a, MathLib::Point3d const& b)
{
    return MathLib::Point3d(std::array<double,3>({{
        (a[0]+b[0])/2, (a[1]+b[1])/2, (a[2]+b[2])/2}}));
}

inline std::tuple<MathLib::Point3d, MathLib::Point3d, MathLib::Point3d>
getEdgeMiddlePoints(GeoLib::Triangle const& tri)
{
    return std::make_tuple(
        getEdgeMiddlePoint(*tri.getPoint(0), *tri.getPoint(1)),
        getEdgeMiddlePoint(*tri.getPoint(1), *tri.getPoint(2)),
        getEdgeMiddlePoint(*tri.getPoint(2), *tri.getPoint(0)));
}

/// Computes rotation matrix according to the z,x',z'' convention
inline MathLib::DenseMatrix<double, std::size_t>
getRotMat(double alpha, double beta, double gamma)
{
    MathLib::DenseMatrix<double, std::size_t> rot_mat(3,3);
    rot_mat(0,0) = cos(alpha)*cos(gamma) - sin(alpha)*cos(beta)*sin(gamma);
    rot_mat(0,1) = sin(alpha)*cos(gamma) + cos(alpha)*cos(beta)*sin(gamma);
    rot_mat(0,2) = sin(beta)*sin(gamma);
    rot_mat(1,0) = -cos(alpha)*sin(gamma) - sin(alpha)*cos(beta)*cos(gamma);
    rot_mat(1,1) = -sin(alpha)*sin(gamma) + cos(alpha)*cos(beta)*cos(gamma);
    rot_mat(1,2) = sin(beta)*cos(gamma);
    rot_mat(2,0) = sin(alpha)*sin(beta);
    rot_mat(2,1) = -cos(alpha)*sin(beta);
    rot_mat(2,2) = cos(beta);
    return rot_mat;
}

TEST(GeoLib, SurfaceIsPointInSurface)
{
    std::vector<std::function<double(double, double)>> surface_functions;
    surface_functions.emplace_back(constant);
    surface_functions.emplace_back(coscos);

    for (const auto& f : surface_functions) {
        std::random_device rd;

        std::string name("Surface");
        // generate ll and ur in random way
        std::mt19937 random_engine_mt19937(rd());
        std::normal_distribution<> normal_dist_ll(-10, 2);
        std::normal_distribution<> normal_dist_ur(10, 2);
        MathLib::Point3d ll(std::array<double,3>({{
            normal_dist_ll(random_engine_mt19937),
            normal_dist_ll(random_engine_mt19937),
            0.0}}));
        MathLib::Point3d ur(std::array<double,3>({{
            normal_dist_ur(random_engine_mt19937),
            normal_dist_ur(random_engine_mt19937),
            0.0}}));
        for (std::size_t k(0); k<3; ++k)
            if (ll[k] > ur[k])
                std::swap(ll[k], ur[k]);

        // random discretization of the domain
        std::default_random_engine re(rd());
        std::uniform_int_distribution<std::size_t> uniform_dist(2, 25);
        std::array<std::size_t,2> n_steps = {{uniform_dist(re),uniform_dist(re)}};

        std::unique_ptr<MeshLib::Mesh> sfc_mesh(
            MeshLib::MeshGenerator::createSurfaceMesh(
                name, ll, ur, n_steps, f
            )
        );

        // random rotation angles
        std::normal_distribution<> normal_dist_angles(
            0, boost::math::double_constants::two_pi);
        std::array<double,3> euler_angles = {{
            normal_dist_angles(random_engine_mt19937),
            normal_dist_angles(random_engine_mt19937),
            normal_dist_angles(random_engine_mt19937)
            }};

        MathLib::DenseMatrix<double, std::size_t> rot_mat(getRotMat(
            euler_angles[0], euler_angles[1], euler_angles[2]));

        std::vector<MeshLib::Node*> const& nodes(sfc_mesh->getNodes());
        GeoLib::rotatePoints<MeshLib::Node>(rot_mat, nodes);

        MathLib::Vector3 const normal(0,0,1.0);
        MathLib::Vector3 const surface_normal(rot_mat * normal);
        double const scaling(1e-6);
        MathLib::Vector3 const displacement(scaling * surface_normal);

        GeoLib::GEOObjects geometries;
        MeshLib::convertMeshToGeo(*sfc_mesh, geometries);

        std::vector<GeoLib::Surface*> const& sfcs(*geometries.getSurfaceVec(name));
        GeoLib::Surface const*const sfc(sfcs.front());
        std::vector<GeoLib::Point*> const& pnts(*geometries.getPointVec(name));

        double const eps(std::numeric_limits<double>::epsilon());

        // test triangle edge point of the surface triangles
        for (auto const p : pnts) {
            EXPECT_TRUE(sfc->isPntInSfc(*p, eps));
            MathLib::Point3d q(*p);
            for (std::size_t k(0); k<3; ++k)
                q[k] += displacement[k];
            EXPECT_FALSE(sfc->isPntInSfc(q, eps));
        }
        // test edge middle points of the triangles
        for (std::size_t k(0); k<sfc->getNumberOfTriangles(); ++k) {
            MathLib::Point3d p, q, r;
            std::tie(p,q,r) = getEdgeMiddlePoints(*(*sfc)[k]);
            EXPECT_TRUE(sfc->isPntInSfc(p, eps));
            EXPECT_TRUE(sfc->isPntInSfc(q, eps));
            EXPECT_TRUE(sfc->isPntInSfc(r, eps));
        }
    }
}


/*****************************************************************************
*
* Copyright (C) 2015-2020 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

// Calculating free energy, energy, and specific heat of triangular lattice Ising model

#include <iomanip>
#include <iostream>
#include <boost/lexical_cast.hpp>
#include "triangle/infinite.hpp"

int main(int argc, char **argv) {
  typedef double real_t;
  real_t Ja, Jb, Jc, t_min, t_max, t_step;
  if (argc == 7) {
    Ja = boost::lexical_cast<real_t>(argv[1]);
    Jb = boost::lexical_cast<real_t>(argv[2]);
    Jc = boost::lexical_cast<real_t>(argv[3]);
    t_min = boost::lexical_cast<real_t>(argv[4]);
    t_max = boost::lexical_cast<real_t>(argv[5]);
    t_step = boost::lexical_cast<real_t>(argv[6]);
  } else if (argc == 1) {
    std::cin >> Ja >> Jb >> Jc >> t_min >> t_max >> t_step;
  } else {
    std::cerr << "Usage: " << argv[0] << " [Ja Jb Jc t_min t_max t_step]\n";
    return 127;
  }
  std::cout << std::scientific << std::setprecision(std::numeric_limits<real_t>::digits10);
  std::cout << "# triangular lattice Ising model\n";
  std::cout << "# Ja, Jb, Jc, T, free energy density, energy density, specific heat\n";
  for (real_t t = t_min; t <= t_max; t += t_step) {
    real_t beta = 1 / t;
    auto result = ising::triangle::infinite(beta, Ja, Jb, Jc);
    std::cout << Ja << ' ' << Jb << ' ' << Jc << ' ' << t << ' ' << std::get<0>(result) << ' '
              << std::get<1>(result) << ' ' << std::get<2>(result) << std::endl;
  }
}