/*This program uses Rcpp attributes to import pure C++ functions into R. The main
function takes in an anrmadillo matrix(which has already had the anova
filter applied to it) and parses through its rows. NA values are skipped
and non-NA values are stored in a vector of vectors called groups, depending
on their group membership. All the non-NA values are concatenated into a vector which
is sorted in ascending order. Each non-NA value is assigned a rank, all the ranks
are stored in another vector called rankvec. The ranks are used in the
calculation of the Kruskall Wallis h statistic to then calculate the p-value of
that specific row.*/

/*The fuction group_size takes one input, a vector of strings called group. This
vector is an ordered character vector indicating what "group" factor level a
specific data element belongs to. This function makes a copy of group, called temp.
Next a unique function is applied to temp, exposing the levels of the factor.
Next we iterate through temp and count how many elements belong to each level,
these counts are stored in gsize.*/

/*The function calculate_kwh takes two inputs, a vector of ranks and a vector
of the number of non-NA values per group.This function implements the
mathematical formula for Kruskal Wallis test statistic. It returns a numeric
value(p-value).*/

/*The function compute_pvalue, simply takes in the Kruskall Wallis test statistic
and computes a p-value.*/

/*The main function is the kwh function, it takes two inputs, an armadillo
matrix and a vector of strings called "group". The vector "group" is fed to
the group_size function which will return a vector of group sizes. One row
of the armadillo matrix is stored in a vector named "cp". We iterate over
"cp" and collect all non-NA values and store them in a vector of vectors
called "groups". We iterate over "groups" and take the size of each group
and store it in a vector called "nonmiss".Next we concatenate all the
vectors from "groups" into one large vector called "cpy." Next we sort
"cpy" by its indices and store the indices in the vector "yvec." Then
we allocate a vector "rankvec", the same size as "yvec". We assing a rank
to each of the elements of "cpy" and keep them in the same order as "yvec".
We then take "rankvec" and and un-concatenate the vector into three smaller
vectors which are stored in "ranks" (the ranks of each group). Finally "ranks"
is input into calculate_kwh and a p-value is returned for that specific row
of the armadillo matrix. This process is repeated untill the last row of the
matrix is reached, all the p-values are stored in order in a list named
"final."  */

#include <RcppArmadillo.h>
#include <boost/math/distributions/chi_squared.hpp>
// [[Rcpp::plugins("cpp11")]]
// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::depends(BH)]]
using namespace Rcpp;

std::vector<int> gp_size(std::vector<std::string> group)
{
  std::vector<std::string> temp;
  temp = group;

  //std::sort(temp.begin(),temp.end());

  temp.erase( std::unique( temp.begin(), temp.end() ), temp.end() );
  int tempsize = 0;

  tempsize = temp.size();

  std::vector<int> gsize(tempsize);

  for(unsigned int i = 0;i<group.size();i++)
  {
    for(unsigned int k=0;k<temp.size();k++)
    {
      if(group[i]==temp[k])
        gsize[k]++;
    }
  }

  return gsize;
}

double calculate_kwh(std::vector <std::vector <double> > ranks, std::vector <double> nonmiss_sizes)
{
  std::vector <double> rsums;
  double tempsum = 0,temporary = 0;
  double n = 0,summation = 0, result = 0;

  for(unsigned int i = 0;i<nonmiss_sizes.size();i++)
  {
    n = n + nonmiss_sizes[i];
  }

  for(unsigned int i = 0,j=0;i<ranks.size();i++)
  {
    for(int k = 0;k<nonmiss_sizes[j];k++)
    {
      tempsum = tempsum + ranks[i][k];
    }

    temporary = pow(tempsum,2)/ nonmiss_sizes[i];
    rsums.push_back(temporary);
    tempsum = 0;
    temporary = 0;
    j++;
  }

  for(unsigned int i = 0;i<rsums.size();i++)
  {
    summation = summation + rsums[i];
  }

  result = ((12/(n*(n+1)))*summation)- 3*(n+1);

  return result;
}

double compute_pvalue(double h,std::vector<double> nonmiss)
{
  double p = 0;
  int df = 0;
  df = nonmiss.size()-1;

  boost::math::chi_squared mydist(df);
  p = boost::math::cdf(mydist, h);

  return 1 - p;
}

// [[Rcpp::export]]
std::list<double> kw_rcpp(arma::mat mtr,std::vector<std::string> group)
{
  std::vector<std::string> unique_groups = group;
  std::sort(unique_groups.begin(),unique_groups.end());
  unique_groups.erase( std::unique( unique_groups.begin(), unique_groups.end() ), unique_groups.end() );

  std::list<double> final;
  std:: vector<double> cp(mtr.n_cols);
  std::vector<std::vector<double> >groups;

  double tempnonmis = 0;

  std::vector<double> nonmiss;
  std::vector<double> tmp;

  for(unsigned int i = 0; i <mtr.n_rows;i++)
  {
        cp = arma::conv_to< std::vector<double> >::from(mtr.row(i));

        for (unsigned int i = 0; i < unique_groups.size(); i++)
        {
          for (unsigned int k = 0; k < cp.size(); k++)
          {
            if (ISNAN(cp[k])){
              continue;
            }
            else if(unique_groups[i] == group[k]) {
              tmp.push_back(cp[k]);
            }
          }

          groups.push_back(tmp);
          tmp.clear();

       }

            int all_na_count = 0;

            for (unsigned int i = 0; i < groups.size(); i++)
            {
              if(groups[i].size()==0)
              {
                all_na_count++;
              }

             else
             {
               tempnonmis = groups[i].size();
               nonmiss.push_back(tempnonmis);
             }
           }

            if((groups.size()-all_na_count) < 2)
            {
              final.push_back(NA_REAL);
              nonmiss.clear();
              groups.clear();
            }

          else
            {
              std::vector<double> cpy;

              for(unsigned int i = 0; i <groups.size();i++)
              {
                if(groups[i].size() == 0)
                {
                  continue;
                }

                else
                {
                  cpy.insert(cpy.end(), groups[i].begin(), groups[i].end());
                }
              }

              std::vector <double> rankvec (cpy.size());
              std::vector<double> yvec(cpy.size());
              std::size_t n(0);

              std::generate(yvec.begin(), yvec.end(), [&]{ return n++; });
              std::sort(yvec.begin(), yvec.end(), [&](int i1, int i2) {return cpy[i1] < cpy[i2]; } );

              for(unsigned int i = 0,j = 1;i<yvec.size();i++)
              {
                rankvec[yvec[i]]= j;
                j++;
              }

              double tempr = 0;
              std::vector<double>tempvec;
              std::vector<std::vector<double> > ranks;

                for (unsigned int i = 0, j = 0; i <nonmiss.size(); i++)
                {
                    for (unsigned int i = 0; i < nonmiss[j]; i++)
                    {
                      tempr = rankvec[i];
                      tempvec.push_back(tempr);
                    }

                  ranks.push_back(tempvec);
                  rankvec.erase(rankvec.begin(),rankvec.begin()+nonmiss[j]);
                  tempvec.clear();

                  j++;
                }

                double h = 0, p = 0;

                h = calculate_kwh(ranks,nonmiss);
                p = compute_pvalue(h,nonmiss);

                final.push_back(p);

                nonmiss.clear();
                groups.clear();
                ranks.clear();
                rankvec.clear();
                yvec.clear();
                cpy.clear();
            }
      }

  return final;
}


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

#include "math_unit_test.hpp"
#include <vector>
#include <random>
#include <boost/math/constants/constants.hpp>
#include <boost/math/interpolators/cardinal_trigonometric.hpp>
#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
#endif

using std::sin;
using boost::math::constants::two_pi;
using boost::math::interpolators::cardinal_trigonometric;

template<class Real>
void test_constant()
{
    Real t0 = 0;
    Real h = 1;
    for(size_t n = 1; n < 20; ++n)
    {
      Real c = 8;
      std::vector<Real> v(n, c);
      auto ct = cardinal_trigonometric<decltype(v)>(v, t0, h);
      CHECK_ULP_CLOSE(c, ct(0.3), 3);
      CHECK_ULP_CLOSE(c*h*n, ct.integrate(), 3);
      CHECK_ULP_CLOSE(c*c*h*n, ct.squared_l2(), 3);
      CHECK_MOLLIFIED_CLOSE(Real(0), ct.prime(0.8), 25*std::numeric_limits<Real>::epsilon());
      CHECK_MOLLIFIED_CLOSE(Real(0), ct.double_prime(0.8), 25*std::numeric_limits<Real>::epsilon());
    }
}

template<class Real>
void test_interpolation_condition()
{
  std::mt19937 gen(1234);
  std::uniform_real_distribution<Real> dis(1, 10);

  for(size_t n = 1; n < 20; ++n) {
    Real t0 = dis(gen);
    Real h = dis(gen);
    std::vector<Real> v(n);
    for (size_t i = 0; i < n; ++i) {
      v[i] = dis(gen);
    }
    auto ct = cardinal_trigonometric<decltype(v)>(v, t0, h);
    for (size_t i = 0; i < n; ++i) {
      Real arg = t0 + i*h;
      Real expected = v[i];
      Real computed = ct(arg);
      if(!CHECK_ULP_CLOSE(expected, computed, 5*n))
      {
        std::cerr << "  Samples: " << n << "\n";
      }
    }
  }

}

#ifdef BOOST_HAS_FLOAT128
void test_constant_q()
{
    __float128 t0 = 0;
    __float128 h = 1;
    for(size_t n = 1; n < 20; ++n)
    {
      __float128 c = 8;
      std::vector<__float128> v(n, c);
      auto ct = cardinal_trigonometric<decltype(v)>(v, t0, h);
      CHECK_ULP_CLOSE(boost::multiprecision::float128(c), boost::multiprecision::float128(ct(0.3)), 3);
      CHECK_ULP_CLOSE(boost::multiprecision::float128(c*h*n), boost::multiprecision::float128(ct.integrate()), 3);
    }
}
#endif

template<class Real>
void test_sampled_sine()
{
    using std::sin;
    using std::cos;
    for (unsigned n = 15; n < 50; ++n)
    {
      Real t0 = 0;
      Real T = 1;
      Real h = T/n;
      std::vector<Real> v(n);
      auto s = [&](Real t) { return sin(two_pi<Real>()*(t-t0)/T);};
      auto s_prime = [&](Real t) { return two_pi<Real>()*cos(two_pi<Real>()*(t-t0)/T)/T;};
      auto s_double_prime = [&](Real t) { return -two_pi<Real>()*two_pi<Real>()*sin(two_pi<Real>()*(t-t0)/T)/(T*T);};
      for(size_t j = 0; j < v.size(); ++j)
      {
          Real t = t0 + j*h;
          v[j] = s(t);
      }
      auto ct = cardinal_trigonometric<decltype(v)>(v, t0, h);
      CHECK_ULP_CLOSE(T, ct.period(), 3);
      std::mt19937 gen(1234);
      std::uniform_real_distribution<Real> dist(0, 500);

      unsigned j = 0;
      while (j++ < 50) {
        Real arg = dist(gen);
        Real expected = s(arg);
        Real computed = ct(arg);
        CHECK_MOLLIFIED_CLOSE(expected, computed, std::numeric_limits<Real>::epsilon()*4000);

        expected = s_prime(arg);
        computed = ct.prime(arg);
        CHECK_MOLLIFIED_CLOSE(expected, computed, 18000*std::numeric_limits<Real>::epsilon());

        expected = s_double_prime(arg);
        computed = ct.double_prime(arg);
        CHECK_MOLLIFIED_CLOSE(expected, computed, 100000*std::numeric_limits<Real>::epsilon());

      }
      CHECK_MOLLIFIED_CLOSE(Real(0), ct.integrate(), std::numeric_limits<Real>::epsilon());
    }
}

template<class Real>
void test_bump()
{
  using std::exp;
  using std::abs;
  using std::sqrt;
  using std::pow;
  auto bump = [](Real x)->Real { if (abs(x) >= 1) { return Real(0); } return exp(-Real(1)/(Real(1)-x*x)); };
  auto bump_prime = [](Real x)->Real {
      if (abs(x) >= 1) { return Real(0); }

      return -2*x*exp(-Real(1)/(Real(1)-x*x))/pow(1-x*x,2);
  };

  auto bump_double_prime = [](Real x)->Real {
      if (abs(x) >= 1) { return Real(0); }

      return (6*pow(x,4)-2)*exp(-Real(1)/(Real(1)-x*x))/pow(1-x*x,4);
  };

  Real t0 = -1;
  size_t n = 4096;
  Real h = Real(2)/Real(n);

  std::vector<Real> v(n);
  for(size_t i = 0; i < n; ++i)
  {
      Real t = t0 + i*h;
      v[i] = bump(t);
  }

  auto ct = cardinal_trigonometric<decltype(v)>(v, t0, h);
  std::mt19937 gen(323723);
  std::uniform_real_distribution<long double> dis(-0.9, 0.9);

  size_t i = 0;
  while (i++ < 1000)
  {
      Real t = static_cast<Real>(dis(gen));
      Real expected = bump(t);
      Real computed = ct(t);
      if(!CHECK_MOLLIFIED_CLOSE(expected, computed, 2*std::numeric_limits<Real>::epsilon())) {
          std::cerr << "  Problem occured at abscissa " << t << "\n";
      }

      expected = bump_prime(t);
      computed = ct.prime(t);
      if(!CHECK_MOLLIFIED_CLOSE(expected, computed, 4000*std::numeric_limits<Real>::epsilon())) {
          std::cerr << "  Problem occured at abscissa " << t << "\n";
      }

      expected = bump_double_prime(t);
      computed = ct.double_prime(t);
      if(!CHECK_MOLLIFIED_CLOSE(expected, computed, 4000*4000*std::numeric_limits<Real>::epsilon())) {
          std::cerr << "  Problem occured at abscissa " << t << "\n";
      }

  }

  // Wolfram Alpha:
  // NIntegrate[Exp[-1/(1-x*x)],{x,-1,1}]
  CHECK_ULP_CLOSE(Real(0.443993816168079437823L), ct.integrate(), 3);

  // NIntegrate[Exp[-2/(1-x*x)],{x,-1,1}]
  CHECK_ULP_CLOSE(Real(0.1330861208449942715569473279553285713625791551628130055345002588895389L), ct.squared_l2(), 1);

}

int main()
{

#ifdef TEST1
    test_constant<float>();
    test_sampled_sine<float>();
    test_bump<float>();
    test_interpolation_condition<float>();
#endif

#ifdef TEST2
    test_constant<double>();
    test_sampled_sine<double>();
    test_bump<double>();
    test_interpolation_condition<double>();
#endif

#ifdef TEST3
    test_constant<long double>();
    test_sampled_sine<long double>();
    test_bump<long double>();
    test_interpolation_condition<long double>();
#endif

#ifdef TEST4
#ifdef BOOST_HAS_FLOAT128
test_constant_q();
#endif
#endif

    return boost::math::test::report_errors();
}


#pragma once

#include <Eigen/Dense>

using Eigen::MatrixXf;
using Eigen::VectorXf;

namespace hyped {
namespace utils {
namespace math {

/**
 * @brief    This class is for filtering the data from sensors to smoothen it.
 */
class KalmanMultivariate {
 public:
  /**
   * @brief    Construct a new Kalman object with respective dimensions (with control)
   *
   * @param[in] n                       state dimensionality
   * @param[in] m                       measurement dimensionality
   * @param[in] k                       control dimensionality (default 0)
   */
  KalmanMultivariate(unsigned int n, unsigned int m, unsigned int k = 0);

  /**
   * @brief    Set dynamics model matrices (without control)
   *
   * @param[in] A                       state transition matrix
   * @param[in] Q                       process noise covariance
   */
  void setDynamicsModel(MatrixXf &A, MatrixXf &Q);

  /**
   * @brief    Set dynamics model matrices (with control)
   *
   * @param[in] A                       state transition matrix
   * @param[in] B                       control matrix
   * @param[in] Q                       process noise covariance
   */
  void setDynamicsModel(MatrixXf &A, MatrixXf &B, MatrixXf &Q);

  /**
   * @brief    Set measurement model matrices
   *
   * @param[in] H                       measurement matrix
   * @param[in] R                       measurement noise covariance
   */
  void setMeasurementModel(MatrixXf &H, MatrixXf &R);

  /**
   * @brief    Set model matrices (without control)
   *
   * @param[in] A                       state transition matrix
   * @param[in] Q                       process noise covariance
   * @param[in] H                       measurement matrix
   * @param[in] R                       measurement noise covariance
   */
  void setModels(MatrixXf &A, MatrixXf &Q, MatrixXf &H, MatrixXf &R);

  /**
   * @brief    Set model matrices (with control)
   *
   * @param[in] A                       state transition matrix
   * @param[in] B                       control matrix
   * @param[in] Q                       process noise covariance
   * @param[in] H                       measurement matrix
   * @param[in] R                       measurement noise covariance
   */
  void setModels(MatrixXf &A, MatrixXf &B, MatrixXf &Q, MatrixXf &H, MatrixXf &R);

  /**
   * @brief    Update state transition matrix
   *
   * @param[in] A                       state transition matrix
   */
  void updateA(MatrixXf &A);

  /**
   * @brief    Update measurement covariance matrix
   *
   * @param[in] R                       measurement covariance matrix
   */
  void updateR(MatrixXf &R);

  /**
   * @brief    Set initial beliefs
   *
   * @param[in] x0                      initial state belief
   * @param[in] P0                      initial state covariance (uncertainty)
   */
  void setInitial(VectorXf &x0, MatrixXf &P0);

  /**
   * @brief    Filter measurement and update state belief with covariance (without control)
   *
   * @param[in] z                       measurement vector
   */
  void filter(VectorXf &z);

  /**
   * @brief    Filter measurement and update state belief with covariance (with control)
   *
   * @param[in] u                       control vector
   * @param[in] z                       measurement vector
   */
  void filter(VectorXf &u, VectorXf &z);

  /**
   * @brief     Get the state estimate
   *
   * @return    Returns the current state estimate
   */
  VectorXf &getStateEstimate();

  /**
   * @brief     Get the state uncertainty
   *
   * @return    Returns the current state covariance
   */
  MatrixXf &getStateCovariance();

 private:
  /* problem dimensions */
  unsigned int n_;  // state dimension
  unsigned int m_;  // measurement dimension
  unsigned int k_;  // control dimension (0 if not set)

  /* dynamics model matrices */
  MatrixXf A_;  // state transition matrix: n x n
  MatrixXf B_;  // control matrix: n x k
  MatrixXf Q_;  // process noise covariance: n x n

  /* measurement model matrices */
  MatrixXf H_;  // measurement matrix: m x n
  MatrixXf R_;  // measurement noise covariance: m x m

  /* state estimates */
  VectorXf x_;  // state vector: n x 1
  MatrixXf P_;  // state covariance: n x n
  MatrixXf I_;  // identity matrix: n x n

  /**
   * @brief    Predict state belief with covariance based on dynamics (without control)
   */
  void predict();

  /**
   * @brief    Predict state belief with covariance based on dynamics (with control)
   *
   * @param[in] u                       control vector
   */
  void predict(VectorXf &u);

  /**
   * @brief    Correct state belief with covariance based on measurement
   *
   * @param[in] z                       measurement vector
   */
  void correct(VectorXf &z);
};
}  // namespace math
}  // namespace utils
}  // namespace hyped


/*
   Copyright (C) 2015-2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>

   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 <cmath>
#include <gtest/gtest.h>
#include <boost/math/differentiation/autodiff.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include "ising/mp_wrapper.hpp"
#include "ising/free_energy/common.hpp"
#include "ising/free_energy/square.hpp"

using namespace boost::multiprecision;
using namespace ising::free_energy;

TEST(IsingFreeEnergy, SquareFinite0) {
  typedef double real_t;
  unsigned Lx = 4;
  unsigned Ly = 4;
  auto Jx = convert<real_t>("1.5");
  auto Jy = convert<real_t>("2.5");
  auto t = convert<real_t>("2");
  auto vars = boost::math::differentiation::make_ftuple<real_t, 2, 2>(1 / t, 0);
  auto& beta = std::get<0>(vars);
  auto& h = std::get<1>(vars);
  auto fc = square::finite_count(Lx, Ly, Jx, Jy, beta, h);
  auto ff = square::finite(Lx, Ly, Jx, Jy, beta);
  double eps = 1e-12;
  EXPECT_TRUE(abs(free_energy(fc, beta, h) - free_energy(ff, beta)) < eps);
  EXPECT_TRUE(abs(energy(fc, beta, h) - energy(ff, beta)) < eps);
  EXPECT_TRUE(abs(specific_heat(fc, beta, h) - specific_heat(ff, beta)) < eps);
}

TEST(IsingFreeEnergy, SquareFinite1) {
  typedef mp_wrapper<cpp_dec_float_50> real_t;
  unsigned Lx = 4;
  unsigned Ly = 4;
  auto Jx = convert<real_t>("1.5");
  auto Jy = convert<real_t>("2.5");
  auto t = convert<real_t>("2");
  auto vars = boost::math::differentiation::make_ftuple<real_t, 2, 2>(1 / t, 0);
  auto& beta = std::get<0>(vars);
  auto& h = std::get<1>(vars);
  auto fc = square::finite_count(Lx, Ly, Jx, Jy, beta, h);
  auto ff = square::finite(Lx, Ly, Jx, Jy, beta);
  double eps = 1e-40;
  EXPECT_TRUE(abs(free_energy(fc, beta, h) - free_energy(ff, beta)) < eps);
  EXPECT_TRUE(abs(energy(fc, beta, h) - energy(ff, beta)) < eps);
  EXPECT_TRUE(abs(specific_heat(fc, beta, h) - specific_heat(ff, beta)) < eps);
}

TEST(IsingFreeEnergy, SquareFinite2) {
  typedef mp_wrapper<cpp_dec_float_100> real_t;
  unsigned Lx = 4;
  unsigned Ly = 4;
  auto Jx = convert<real_t>("1.5");
  auto Jy = convert<real_t>("2.5");
  auto t = convert<real_t>("2");
  auto vars = boost::math::differentiation::make_ftuple<real_t, 2, 2>(1 / t, 0);
  auto& beta = std::get<0>(vars);
  auto& h = std::get<1>(vars);
  auto fc = square::finite_count(Lx, Ly, Jx, Jy, beta, h);
  auto ff = square::finite(Lx, Ly, Jx, Jy, beta);
  double eps = 1e-80;
  EXPECT_TRUE(abs(free_energy(fc, beta, h) - free_energy(ff, beta)) < eps);
  EXPECT_TRUE(abs(energy(fc, beta, h) - energy(ff, beta)) < eps);
  EXPECT_TRUE(abs(specific_heat(fc, beta, h) - specific_heat(ff, beta)) < eps);
}


// Unit test for the isEqualDouble function in testing_functions.cpp
// This function determines if two numbers are equal within some tolerance
// This is important because there is inherent error in double precision numbers
// COMPLETE. This function works as intended. 2021/11/14

// Include headers
#include <iostream>
#include <Eigen/Dense>
#include "../main/fvm_1D_functions.h"
#include "../main/testing_functions.h"

using namespace std;
using namespace Eigen;

int main() {

    // Print what we are doing
    cout << "------------------------------------- " << \
    "This is a test of the isEqualDouble function." << \
    " -------------------------------------" << endl;

    // Set a tolerance
    double TOL = 1e-10;

    // Set some arbitrary value
    double val = 1.1;

    // We can test 4 different modes of this function to determine how well it works

    // First, we will test to make sure that val+0.1*TOL is considered equal since it will by definition be within the tolerance
    bool higher_within_TOL = isEqualDouble(val, val + 0.1*TOL, TOL);   // This should result in 1
    if (higher_within_TOL == 1) {  //
        cout << "Test of higher but within tolerance is a success." << endl;
    }
    else {
        cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl \
        << "Test of higher but within tolerance is a failure." << endl \
        << "!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl;
    }

    // Test to make sure that val+1.1*TOL is not considered equal since it will by definition be outside the tolerance
    bool higher_over_TOL = isEqualDouble(val, val + 1.1*TOL, TOL);   // This should result in 0
    if (higher_over_TOL == 0) {  //
        cout << "Test of higher over tolerance is a success." << endl;
    }
    else {
        cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl \
        << "Test of higher over tolerance is a failure." << endl \
        << "!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl;
    }

    // Test to make sure that val-0.1*TOL is considered equal since it will by definition be within the tolerance
    bool lower_within_TOL = isEqualDouble(val, val - 0.1*TOL, TOL);   // This should result in 1
    if (lower_within_TOL == 1) {  //
        cout << "Test of lower but within tolerance is a success." << endl;
    }
    else {
        cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl \
        << "Test of lower but within tolerance is a failure." << endl \
        << "!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl;
    }

    // Test to make sure that val-1.1*TOL is not considered equal since it will by definition be outside the tolerance
    bool lower_less_TOL = isEqualDouble(val, val - 1.1*TOL, TOL);   // This should result in 0
    if (lower_less_TOL == 0) {  //
        cout << "Test of lower but outside tolerance is a success." << endl;
    }
    else {
        cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl \
        << "Test of lower but outside tolerance is a failure." << endl \
        << "!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl;
    }

    cout << endl << endl;
}


// Component
#include "CloudTransformer.hpp"
#include "RosConversionHelper.hpp"

// Libararies
#include <Eigen/Dense>
#include <pcl/filters/passthrough.h>

// Standard
#include <utility>

namespace cm
{

void CloudTransformer::transformCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, const tf::StampedTransform& tf, const geometry_msgs::Quaternion& q)
{
    geometry_msgs::Quaternion q_;
    tf::quaternionTFToMsg(tf.getRotation(), q_);
    const Eigen::Matrix3d pc_rot_mat = RosConversionHelper::quaternionMsgToRotationMatrix(q_);

    const Eigen::Matrix3d rot_mat = RosConversionHelper::quaternionMsgToRotationMatrixIgnoreYaw(q).inverse()*pc_rot_mat.inverse();
    Eigen::Matrix4d hom_tf;
    hom_tf << rot_mat (0, 0), rot_mat(0, 1), rot_mat(0, 2), 0.0,
              rot_mat (1, 0), rot_mat(1, 1), rot_mat(1, 2), 0.0,
              rot_mat (2, 0), rot_mat(2, 1), rot_mat(2, 2), 0.0,
              rot_mat (3, 0), rot_mat(3, 1), rot_mat(3, 2), 1.0;

    pcl::PointCloud<pcl::PointXYZ> transformed_cloud;
    pcl::transformPointCloud(*cloud, transformed_cloud, hom_tf);
    *cloud = std::move(transformed_cloud);
}

void CloudTransformer::trimCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, const float64_t band_m)
{
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(-band_m/2.0, band_m/2.0);
    pcl::PointCloud<pcl::PointXYZ> filtered_cloud;
    pass.filter(filtered_cloud);
    *cloud = std::move(filtered_cloud);
}

} // namespace cm


// Example of using the GeographicLib::NearestNeighbor class.  WARNING: this
// creates a file, pointset.xml or pointset.txt, in the current directory.
// Read lat/lon locations from locations.txt and lat/lon queries from
// queries.txt.  For each query print to standard output: the index for the
// closest location and the distance to it.  Print statistics to standard error
// at the end.

#include <iostream>
#include <exception>
#include <vector>
#include <fstream>
#include <string>

#if !defined(GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION)
#define GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION 0
#endif

#if GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION
// If Boost serialization is available, use it.
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
#endif

#include <GeographicLib/NearestNeighbor.hpp>
#include <GeographicLib/Geodesic.hpp>
#include <GeographicLib/DMS.hpp>

using namespace std;
using namespace GeographicLib;

// A structure to hold a geographic coordinate.
struct pos {
  double _lat, _lon;
  pos(double lat = 0, double lon = 0) : _lat(lat), _lon(lon) {}
};

// A class to compute the distance between 2 positions.
class DistanceCalculator {
private:
  Geodesic _geod;
public:
  explicit DistanceCalculator(const Geodesic& geod) : _geod(geod) {}
  double operator() (const pos& a, const pos& b) const {
    double d;
    _geod.Inverse(a._lat, a._lon, b._lat, b._lon, d);
    if ( !(d >= 0) )
      // Catch illegal positions which result in d = NaN
      throw GeographicErr("distance doesn't satisfy d >= 0");
    return d;
  }
};

int main() {
  try {
    // Read in locations
    vector<pos> locs;
    double lat, lon;
    string sa, sb;
    {
      ifstream is("locations.txt");
      if (!is.good())
        throw GeographicErr("locations.txt not readable");
      while (is >> sa >> sb) {
        DMS::DecodeLatLon(sa, sb, lat, lon);
        locs.push_back(pos(lat, lon));
      }
      if (locs.size() == 0)
        throw GeographicErr("need at least one location");
    }

    // Define a distance function object
    DistanceCalculator distance(Geodesic::WGS84());

    // Create NearestNeighbor object
    NearestNeighbor<double, pos, DistanceCalculator> pointset;

    {
      // Used saved object if it is available
#if GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION
      ifstream is("pointset.xml");
      if (is.good()) {
        boost::archive::xml_iarchive ia(is);
        ia >> BOOST_SERIALIZATION_NVP(pointset);
      }
#else
      ifstream is("pointset.txt");
      if (is.good())
        is >> pointset;
#endif
    }
    // Is the saved pointset up-to-date?
    if (pointset.NumPoints() != int(locs.size())) {
      // else initialize it
      pointset.Initialize(locs, distance);
      // and save it
#if GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION
      ofstream os("pointset.xml");
      if (!os.good())
        throw GeographicErr("cannot write to pointset.xml");
      boost::archive::xml_oarchive oa(os);
      oa << BOOST_SERIALIZATION_NVP(pointset);
#else
      ofstream os("pointset.txt");
      if (!os.good())
        throw GeographicErr("cannot write to pointset.txt");
      os << pointset << "\n";
#endif
    }

    ifstream is("queries.txt");
    double d;
    int count = 0;
    vector<int> k;
    while (is >> sa >> sb) {
      ++count;
      DMS::DecodeLatLon(sa, sb, lat, lon);
      d = pointset.Search(locs, distance, pos(lat, lon), k);
      if (k.size() != 1)
          throw GeographicErr("unexpected number of results");
      cout << k[0] << " " << d << "\n";
    }
    int setupcost, numsearches, searchcost, mincost, maxcost;
    double mean, sd;
    pointset.Statistics(setupcost, numsearches, searchcost,
                        mincost, maxcost, mean, sd);
    int
      totcost = setupcost + searchcost,
      exhaustivecost = count * pointset.NumPoints();
    cerr
      << "Number of distance calculations = " << totcost << "\n"
      << "With an exhaustive search = " << exhaustivecost << "\n"
      << "Ratio = " << double(totcost) / exhaustivecost << "\n"
      << "Efficiency improvement = "
      << 100 * (1 - double(totcost) / exhaustivecost) << "%\n";
  }
  catch (const exception& e) {
    cerr << "Caught exception: " << e.what() << "\n";
    return 1;
  }
}


/* test_gamma.cpp
 *
 * Copyright Steven Watanabe 2010
 * 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)
 *
 * $Id$
 *
 */

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

#define BOOST_RANDOM_DISTRIBUTION boost::random::gamma_distribution<>
#define BOOST_RANDOM_DISTRIBUTION_NAME gamma
#define BOOST_MATH_DISTRIBUTION boost::math::gamma_distribution<>
#define BOOST_RANDOM_ARG1_TYPE double
#define BOOST_RANDOM_ARG1_NAME alpha
#define BOOST_RANDOM_ARG1_DEFAULT 1000.0
#define BOOST_RANDOM_ARG1_DISTRIBUTION(n) boost::uniform_real<>(0.00001, n)
#define BOOST_RANDOM_ARG2_TYPE double
#define BOOST_RANDOM_ARG2_NAME beta
#define BOOST_RANDOM_ARG2_DEFAULT 1000.0
#define BOOST_RANDOM_ARG2_DISTRIBUTION(n) boost::uniform_real<>(0.00001, n)

#include "test_real_distribution.ipp"


/*
 * Copyright Andrey Semashev 2020
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * https://www.boost.org/LICENSE_1_0.txt)
 */
/*!
 * \file bit_floor.hpp
 *
 * This header defines \c bit_floor algorithm, which produces a nearest power of 2 integer
 * that is less or equal to the input integer.
 */

#ifndef BOOST_BIT_OPS_POW2_BIT_FLOOR_HPP_INCLUDED_
#define BOOST_BIT_OPS_POW2_BIT_FLOOR_HPP_INCLUDED_

#include <limits>
#include <boost/bit_ops/detail/config.hpp>
#include <boost/bit_ops/detail/type_traits/enable_if.hpp>
#include <boost/bit_ops/detail/type_traits/is_integral.hpp>
#include <boost/bit_ops/detail/type_traits/is_unsigned.hpp>
#include <boost/bit_ops/count/countl_zero.hpp>

namespace boost {
namespace bit_ops {

/*!
 * \brief Returns the nearest power of 2 integer that is less or equal to \a value
 *
 * \pre \a value must not be zero
 */
template< typename T >
inline typename bit_ops::detail::enable_if<
    bit_ops::detail::is_integral< T >::value && bit_ops::detail::is_unsigned< T >::value,
    T
>::type bit_floor_nz(T value) BOOST_NOEXCEPT
{
    return static_cast< T >(1u) << ((std::numeric_limits< T >::digits - 1u) - bit_ops::countl_zero_nz(value));
}

//! Returns the nearest power of 2 integer that is less or equal to \a value, or 0 if \a value is zero
template< typename T >
inline typename bit_ops::detail::enable_if<
    bit_ops::detail::is_integral< T >::value && bit_ops::detail::is_unsigned< T >::value,
    T
>::type bit_floor(T value) BOOST_NOEXCEPT
{
    return value == 0u ? static_cast< T >(0u) : bit_ops::bit_floor_nz(value);
}

} // namespace bit_ops
} // namespace boost

#endif // BOOST_BIT_OPS_POW2_BIT_FLOOR_HPP_INCLUDED_


/*
 * File: trim_dht.hpp
 * Created Date: 2020-01-01
 * Author: Lei Pan
 * Contact: <panlei7@gmail.com>
 *
 * Last Modified: Wednesday January 1st 2020 11:27:12 am
 *
 * MIT License
 *
 * Copyright (c) 2020 Lei Pan
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the 'Software'), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED 'AS IS', WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 * -----
 * HISTORY:
 * Date      	 By	Comments
 * ----------	---
 * ----------------------------------------------------------
 */

#ifndef DHT_TRIM_DHT_H_
#define DHT_TRIM_DHT_H_

#include "dht.hpp"
#include <Eigen/Dense>

class TrimDHT : public DiscreteHankelTransform {
public:
  TrimDHT(int order, int nr, double rmax, int nexp);
  ~TrimDHT();

  Eigen::VectorXd perform(const Eigen::Ref<const Eigen::VectorXd> &src);

  Eigen::VectorXd r_sampling();
  Eigen::VectorXd k_sampling();

  int get_nr() const;

private:
  Eigen::MatrixXd shift_matrix_;
  double rmax_extend_;
};

#endif


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 1999 - 2020 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Authors: Wolfgang Bangerth, 1999,
 *          Guido Kanschat, 2011
 *          Luca Heltai, 2021
 */
#include "stokes.h"

#include <deal.II/lac/linear_operator.h>
#include <deal.II/lac/linear_operator_tools.h>
#include <deal.II/lac/solver_gmres.h>

#include <deal.II/numerics/error_estimator.h>

using namespace dealii;

namespace
{
  std::vector<std::string>
  get_component_names(int dim)
  {
    std::vector<std::string> names(dim + 1, "u");
    names[dim] = "p";
    return names;
  }
} // namespace

template <int dim>
Stokes<dim>::Stokes()
  : BaseBlockProblem<dim>(get_component_names(dim),
                          "Stokes<" + std::to_string(dim) + ">")
  , velocity(0)
  , pressure(dim)
{
  // Output the vector result.
  this->add_data_vector.connect([&](auto &data_out) {
    std::vector<DataComponentInterpretation::DataComponentInterpretation>
      interpretation(dim + 1,
                     DataComponentInterpretation::component_is_part_of_vector);

    interpretation[dim] = DataComponentInterpretation::component_is_scalar;

    data_out.add_data_vector(this->locally_relevant_block_solution,
                             this->component_names,
                             DataOut<dim>::type_dof_data,
                             interpretation);
  });
}

template <int dim>
void
Stokes<dim>::assemble_system_one_cell(
  const typename DoFHandler<dim>::active_cell_iterator &cell,
  ScratchData &                                         scratch,
  CopyData &                                            copy)
{
  auto &cell_matrix = copy.matrices[0];
  auto &cell_rhs    = copy.vectors[0];

  cell->get_dof_indices(copy.local_dof_indices[0]);

  const auto &fe_values = scratch.reinit(cell);
  cell_matrix           = 0;
  cell_rhs              = 0;

  for (const unsigned int q_index : fe_values.quadrature_point_indices())
    {
      for (const unsigned int i : fe_values.dof_indices())
        {
          const auto eps_v = fe_values[velocity].symmetric_gradient(
            i, q_index); // SymmetricTensor<2,dim>
          const auto div_v =
            fe_values[velocity].divergence(i, q_index);         // double
          const auto q = fe_values[pressure].value(i, q_index); // double

          for (const unsigned int j : fe_values.dof_indices())
            {
              const auto eps_u = fe_values[velocity].symmetric_gradient(
                j, q_index); // SymmetricTensor<2,dim>
              const auto div_u =
                fe_values[velocity].divergence(j, q_index);         // double
              const auto p = fe_values[pressure].value(j, q_index); // double

              cell_matrix(i, j) +=
                (scalar_product(eps_v, eps_u) - p * div_v - q * div_u) *
                fe_values.JxW(q_index); // dx
            }
          for (const unsigned int i : fe_values.dof_indices())
            {
              const auto comp_i = this->fe->system_to_component_index(i).first;
              cell_rhs(i) +=
                (fe_values.shape_value(i, q_index) * // phi_i(x_q)
                 this->forcing_term.value(fe_values.quadrature_point(q_index),
                                          comp_i) * // f(x_q)
                 fe_values.JxW(q_index));           // dx
            }
        }
    }

  if (cell->at_boundary())
    //  for(const auto face: cell->face_indices())
    for (unsigned int f = 0; f < GeometryInfo<dim>::faces_per_cell; ++f)
      if (this->neumann_ids.find(cell->face(f)->boundary_id()) !=
          this->neumann_ids.end())
        {
          auto &fe_face_values = scratch.reinit(cell, f);
          for (const unsigned int q_index :
               fe_face_values.quadrature_point_indices())
            for (const unsigned int i : fe_face_values.dof_indices())
              {
                const auto comp_i =
                  this->fe->system_to_component_index(i).first;
                cell_rhs(i) +=
                  fe_face_values.shape_value(i, q_index) *
                  this->neumann_boundary_condition.value(
                    fe_face_values.quadrature_point(q_index), comp_i) *
                  fe_face_values.JxW(q_index);
              }
        }
}

template <int dim>
void
Stokes<dim>::solve()
{
  TimerOutput::Scope                timer_section(this->timer, "solve");
  SolverGMRES<LA::MPI::BlockVector> solver(this->solver_control);

  LA::MPI::PreconditionAMG amg;
  amg.initialize(this->system_block_matrix.block(0, 0));

  solver.solve(this->system_block_matrix,
               this->block_solution,
               this->system_block_rhs,
               PreconditionIdentity());
  this->constraints.distribute(this->block_solution);
  this->locally_relevant_block_solution = this->block_solution;
}

template <int dim>
void
Stokes<dim>::estimate()
{
  TimerOutput::Scope timer_section(this->timer, "estimate");
  if (this->estimator_type == "kelly")
    {
      std::map<types::boundary_id, const Function<dim> *> neumann;
      for (const auto id : this->neumann_ids)
        neumann[id] = &this->neumann_boundary_condition;

      QGauss<dim - 1> face_quad(this->fe->degree + 1);
      KellyErrorEstimator<dim>::estimate(*this->mapping,
                                         this->dof_handler,
                                         face_quad,
                                         neumann,
                                         this->locally_relevant_block_solution,
                                         this->error_per_cell,
                                         this->fe->component_mask(velocity));
    }
  else
    {
      AssertThrow(false, ExcNotImplemented());
    }
  auto global_estimator = this->error_per_cell.l2_norm();
  this->error_table.add_extra_column("estimator", [global_estimator]() {
    return global_estimator;
  });
  this->error_table.error_from_exact(*this->mapping,
                                     this->dof_handler,
                                     this->locally_relevant_block_solution,
                                     this->exact_solution);
}

template class Stokes<1>;
template class Stokes<2>;
template class Stokes<3>;


/*(utf8)
 * This is a slight tweek of the
 * 1€ Filter, template-compliant version
 * by Jonathan Aceituno <join@oin.name>
 * omiting timestamps, alowing for interger input,
 * and giving independant access to the lowpass filter.
 *
 * For details, see http://www.lifl.fr/~casiez/1euro
 * * quote * "Note that parameters fcmin and beta have clear
 * conceptual relationships: if high speed lag is a problem,
 * increase beta; if slow speed jitter is a problem, decrease
 * fcmin."
*/

#include "filter_base.hpp"

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

#define INIT_FREQ 120.
#define INIT_CUTOFF 1.
#define INIT_MIN 1.
#define INIT_BETA 1.

using namespace boost::math::constants;

namespace value_filters
{
template <typename T = double>
class low_pass_filter
{
public:
  low_pass_filter(double _freq = INIT_FREQ, double _dcutoff = INIT_CUTOFF)
      : hatxprev {0}, xprev {0}, alpha {0}, hadprev {false}
  {
    set_alpha(_freq, _dcutoff);
  }

  T operator()(T x)
  {
    T hatx {0};

    if (hadprev)
    {
      hatx = alpha * x + (1 - alpha) * hatxprev;
    }
    else
    {
      hatx = x;
      hadprev = true;
    }

    hatxprev = hatx;
    xprev = x;
    return hatx;
  }

  T xprev {};
  bool hadprev {};
  double dcutoff {}, freq {};

  void set_alpha(double cutoff, double _freq)
  {
    dcutoff = cutoff;
    freq = _freq;
    compute_alpha();
  }

  void set_amount(double amt)
  {
    set_alpha(pow(1 / (1 + amt), 2), freq);
  }

  void update()
  {
    compute_alpha();
  }

private:
  T hatxprev{}, alpha{};

  void compute_alpha()
  {
    T tau = one_div_two_pi<double>() * (1. / dcutoff);
    double te = 1. / freq;

    alpha = 1. / (1. + tau / te);
  }
};

template <typename T = double>
struct one_euro_filter
{
  one_euro_filter(
      double _freq = INIT_FREQ, double _mincutoff = INIT_MIN,
      double _beta = INIT_BETA, double _dcutoff = INIT_CUTOFF)
      : freq {_freq}, beta {_beta}, dcutoff {_dcutoff}, mincutoff {_mincutoff}
  {
  }

  T operator()(T x)
  {
    T dx {0};

    if (xfilt_.hadprev)
      dx = (x - xfilt_.xprev) * freq;

    dxfilt_.set_alpha(dcutoff, freq);
    T edx = dxfilt_(dx);
    T cutoff = mincutoff + beta * std::abs(static_cast<double>(edx));

    xfilt_.set_alpha(cutoff, freq);
    return xfilt_(x);
  }

  double freq {}, beta {}, dcutoff {};

  void set_amount(double amt)
  {
    if (amt <= 0.) amt = 0.0001;
    // mincutoff is basicly the inverse of the amount of filtering
    mincutoff = SCALED_AMOUNT - amt;
  }

  void update()
  {
  }
private:
  double mincutoff {};

  low_pass_filter<T> xfilt_ {}, dxfilt_ {};
};

}


#include "gtest/gtest.h"

#include <sstream>
#include <boost/scoped_ptr.hpp>

#include "wali/domains/matrix/Matrix.hpp"

#include "fixtures-boolmatrix.hpp"
#include "matrix-equal.hpp"

using namespace testing::boolmatrix;

namespace wali {
namespace domains {

TEST(wali$domains$matrix$BoolMatrix$$constructorAndMatrix, basicTest3x3)
{
    RandomMatrix1_3x3 f;
    BoolMatrix m(f.mat);

    EXPECT_EQ(m.matrix(), f.mat);
}

#define NUM_ELEMENTS(arr) ((sizeof arr)/(sizeof arr[0]))

TEST(wali$domains$matrix$BoolMatrix$$equalAndIsZeroAndIsOne, battery)
{
    MatrixFixtures_3x3 f;
    BoolMatrix mats[] = {
        BoolMatrix(f.zero.mat),
        BoolMatrix(f.id.mat),
        BoolMatrix(f.r1.mat),
        BoolMatrix(f.r2.mat),
        BoolMatrix(f.ext_r1_r2.mat),
        BoolMatrix(f.ext_r2_r1.mat),
    };

    for (size_t left=0; left<NUM_ELEMENTS(mats); ++left) {
        for (size_t right=0; right<NUM_ELEMENTS(mats); ++right) {
            EXPECT_EQ(left == right,
                      mats[left].equal(&mats[right]));
        }
    }
}

TEST(wali$domains$matrix$BoolMatrix$$zero_raw, basicTest3x3)
{
    RandomMatrix1_3x3 f;
    ZeroBackingMatrix_3x3 z;

    BoolMatrix m(f.mat);
    BoolMatrix mz(z.mat);

    boost::scoped_ptr<BoolMatrix> result(m.zero_raw());

    EXPECT_EQ(z.mat, result->matrix());
    EXPECT_TRUE(mz.equal(result.get()));
}

TEST(wali$domains$matrix$BoolMatrix$$one_raw, basicTest3x3)
{
    RandomMatrix1_3x3 f;
    IdBackingMatrix_3x3 z;

    BoolMatrix m(f.mat);
    BoolMatrix mid(z.mat);

    boost::scoped_ptr<BoolMatrix> result(m.one_raw());

    EXPECT_EQ(z.mat, result->matrix());
    EXPECT_TRUE(mid.equal(result.get()));
}

TEST(wali$domains$matrix$BoolMatrix$$extend_raw, twoRandomMatrices)
{
    RandomMatrix1_3x3 f1;
    RandomMatrix2_3x3 f2;
    ExtendR1R2_3x3 fr12;
    ExtendR2R1_3x3 fr21;

    BoolMatrix m1(f1.mat);
    BoolMatrix m2(f2.mat);

    boost::scoped_ptr<BoolMatrix> result12(m1.extend_raw(&m2));
    boost::scoped_ptr<BoolMatrix> result21(m2.extend_raw(&m1));

    EXPECT_EQ(fr12.mat, result12->matrix());
    EXPECT_EQ(fr21.mat, result21->matrix());
}

TEST(wali$domains$matrix$BoolMatrix$$extend_raw, extendAgainstZero)
{
    RandomMatrix1_3x3 f;
    ZeroBackingMatrix_3x3 z;

    BoolMatrix mf(f.mat);
    BoolMatrix mz(z.mat);

    boost::scoped_ptr<BoolMatrix>
        result1Z(mf.extend_raw(&mz)),
        resultZ1(mz.extend_raw(&mf)),
        resultZZ(mz.extend_raw(&mz));

    EXPECT_EQ(z.mat, result1Z->matrix());
    EXPECT_EQ(z.mat, resultZ1->matrix());
    EXPECT_EQ(z.mat, resultZZ->matrix());

    EXPECT_TRUE(mz.equal(result1Z.get()));
    EXPECT_TRUE(mz.equal(resultZ1.get()));
    EXPECT_TRUE(mz.equal(resultZZ.get()));
}

TEST(wali$domains$matrix$BoolMatrix$$extend_raw, extendAgainstOne)
{
    RandomMatrix1_3x3 fr1;
    IdBackingMatrix_3x3 id;

    BoolMatrix mr1(fr1.mat);
    BoolMatrix mid(id.mat);

    boost::scoped_ptr<BoolMatrix>
        result_R1_Id(mr1.extend_raw(&mid)),
        result_Id_R1(mid.extend_raw(&mr1)),
        result_Id_Id(mid.extend_raw(&mid));

    EXPECT_EQ(fr1.mat, result_R1_Id->matrix());
    EXPECT_EQ(fr1.mat, result_Id_R1->matrix());
    EXPECT_EQ(id.mat,  result_Id_Id->matrix());

    EXPECT_TRUE(mr1.equal(result_R1_Id.get()));
    EXPECT_TRUE(mr1.equal(result_Id_R1.get()));
    EXPECT_TRUE(mid.equal(result_Id_Id.get()));
}

TEST(wali$domains$matrix$BoolMatrix$$combine_raw, randomAndId)
{
    RandomMatrix1_3x3 f1;
    IdBackingMatrix_3x3 f2;
    CombineR1Id_3x3 fr;

    BoolMatrix m1(f1.mat);
    BoolMatrix m2(f2.mat);
    BoolMatrix mr(fr.mat);

    boost::scoped_ptr<BoolMatrix> result12(m1.combine_raw(&m2));
    boost::scoped_ptr<BoolMatrix> result21(m2.combine_raw(&m1));

    EXPECT_EQ(fr.mat, result12->matrix());
    EXPECT_EQ(fr.mat, result21->matrix());

    EXPECT_TRUE(mr.equal(result12.get()));
    EXPECT_TRUE(mr.equal(result21.get()));
}

TEST(wali$domains$matrix$BoolMatrix$$print, random)
{
    RandomMatrix1_3x3 f;
    BoolMatrix m(f.mat);
    std::stringstream ss;

    m.print(ss);

    EXPECT_EQ("Matrix: [3,3]((1,1,0),(1,0,1),(0,0,1))", ss.str());
}

TEST(wali$domains$matrix$BoolMatrix, callWaliTestSemElemImpl)
{
    RandomMatrix1_3x3 f;
    sem_elem_t m = new BoolMatrix(f.mat);
    test_semelem_impl(m);
}

}
}


#include <iostream>

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

#define MATRIX_SIZE 50

int main(int argc, char **argv)
{
    Eigen::Matrix<float, 2, 3> matrix_23;

    return 0;
}


#include <utils.hpp>
#include <iostream>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <partition.hpp>

using namespace boost::numeric::ublas;

int main () {

        int n = 4;
        int partitionSize = 2;

        CsrMatrix<double> m (n, n, n * n);
        for (int i = 0; i < n; ++i)
        {
                for (int j = 0; j < n; ++j)
                {
                        m(i, j) = i * (j + 1) + 1;
                }
        }
        auto ps = partition(m, 2);
        std::cout << "In:" << std::endl;
        std::cout << m << std::endl;
        std::cout << "Out: " << std::endl;
        for (int i = 0; i < ps.size(); ++i)
        {
            std::cout << ps[i] << std::endl;
        }

        for (int i = 0; i < ps.size(); ++i)
        {
            for (int j = 0; j < n; ++j)
            {
                    for (int k = 0; k < partitionSize; ++k)
                    {
                            double got = ps[i](j, k);
                            double exp = m(j, i * partitionSize + k);
                            if (exp != got) {
                                    std::cout << "Error - exp: " << exp << " got: " << got << std::endl;
                                    return 1;
                            }
                    }
            }
        }
        return 0;
}


#include <ceres/ceres.h>
#include <ceres/rotation.h>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <glog/logging.h>
#include <gtest/gtest.h>

#include <ceres-error-terms/parameterization/quaternion-param-jpl.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/quaternion-math.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>

using namespace ceres_error_terms;  // NOLINT

struct CostFunctor {
  explicit CostFunctor(const pose::Quaternion& reference)
      : reference_(reference) {}
  template <typename T>
  bool operator()(const T* const x1, const T* const x2, T* residual) const {
    typedef kindr::minimal::RotationQuaternionTemplate<T> QuaternionT;

    const Eigen::Map<const Eigen::Quaternion<T> > quaternion1(x1);
    const Eigen::Map<const Eigen::Quaternion<T> > quaternion2(x2);
    Eigen::Map<Eigen::Matrix<T, 3, 1> > error(residual);

    QuaternionT error_quaternion = common::signedQuaternionProductHamilton(
        common::signedQuaternionProductHamilton(
            QuaternionT(quaternion2), QuaternionT(quaternion1).inverse()),
        QuaternionT(reference_.toImplementation().cast<T>()).inverse());
    error = Eigen::Matrix<T, 3, 1>(
        T(2.0) * error_quaternion.x(), T(2.0) * error_quaternion.y(),
        T(2.0) * error_quaternion.z());
    return true;
  }

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 private:
  pose::Quaternion reference_;
};

// The test verifies if 4 close-to-unit quaternions (with some random noise),
// tied with relative rotations among x-axis:
// * q1->q2 = +90deg
// * q2->q3 = +90deg
// * q3->q4 = +90deg
// * q4->q1 = +90deg
// and a predefined value of q1(1, 0, 0, 0) will end up with correct relative
// and absolute rotations, i.e.:
// * q1 = 0deg
// * q2 = 90deg
// * q3 = 180deg
// * q4 = 270deg
TEST(PosegraphErrorTerms, TwoPointsRotationSolving_Minimization) {
  pose::Quaternion q1(1, 0, 0, 0);
  pose::Quaternion q2(
      0.9965159041385382, 0.06892005802323266, 0.020435594117957374,
      0.042290245850220926);
  pose::Quaternion q3(
      0.9990182922767228, -0.007804703449117966, -0.026464453222036003,
      0.03465791419330294);
  pose::Quaternion q4(
      0.9988646699884802, -0.00795566765132752, -0.043763237371076937,
      0.01705454355380988);

  // 90deg rotation as relative transformation between nodes
  pose::Quaternion reference(sqrt(2) / 2, sqrt(2) / 2, 0, 0);

  ceres::Problem problem;
  ceres::CostFunction* cost_function =
      new ceres::AutoDiffCostFunction<CostFunctor, 3, 4, 4>(
          new CostFunctor(reference));

  problem.AddResidualBlock(
      cost_function, NULL, q1.toImplementation().coeffs().data(),
      q2.toImplementation().coeffs().data());
  problem.AddResidualBlock(
      cost_function, NULL, q2.toImplementation().coeffs().data(),
      q3.toImplementation().coeffs().data());
  problem.AddResidualBlock(
      cost_function, NULL, q3.toImplementation().coeffs().data(),
      q4.toImplementation().coeffs().data());
  problem.AddResidualBlock(
      cost_function, NULL, q4.toImplementation().coeffs().data(),
      q1.toImplementation().coeffs().data());

  ceres::LocalParameterization* quaternion_parameterization =
      new ceres_error_terms::JplQuaternionParameterization;
  problem.SetParameterization(
      q1.toImplementation().coeffs().data(), quaternion_parameterization);
  problem.SetParameterization(
      q2.toImplementation().coeffs().data(), quaternion_parameterization);
  problem.SetParameterization(
      q3.toImplementation().coeffs().data(), quaternion_parameterization);
  problem.SetParameterization(
      q4.toImplementation().coeffs().data(), quaternion_parameterization);

  problem.SetParameterBlockConstant(q1.toImplementation().coeffs().data());

  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  options.minimizer_progress_to_stdout = false;
  options.gradient_tolerance = 1e-16;
  options.function_tolerance = 1e-16;
  options.parameter_tolerance = 1e-16;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  EXPECT_NEAR_KINDR_QUATERNION(
      pose::Quaternion(q2 * q1.inverse()),
      pose::Quaternion(sqrt(2) / 2, sqrt(2) / 2, 0, 0), 1e-8);
  EXPECT_NEAR_KINDR_QUATERNION(
      pose::Quaternion(q3 * q2.inverse()),
      pose::Quaternion(sqrt(2) / 2, sqrt(2) / 2, 0, 0), 1e-8);
  EXPECT_NEAR_KINDR_QUATERNION(
      pose::Quaternion(q4 * q3.inverse()),
      pose::Quaternion(sqrt(2) / 2, sqrt(2) / 2, 0, 0), 1e-8);
  EXPECT_NEAR_KINDR_QUATERNION(
      pose::Quaternion(q1 * q4.inverse()),
      pose::Quaternion(sqrt(2) / 2, sqrt(2) / 2, 0, 0), 1e-8);

  EXPECT_NEAR_KINDR_QUATERNION(q1, pose::Quaternion(1, 0, 0, 0), 1e-8);
  EXPECT_NEAR_KINDR_QUATERNION(
      q2, pose::Quaternion(sqrt(2) / 2, sqrt(2) / 2, 0, 0), 1e-8);
  EXPECT_NEAR_KINDR_QUATERNION(q3, pose::Quaternion(0, 1, 0, 0), 1e-8);
  EXPECT_NEAR_KINDR_QUATERNION(
      q4, pose::Quaternion(-sqrt(2) / 2, sqrt(2) / 2, 0, 0), 1e-8);

  LOG(INFO) << summary.BriefReport();
}

MAPLAB_UNITTEST_ENTRYPOINT


#include <vector>
#include <Eigen/Dense>

using std::vector; using std::string;

typedef Eigen::MatrixXd matrix;

int atom(int ao_index, int orbitals_per_atom)
{
    return ao_index / orbitals_per_atom;
}

int orb_index(int ao_index, int orbitals_per_atom)
{
    return ao_index % orbitals_per_atom;
}

float chi_on_atom(int o1, int o2, int o3, double dipole)
{
    if (o1 == o2 && o3 == 0)
        return 1.0;
    else if (o1 == o3 && (o3 > 0 && o3 <= 3) && o2 == 0)
        return dipole;
    else if (o2 == o3 && (o3 > 0 && o3 <= 3) && o1 == 0)
        return dipole;
    return 0.0;
}

matrix calculate_fock_matrix(matrix hamiltonian_matrix, matrix interaction_matrix, matrix density_matrix, int orbitals_per_atom, double dipole){
    int ndof = hamiltonian_matrix.row(0).size();
    matrix fock_matrix = hamiltonian_matrix;

    for (int p = 0; p < ndof; p++)
    {
        int at_p = atom(p, orbitals_per_atom);
        int orb_p = orb_index(p, orbitals_per_atom);
        for (int orb_q = 0; orb_q < orbitals_per_atom; orb_q++)
        {
            int q = orb_q + at_p * orbitals_per_atom;
            for (int orb_t = 0; orb_t < orbitals_per_atom; orb_t++)
            {
                int t = orb_t + at_p * orbitals_per_atom;
                float chi_pqt = chi_on_atom(orb_p, orb_q, orb_t, dipole);
                for (int r = 0; r < ndof; r++)
                {
                    int at_r = atom(r, orbitals_per_atom);
                    int orb_r = orb_index(r, orbitals_per_atom);
                    for (int orb_s = 0; orb_s < orbitals_per_atom; orb_s++)
                    {
                        int s = orb_s + at_r * orbitals_per_atom;
                        for (int orb_u = 0; orb_u < orbitals_per_atom; orb_u++)
                        {
                            int u = orb_u + at_r * orbitals_per_atom;
                            float chi_rsu = chi_on_atom(orb_r, orb_s, orb_u, dipole);
                            fock_matrix(p,q) += 2.0 * chi_pqt * chi_rsu * interaction_matrix(t, u) * density_matrix(r, s);
                        }
                    }
                }
            }
        }
    }
    for (int p = 0; p < ndof; p++)
    {
        int at_p = atom(p, orbitals_per_atom);
        int orb_p = orb_index(p, orbitals_per_atom);
        for (int orb_s = 0; orb_s < orbitals_per_atom; orb_s++)
        {
            int s = orb_s + at_p * orbitals_per_atom;
            for (int orb_u = 0; orb_u < orbitals_per_atom; orb_u++)
            {
                int u = orb_u + at_p * orbitals_per_atom;
                float chi_psu = chi_on_atom(orb_p, orb_s, orb_u, dipole);
                for (int q = 0; q < ndof; q++)
                {
                    int at_q = atom(q, orbitals_per_atom);
                    int orb_q = orb_index(q, orbitals_per_atom);
                    for (int orb_r = 0; orb_r < orbitals_per_atom; orb_r++)
                    {
                        int r = orb_r + at_q * orbitals_per_atom;
                        for (int orb_t = 0; orb_t < orbitals_per_atom; orb_t++)
                        {
                            int t = orb_t + at_q * orbitals_per_atom;
                            float chi_rqt = chi_on_atom(orb_r, orb_q, orb_t, dipole);
                            fock_matrix(p,q) -= chi_rqt * chi_psu * interaction_matrix(t, u) * density_matrix(r, s);
                        }
                    }
                }
            }
        }
    }
    return fock_matrix;
}


/**
 * @file spherical_kernel.hpp
 * @author Neil Slagle
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_CORE_KERNELS_SPHERICAL_KERNEL_HPP
#define MLPACK_CORE_KERNELS_SPHERICAL_KERNEL_HPP

#include <boost/math/special_functions/gamma.hpp>
#include <mlpack/prereqs.hpp>

namespace mlpack {
namespace kernel {

/**
 * The spherical kernel, which is 1 when the distance between the two argument
 * points is less than or equal to the bandwidth, or 0 otherwise.
 */
class SphericalKernel
{
 public:
  /**
   * Construct the SphericalKernel with the given bandwidth.
   */
  SphericalKernel(const double bandwidth = 1.0) :
    bandwidth(bandwidth),
    bandwidthSquared(std::pow(bandwidth, 2.0))
  { /* Nothing to do. */ }

  /**
   * Evaluate the spherical kernel with the given two vectors.
   *
   * @tparam VecTypeA Type of first vector.
   * @tparam VecTypeB Type of second vector.
   * @param a First vector.
   * @param b Second vector.
   * @return The kernel evaluation between the two vectors.
   */
  template<typename VecTypeA, typename VecTypeB>
  double Evaluate(const VecTypeA& a, const VecTypeB& b) const
  {
    return
        (metric::SquaredEuclideanDistance::Evaluate(a, b) <= bandwidthSquared) ?
        1.0 : 0.0;
  }
  /**
   * Obtains the convolution integral [integral K(||x-a||)K(||b-x||)dx]
   * for the two vectors.
   *
   * @tparam VecTypeA Type of first vector (arma::vec, arma::sp_vec should be
   *       expected).
   * @tparam VecTypeB Type of second vector.
   * @param a First vector.
   * @param b Second vector.
   * @return the convolution integral value.
   */
  template<typename VecTypeA, typename VecTypeB>
  double ConvolutionIntegral(const VecTypeA& a, const VecTypeB& b) const
  {
    double distance = sqrt(metric::SquaredEuclideanDistance::Evaluate(a, b));
    if (distance >= 2.0 * bandwidth)
    {
      return 0.0;
    }
    double volumeSquared = pow(Normalizer(a.n_rows), 2.0);

    switch (a.n_rows)
    {
      case 1:
        return 1.0 / volumeSquared * (2.0 * bandwidth - distance);
        break;
      case 2:
        return 1.0 / volumeSquared *
          (2.0 * bandwidth * bandwidth * acos(distance/(2.0 * bandwidth)) -
          distance / 4.0 * sqrt(4.0*bandwidth*bandwidth-distance*distance));
        break;
      default:
        Log::Fatal << "The spherical kernel does not support convolution\
          integrals above dimension two, yet..." << std::endl;
        return -1.0;
        break;
    }
  }
  double Normalizer(size_t dimension) const
  {
    return pow(bandwidth, (double) dimension) * pow(M_PI, dimension / 2.0) /
        std::tgamma(dimension / 2.0 + 1.0);
  }

  /**
   * Evaluate the kernel when only a distance is given, not two points.
   *
   * @param t Argument to kernel.
   */
  double Evaluate(const double t) const
  {
    return (t <= bandwidth) ? 1.0 : 0.0;
  }
  double Gradient(double t)
  {
    return t == bandwidth ? arma::datum::nan : 0.0;
  }

  //! Serialize the object.
  template<typename Archive>
  void Serialize(Archive& ar, const unsigned int /* version */)
  {
    ar & data::CreateNVP(bandwidth, "bandwidth");
    ar & data::CreateNVP(bandwidthSquared, "bandwidthSquared");
  }

 private:
  double bandwidth;
  double bandwidthSquared;
};

//! Kernel traits for the spherical kernel.
template<>
class KernelTraits<SphericalKernel>
{
 public:
  //! The spherical kernel is normalized: K(x, x) = 1 for all x.
  static const bool IsNormalized = true;
  //! The spherical kernel doesn't include a squared distance.
  static const bool UsesSquaredDistance = false;
};

} // namespace kernel
} // namespace mlpack

#endif


#ifndef STAN_MATH_FWD_MAT_FUN_DETERMINANT_HPP
#define STAN_MATH_FWD_MAT_FUN_DETERMINANT_HPP

#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/fwd/core.hpp>
#include <stan/math/fwd/mat/fun/typedefs.hpp>
#include <stan/math/prim/mat/fun/multiply.hpp>
#include <stan/math/fwd/mat/fun/multiply.hpp>
#include <stan/math/prim/mat/fun/inverse.hpp>
#include <stan/math/fwd/mat/fun/inverse.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>
#include <boost/math/tools/promotion.hpp>
#include <vector>

namespace stan {
  namespace math {

    template<typename T, int R, int C>
    inline
    fvar<T>
    determinant(const Eigen::Matrix<fvar<T>, R, C>& m) {
      check_square("determinant", "m", m);
      Eigen::Matrix<T, R, C> m_deriv(m.rows(), m.cols());
      Eigen::Matrix<T, R, C> m_val(m.rows(), m.cols());

      for (size_type i = 0; i < m.rows(); i++) {
        for (size_type j = 0; j < m.cols(); j++) {
          m_deriv(i, j) = m(i, j).d_;
          m_val(i, j) = m(i, j).val_;
        }
      }

      Eigen::Matrix<T, R, C> m_inv = inverse(m_val);
      m_deriv = multiply(m_inv, m_deriv);

      fvar<T> result;
      result.val_ = m_val.determinant();
      result.d_ = result.val_ * m_deriv.trace();

      // FIXME:  I think this will overcopy compared to retur fvar<T>(...);
      return result;
    }

  }
}
#endif


#include "TestsWithGL_pcp.h"

#include <Eigen/Dense>
#include <Eigen/Sparse>

#include "MatrixCoefficientSet.hpp"
#include "test_sim_core.h"

template <size_t row_num>
void disp_col_coefs(double col_coefs[row_num])
{
	for (size_t i = 0; i < row_num; i++)
		std::cout << col_coefs[i] << ", ";
	std::cout << "\n";
}

void test_matrix_coefficient_set(void)
{
	MatrixCoefficientSet<> mat;

	mat.init(5);
	for (size_t i = 0; i < 5; i++)
		for (size_t j = 0; j < 5; j++)
			mat.add_coefficient(i, j, double(i + j));
	mat.add_coefficient(3, 3, 2.0);
	mat.add_coefficient(1, 0, 3.5);
	mat.add_coefficient(2, 2, 1.0);
	mat.print();

	double col_coefs[5];
	std::cout << mat.del_col_and_row(3, col_coefs) << "\n";
	disp_col_coefs<5>(col_coefs);
	std::cout << mat.del_col_and_row(2, col_coefs) << "\n";
	disp_col_coefs<5>(col_coefs);

	mat.print();

	mat.add_coefficient(1, 2, 6.0);
	mat.add_coefficient(2, 2, 6.0);

	mat.print();

	Eigen::SparseMatrix<double> g_kmat(5, 5);
	g_kmat.setFromTriplets(mat.begin(), mat.end());
	std::cout << g_kmat << "\n";

	mat.init(6);
	for (size_t i = 0; i < 6; i++)
		for (size_t j = 0; j < 6; j++)
			mat.add_coefficient(i, j, double(i + j));
	mat.add_coefficient(3, 3, 2.0);
	mat.add_coefficient(1, 0, 3.5);
	mat.add_coefficient(2, 2, 1.0);
	mat.print();

	Eigen::SparseMatrix<double> g_kmat2(6, 6);
	g_kmat2.setFromTriplets(mat.begin(), mat.end());
	std::cout << g_kmat2 << "\n";

	system("pause");
}

//void cal_stiffness_mat(double kmat[20][20], double E[3][3], double dN_dx[3][8]);

void test_cal_stiffness_mat(void)
{
	double E[3][3], dN_dx[3][8], kmat[20][20];

	memset(kmat, 0, sizeof(double) * 20 * 20);

	double va = 0.0;
	Eigen::Matrix<double, 3, 3> mat33;
	for (size_t i = 0; i < 3; i++)
		for (size_t j = 0; j < 3; j++)
		{
			mat33(i, j) = va;
			E[i][j] = va;
			va += 1.0;
		}
	std::cout << mat33 << "\n";

	Eigen::Matrix<double, 3, 8> mat38;
	for (size_t i = 0; i < 3; i++)
		for (size_t j = 0; j < 8; j++)
		{
			mat38(i, j) = double(i + j);
			dN_dx[i][j] = double(i + j);
		}
	std::cout << mat38 << "\n";

	std::cout << "\n" << mat33 * mat38 << "\n";

	Eigen::Matrix<double, 8, 8> mat88 = mat38.transpose() * mat33 * mat38;
	std::cout << "\n" << mat88 << "\n";

	//cal_stiffness_mat(kmat, E, dN_dx);
	for (size_t i = 0; i < 10; i++)
	{
		for (size_t j = 0; j < 10; j++)
		{
			std::cout << kmat[i][j] << ", ";
		}
		std::cout << "\n";
	}

	system("pause");
}


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGY_GEOGRAPHIC_AREA_BOX_HPP
#define BOOST_GEOMETRY_STRATEGY_GEOGRAPHIC_AREA_BOX_HPP

#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/srs/spheroid.hpp>
#include <boost/geometry/strategies/spherical/get_radius.hpp>
#include <boost/geometry/strategy/area.hpp>
#include <boost/geometry/util/normalize_spheroidal_box_coordinates.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace area
{

// Based on the approach for spherical coordinate system:
// https://math.stackexchange.com/questions/131735/surface-element-in-spherical-coordinates
// http://www.cs.cmu.edu/afs/cs/academic/class/16823-s16/www/pdfs/appearance-modeling-3.pdf
// https://www.astronomyclub.xyz/celestial-sphere-2/solid-angle-on-the-celestial-sphere.html
// https://mathworld.wolfram.com/SolidAngle.html
// https://en.wikipedia.org/wiki/Spherical_coordinate_system
// and equations for spheroid:
// https://en.wikipedia.org/wiki/Geographic_coordinate_conversion
// https://en.wikipedia.org/wiki/Meridian_arc
// Note that the equations use geodetic latitudes so we do not have to convert them.
// assume(y_max > y_min);
// assume(x_max > x_min);
// M: a*(1-e^2) / (1-e^2*sin(y)^2)^(3/2);
// N: a / sqrt(1-e^2*sin(y)^2);
// O: N*cos(y)*M;
// tellsimp(log(abs(e*sin(y_min)+1)), p_min);
// tellsimp(log(abs(e*sin(y_min)-1)), m_min);
// tellsimp(log(abs(e*sin(y_max)+1)), p_max);
// tellsimp(log(abs(e*sin(y_max)-1)), m_max);
// S: integrate(integrate(O, y, y_min, y_max), x, x_min, x_max);
// combine(S);
//
// An alternative solution to the above formula was suggested by Charles Karney
// https://github.com/boostorg/geometry/pull/832
// The following are formulas for area of a box defined by the equator and some latitude,
// not arbitrary box.
// For e^2 > 0
// dlambda*b^2*sin(phi)/2*(1/(1-e^2*sin(phi)^2) + atanh(e*sin(phi))/(e*sin(phi)))
// For e^2 < 0
// dlambda*b^2*sin(phi)/2*(1/(1-e^2*sin(phi)^2) + atan(ea*sin(phi))/(ea*sin(phi)))
// where ea = sqrt(-e^2)
template
<
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
class geographic_box
{
public:
    template <typename Box>
    struct result_type
        : strategy::area::detail::result_type
            <
                Box,
                CalculationType
            >
    {};

    geographic_box() = default;

    explicit geographic_box(Spheroid const& spheroid)
        : m_spheroid(spheroid)
    {}

    template <typename Box>
    inline auto apply(Box const& box) const
    {
        typedef typename result_type<Box>::type return_type;

        return_type const c0 = 0;

        return_type x_min = get_as_radian<min_corner, 0>(box); // lon
        return_type y_min = get_as_radian<min_corner, 1>(box); // lat
        return_type x_max = get_as_radian<max_corner, 0>(box);
        return_type y_max = get_as_radian<max_corner, 1>(box);

        math::normalize_spheroidal_box_coordinates<radian>(x_min, y_min, x_max, y_max);

        if (x_min == x_max || y_max == y_min)
        {
            return c0;
        }

        return_type const e2 = formula::eccentricity_sqr<return_type>(m_spheroid);

        return_type const x_diff = x_max - x_min;
        return_type const sin_y_min = sin(y_min);
        return_type const sin_y_max = sin(y_max);

        if (math::equals(e2, c0))
        {
            // spherical formula
            return_type const a = get_radius<0>(m_spheroid);
            return x_diff * (sin_y_max - sin_y_min) * a * a;
        }

        return_type const c1 = 1;
        return_type const c2 = 2;
        return_type const b = get_radius<2>(m_spheroid);

        /*
        return_type const c4 = 4;
        return_type const e = math::sqrt(e2);

        return_type const p_min = log(math::abs(e * sin_y_min + c1));
        return_type const p_max = log(math::abs(e * sin_y_max + c1));
        return_type const m_min = log(math::abs(e * sin_y_min - c1));
        return_type const m_max = log(math::abs(e * sin_y_max - c1));
        return_type const n_min = e * sin_y_min * sin_y_min;
        return_type const n_max = e * sin_y_max * sin_y_max;
        return_type const d_min = e * n_min - c1;
        return_type const d_max = e * n_max - c1;

        // NOTE: For equal latitudes the original formula generated by maxima may give negative
        //   result. It's caused by the order of operations, so here they're rearranged for
        //   symmetry.
        return_type const comp0 = (p_min - m_min) / (c4 * e * d_min);
        return_type const comp1 = sin_y_min / (c2 * d_min);
        return_type const comp2 = n_min * (m_min - p_min) / (c4 * d_min);
        return_type const comp3 = (p_max - m_max) / (c4 * e * d_max);
        return_type const comp4 = sin_y_max / (c2 * d_max);
        return_type const comp5 = n_max * (m_max - p_max) / (c4 * d_max);
        return_type const comp02 = comp0 + comp1 + comp2;
        return_type const comp35 = comp3 + comp4 + comp5;

        return b * b * x_diff * (comp02 - comp35);
        */

        return_type const comp0_min = c1 / (c1 - e2 * sin_y_min * sin_y_min);
        return_type const comp0_max = c1 / (c1 - e2 * sin_y_max * sin_y_max);

        // NOTE: For latitudes equal to 0 the original formula returns NAN
        return_type comp1_min = 0, comp1_max = 0;
        if (e2 > c0)
        {
            return_type const e = math::sqrt(e2);
            return_type const e_sin_y_min = e * sin_y_min;
            return_type const e_sin_y_max = e * sin_y_max;

            comp1_min = e_sin_y_min == c0 ? c1 : atanh(e_sin_y_min) / e_sin_y_min;
            comp1_max = e_sin_y_max == c0 ? c1 : atanh(e_sin_y_max) / e_sin_y_max;
        }
        else
        {
            return_type const ea = math::sqrt(-e2);
            return_type const ea_sin_y_min = ea * sin_y_min;
            return_type const ea_sin_y_max = ea * sin_y_max;

            comp1_min = ea_sin_y_min == c0 ? c1 : atan(ea_sin_y_min) / ea_sin_y_min;
            comp1_max = ea_sin_y_max == c0 ? c1 : atan(ea_sin_y_max) / ea_sin_y_max;
        }

        return_type const comp01_min = sin_y_min * (comp0_min + comp1_min);
        return_type const comp01_max = sin_y_max * (comp0_max + comp1_max);

        return b * b * x_diff * (comp01_max - comp01_min) / c2;
    }

    Spheroid model() const
    {
        return m_spheroid;
    }

private:
    Spheroid m_spheroid;
};

}} // namespace strategy::area

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGY_GEOGRAPHIC_AREA_BOX_HPP


#include <math.h>
#include <iostream>
#include <vector>
#include <Eigen/Dense>

#define M_SQRT5 2.2360679774997896964091736687313
#define M_1_16 0.0625

#define P(a, b) velocity(a, b)
#define O(a, b) velocity(b, a)

using Eigen::MatrixXd;

/*

var s = `Eigen::MatrixXf m(${pointsX.length}, 2);\nm << `; for (var i = 0; i < pointsX.length; i++) s += pointsX[i] + ", " + pointsY[i] + (i == pointsX.length-1 ? ";" : ",\n")

http://weitz.de/hobby/

*/

float velocity(float a, float b)
{
    float cosa = cosf(a);
    float cosb = cosf(b);

    float sina = sinf(a);
    float sinb = sinf(b);

    return (4. + 2. * M_SQRT2 * (sina - M_1_16 * sinb) * (sinb - M_1_16 * sina) * (cosa - cosb)) /
           (2. + (M_SQRT5 - 1) * cosa + (3. - M_SQRT5) * cosb);
}

Eigen::MatrixXf calcVectors(Eigen::MatrixXf m)
{
    Eigen::MatrixXf v = Eigen::MatrixXf::Zero(m.rows() - 1, 2);
    for (int i = 1; i < m.rows(); i++)
    {
        float dx = m(i, 0) - m(i - 1, 0);
        float dy = m(i, 1) - m(i - 1, 1);

        v(i - 1, 0) = dx;
        v(i - 1, 1) = dy;
    }

    return v;
}

Eigen::VectorXf calcDistances(Eigen::MatrixXf m)
{
    Eigen::VectorXf d = Eigen::VectorXf::Zero(m.rows());

    for (int i = 0; i < m.rows(); i++)
    {
        d[i] = sqrtf(m(i, 0) * m(i, 0) + m(i, 1) * m(i, 1));
    }

    return d;
}

Eigen::VectorXf calcAngles(Eigen::MatrixXf m, Eigen::VectorXf d)
{
    assert(m.rows() == d.rows());

    Eigen::VectorXf a = Eigen::VectorXf::Zero(m.rows() + 1);

    for (int i = 1; i < m.rows(); i++)
    {
        // credtig goes to: https://de.mathworks.com/matlabcentral/answers/180131-how-can-i-find-the-angle-between-two-vectors-including-directional-information
        float x1 = m(i - 1, 0);
        float y1 = m(i - 1, 1);

        float x2 = m(i, 0);
        float y2 = m(i, 1);

        a[i] = atan2f(x1 * y2 - y1 * x2, x1 * x2 + y1 * y2);
    }

    // last angle is zero

    return a;
}

// Vectors a, b, c and d are const. They will not be modified
// by the function. Vector f (the solution vector) is non-const
// and thus will be calculated and updated by the function.
void thomas_algorithm(const Eigen::VectorXf &a,
                      const Eigen::VectorXf &b,
                      const Eigen::VectorXf &c,
                      const Eigen::VectorXf &d,
                      Eigen::VectorXf &x)
{
    Eigen::Index N = c.rows();

    // Create the temporary vectors
    // Note that this is inefficient as it is possible to call
    // this function many times. A better implementation would
    // pass these temporary matrices by non-const reference to
    // save excess allocation and deallocation
    std::vector<float> c_star(N, 0.0);
    std::vector<float> d_star(N, 0.0);

    // This updates the coefficients in the first row
    // Note that we should be checking for division by zero here
    c_star[0] = c[0] / b[0];
    d_star[0] = d[0] / b[0];

    // Create the c_star and d_star coefficients in the forward sweep
    for (int i = 1; i < N; i++)
    {

        float m = (b[i] - a[i] * c_star[i - 1]);
        c_star[i] = c[i] / m;
        d_star[i] = (d[i] - a[i] * d_star[i - 1]) / m;
    }

    // This is the reverse sweep, used to update the solution vector f
    x[N - 1] = d_star[N - 1];
    for (int i = N - 2; i >= 0; i--)
    {
        x[i] = d_star[i] - c_star[i] * x[i + 1];
    }
}

void solveAlpha(const Eigen::VectorXf &d, const Eigen::VectorXf &g, Eigen::VectorXf &alpha)
{
    assert(d.rows() + 1 == g.rows());

    Eigen::Index N = g.rows();

    Eigen::VectorXf a = Eigen::VectorXf::Zero(N);
    Eigen::VectorXf b = Eigen::VectorXf::Zero(N);
    Eigen::VectorXf c = Eigen::VectorXf::Zero(N);
    Eigen::VectorXf dd = Eigen::VectorXf::Zero(N);

    for (int i = 1; i < (N - 1); i++)
    {
        a[i] = 1 / d[i - 1];
        b[i] = (2 * d[i - 1] + 2 * d[i]) / (d[i - 1] * d[i]);
        c[i] = 1 / d[i];
        dd[i] = -(2 * g[i] * d[i] + g[i + 1] * d[i - 1]) / (d[i - 1] * d[i]);
    }

    float omega = 1.;
    b[0] = 2. + omega;
    c[0] = 2. * omega + 1.;
    dd[0] = -c[0] * g[1];
    a[N - 1] = 2. * omega + 1.;
    b[N - 1] = 2. + omega;
    dd[N - 1] = 0.;

    thomas_algorithm(a, b, c, dd, alpha);
}

int main()
{
    Eigen::MatrixXf m(9, 2);
    m << 61.21672821044922, 309.6958312988281,
        263.11785888671875, 680.9885864257812,
        509.5057067871094, 653.6121826171875,
        807.2243041992188, 415.77947998046875,
        300.7604675292969, 424.3345947265625,
        141.63497924804688, 723.7642822265625,
        206.65399169921875, 290.87451171875,
        182.69961547851562, 545.8175048828125,
        839.7338256835938, 602.2813720703125;

    Eigen::Index N = m.rows();

    // test velocity function
    float test = P(1.2, 0.5) - O(0.5, 1.2);
    printf("test [should be 0]: %f\n", test);

    // calculate vectors
    Eigen::MatrixXf v = calcVectors(m);
    std::cout << "Vectors:" << std::endl;
    std::cout << v << std::endl;

    // calculate distances
    Eigen::VectorXf d = calcDistances(v);
    std::cout << "Distances:" << std::endl;
    std::cout << d << std::endl;

    // calculate angles
    Eigen::VectorXf g = calcAngles(v, d);
    std::cout << "Gamma Angles:" << std::endl;
    std::cout << g << std::endl;

    // prepare thomas algorithm
    Eigen::VectorXf alpha = Eigen::VectorXf::Zero(N);
    solveAlpha(d, g, alpha);
    std::cout << "Alpha Angles:" << std::endl;
    std::cout << alpha << std::endl;

    // calculate beta, a, b, sx, sy
    Eigen::VectorXf beta = Eigen::VectorXf::Zero(N);
    Eigen::VectorXf a = Eigen::VectorXf::Zero(N);
    Eigen::VectorXf b = Eigen::VectorXf::Zero(N);

    Eigen::VectorXf sx1 = Eigen::VectorXf::Zero(N);
    Eigen::VectorXf sy1 = Eigen::VectorXf::Zero(N);
    Eigen::VectorXf sx2 = Eigen::VectorXf::Zero(N);
    Eigen::VectorXf sy2 = Eigen::VectorXf::Zero(N);

    for (int i = 0; i < N - 1; i++)
    {
        if (i < N - 2)
        {
            beta[i] = -alpha[i + 1] - g[i + 1];
        }
        else
        {
            beta[i] = -alpha[N - 1];
        }

        a[i] = P(alpha[i], beta[i]) * d[i] / 3.;
        b[i] = O(alpha[i], beta[i]) * d[i] / 3.;

        Eigen::Vector2f t(v(i, 0), v(i, 1));

        Eigen::Vector2f v1(m(i, 0), m(i, 1));
        Eigen::Vector2f v2(m(i + 1, 0), m(i + 1, 1));

        Eigen::Rotation2D<float> rotAlpha(alpha[i]);
        Eigen::Rotation2D<float> rotBeta(-beta[i]);

        Eigen::Vector2f t1 = v1 + (rotAlpha * t).normalized() * a[i];
        Eigen::Vector2f t2 = v2 - (rotBeta * t).normalized() * b[i];

        sx1[i] = t1(0);
        sy1[i] = t1(1);
        sx2[i] = t2(0);
        sy2[i] = t2(1);
    }

    std::cout << "Beta Angles:" << std::endl;
    std::cout << beta << std::endl;

    std::cout << "a:" << std::endl;
    std::cout << a << std::endl;

    std::cout << "b:" << std::endl;
    std::cout << b << std::endl;

    // print html stuff for chrome

    std::cout << "var canvas = document.createElement('canvas');\n"
              << "\n"
              << "document.body = document.createElement('body');\n"
              << "document.body.append(canvas);\n"
              << "\n"
              << "canvas.width = document.body.clientWidth;\n"
              << "canvas.height = document.body.clientHeight;\n"
              << "\n"
              << "var ctx = canvas.getContext('2d');\n"
              << "\n"
              << "ctx.beginPath();\n"
              << "\n";

    std::ostringstream bezierString;
    std::ostringstream pointString;

    bezierString << "ctx.moveTo(" << m(0, 0) << "," << m(0, 1) << ");\n\n";
    pointString << "ctx.beginPath(); ctx.arc("
                << m(0, 0) << "," << m(0, 1)
                << ", 5, 0, 2 * Math.PI, false); ctx.fill();\n";

    for (int i = 0; i < N - 1; i++)
    {
        bezierString << "ctx.bezierCurveTo("
                     << sx1(i) << "," << sy1(i) << ","
                     << sx2(i) << "," << sy2(i) << ","
                     << m(i + 1, 0) << "," << m(i + 1, 1) << ");\n";

        pointString << "ctx.beginPath(); ctx.arc("
                    << m(i + 1, 0) << "," << m(i + 1, 1)
                    << ", 5, 0, 2 * Math.PI, false); ctx.fill();\n";
    }

    bezierString << "\nctx.stroke();\n\n";

    std::cout << bezierString.str() << pointString.str();

    return 0;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

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

int main(int, char**)
{
    using namespace std;
    using mtl::lazy; using mtl::io::tout;
    typedef mtl::dense_vector<double> vt;

    mtl::compressed2D<double> A0;
    laplacian_setup(A0, 4, 15);
    tout << "A0 is\n" << A0 << endl;

    vt v(60);
    iota(v);
    tout << "v is " << v << endl;

    vt w1(A0 * v);
    tout << "A0 * v is " << w1 << endl;

    mtl::mat::poisson2D_dirichlet A(4, 15);
    vt  w2(60);
    w2= A * v;
    tout << "A * v is " << w2 << endl;

    if (one_norm(vt(w1 - w2)) > 0.001) throw "Wrong result";

    w2+= A * v;
    tout << "w2+= A * v is " << w2 << endl;
    if (one_norm(vt(w1 + w1 - w2)) > 0.001) throw "Wrong result";

    w2-= A * v;
    tout << "w2-= A * v is " << w2 << endl;
    if (one_norm(vt(w1 - w2)) > 0.001) throw "Wrong result";

    vt w3( w2 - A * v );
    double alpha;
    (lazy(w3)= A * v) || (lazy(alpha)= lazy_dot(w3, v));

    return 0;
}


#pragma once
#include <Eigen/Dense>

namespace OneACPose {

  using Vec2 = Eigen::Vector2d;
  using Vec3 = Eigen::Vector3d;

  using Mat2 = Eigen::Matrix2d;
  using Mat3 = Eigen::Matrix3d;

  using Mat23 = Eigen::Matrix<double, 2, 3>;
  using Mat32 = Eigen::Matrix<double, 3, 2>;
  using Mat34 = Eigen::Matrix<double, 3, 4>;

  using RowVec2 = Eigen::Matrix<double, 1, 2, Eigen::RowMajor>;
  using RowVec3 = Eigen::Matrix<double, 1, 3, Eigen::RowMajor>;

}


/*
put generic functions in this file.
vector operation::
* dot product
*= inplace scalar or elementwise multiplication
/= inplace elementwise division
+= inplace vector addition
-= inplace vector substraction

matrix operation::
+= inplace matrix addition
/= usage: matrix /= cons
*= usage: matrix *= cons
*/
#pragma once

#include <cstddef>
#include <string>
#include <vector>
#include <ctime>
#include <cmath>
#include <cstdlib>
#include <sstream>
#include <limits>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>
typedef std::vector<double> vec_double;
typedef std::vector<std::vector<double>> matrix;
typedef std::vector<std::vector<double>> matrix_double;

inline std::string vec_to_str(const vec_double& v) {
    std::string str("");
    for (auto& x : v) {
        str += std::to_string(x);
        str += " ";
    }
    return str;
}

inline std::string matrix_to_str(matrix_double& m){
    std::stringstream ss;
    ss.precision(2);
    for(auto& row : m){
        for(auto& ele : row){
            ss<< ele <<" ";
        }
        ss << "\n";
    }
    return ss.str();
}

inline matrix_double& operator+= (matrix_double& m1, const matrix_double& m2){
    //for debug
    if(m1.size() != m2.size()){
        std::cout<<"different size!"<<std::endl;
        std::cout<<"m1 "<<m1.size()<<" ";
        for(size_t x = 0; x < m1.size(); x++){
            std::cout<<m1[x].size()<<" ";
        }
        std::cout<<std::endl<<"m2 "<<m2.size()<<" ";
        for(size_t x = 0; x < m2.size(); x++){
            std::cout<<m2[x].size()<<" ";
        }
        std::cout<<std::endl;
    }
    //end

    for(size_t r = 0; r < m1.size(); r++){
        for(size_t c = 0; c < m1[r].size(); c++){
            m1[r][c] += m2[r][c];
        }
    }
    return m1;
}

inline matrix_double& operator/= (matrix_double& m, const double coe){
    for(size_t r = 0; r < m.size(); r++){
        for(size_t c = 0; c < m[r].size(); c++){
            m[r][c] /= coe;
        }
    }
    return m;
}
inline matrix_double& operator*= (matrix_double& m, const double coe){
    for(size_t r = 0; r < m.size(); r++){
        for(size_t c = 0; c < m[r].size(); c++){
            m[r][c] *= coe;
        }
    }
    return m;
}

inline matrix_double operator*(const matrix_double& m, const double coe){
    matrix_double result;
    for(size_t r = 0; r < m.size(); r++){
        result.push_back(vec_double(m[r].size(), 0));
        for(size_t c = 0; c < m[r].size(); c++){
            result[r][c] = m[r][c] * coe;
        }
    }
    return result;
}

/*
inline matrix& operator+= (matrix& ma, const matrix& mb) {

    int m = ma.size();
    int n = ma[0].size();
    for (int i=0; i < m; i++)
      for (int j=0; j< n; j++)
        ma[i][j] += mb[i][j];
    return ma;
}
*/

inline double sum(const vec_double& v){
  double result = 0;
  for(auto ele : v){
    result += ele;
  }
  return result;
}
// Inner Product : tolerance the case where length(va) <= length(vb)
inline double operator* (const vec_double& va, const vec_double& vb) {
    int n = va.size();
    double sum = 0.0;
    for (int i=0; i < n; i++) sum += va[i] * vb[i];
    return sum;
}

// Vector Addition
inline vec_double& operator+= (vec_double& va, const vec_double& vb) {
    int n = va.size();
    for (int i=0; i < n; i++) va[i] += vb[i];
    return va;
}
inline vec_double operator+ (const vec_double& va, const vec_double& vb) {
    int n = va.size();
    vec_double result(n, 0.0);
    for (int i=0; i < n; i++) result[i] = va[i] + vb[i];
    return result;
}

inline vec_double& operator-= (vec_double& va, const vec_double& vb) {
    int n = va.size();
    for (int i=0; i < n; i++) va[i] -= vb[i];
    return va;
}

// Scalar multiplication and division
inline vec_double& operator*= (vec_double& va, const double& c) {
    int n = va.size();
    for (int i=0; i < n; i++) va[i] *= c;
    return va;
}
inline vec_double operator* (const double& c, const vec_double& va) {
    int n = va.size();
    vec_double result(n, 0.0);
    for (int i=0; i < n; i++) result[i] = va[i] * c;
    return result;
}
inline vec_double& operator/= (vec_double& va, const double& c) {
    int n = va.size();
    for (int i=0; i < n; i++) va[i] /= c;
    return va;
}
// elementwise division
inline vec_double& operator /=(vec_double& a, const vec_double& b)
{
    std::size_t a_size = a.size();
    for(std::size_t i = 0; i < a_size; i++)
    {
        a[i] = a[i]/b[i];
    }
    return a;
}
// elementwise multiplcation
inline vec_double& operator *=(vec_double& a, const vec_double& b)
{
    std::size_t a_size = a.size();
    for(std::size_t i = 0; i < a_size; i++)
    {
        a[i] = a[i]*b[i];
    }
    return a;
}

namespace ublas = boost::numeric::ublas;
/* Matrix inversion routine.
   Uses lu_factorize and lu_substitute in uBLAS to invert a matrix */
template<class T>
bool InvertMatrix (const ublas::matrix<T>& input, ublas::matrix<T>& inverse) {

 typedef ublas::permutation_matrix<std::size_t> pmatrix;
 // create a working copy of the input
 ublas::matrix<T> A(input);
 // create a permutation matrix for the LU-factorization
 pmatrix pm(A.size1());
 // perform LU-factorization
 int res = lu_factorize(A,pm);
       if( res != 0 ) return false;
 // create identity matrix of "inverse"
 inverse.assign(ublas::identity_matrix<T>(A.size1()));
 // backsubstitute to get the inverse
 lu_substitute(A, pm, inverse);
 return true;
}

inline bool MatrixInversion(matrix& input){
  int n=input.size();
  ublas::matrix<double> input2 (n,n);
  ublas::matrix<double> output2 (n,n);
  for(int i=0;i<n;i++){
    for(int j=0;j<n;j++){
      input2(i,j)=input[i][j];
    }
  }

  if(!InvertMatrix(input2,output2))
    return false;

  for(int i=0;i<n;i++){
    for(int j=0;j<n;j++){
      input[i][j]=output2(i,j);
    }
  }
}

inline void MatrixVectormultiplication(const matrix& A,const vec_double& B,vec_double& output){
  int m=A.size();
  int n=A[0].size();
  for(int i=0;i<m;i++){
    for(int j=0;j<n;j++){
      output[i]+=A[i][j]*B[j];
    }
  }

}


// Original source code from:
// https://github.com/stegua/MyBlogEntries/tree/master/Dijkstra

/// My typedefs
#include <boost/cstdint.hpp>
#include <boost/integer_traits.hpp>
#include <inttypes.h>

typedef int32_t node_t;
typedef int32_t edge_t;
typedef int64_t cost_t;

/// From STL library
#include <fstream>

#include <vector>
using std::vector;

#include <string>

using std::make_pair;
using std::pair;

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/graph_traits.hpp>
using namespace boost;

typedef adjacency_list<vecS, vecS, directedS, no_property,
                       property<edge_weight_t, cost_t>>
    Digraph;
typedef graph_traits<Digraph>::vertex_descriptor Node;
typedef graph_traits<Digraph>::edge_descriptor Arc;

/// Boost Timer
#include <boost/progress.hpp>
using boost::timer;

using namespace boost;

/// Read input data, build graph, and run Dijkstra
cost_t runDijkstra(char *argv[]) {
  /// Read instance from the OR-lib
  std::ifstream infile(argv[1]);
  if (!infile)
    exit(EXIT_FAILURE);

  int n; /// Number of variables
  int m; /// Number of constraints

  // reads file of the form
  // #nodes #edges
  // e_1 = v_i v_j cost[e_m]
  // ..
  // e_m = v_i v_j cost[e_m]

  /// Read the first line
  infile >> n >> m;
  fprintf(stdout, "n %d, m %d\n", n, m);
  /// Build the graph
  Digraph G(n);

  int v, w;
  cost_t c;
  for (int i = 0; i < m; i++) {
    infile >> v >> w >> c;
    add_edge(v - 1, w - 1, c, G);
  }

  vector<Node> P(n);
  vector<cost_t> D(n, std::numeric_limits<cost_t>::max());
  cost_t T_dist;

  timer TIMER;
  for (int i = 0; i < 50; ++i) {
    double t0 = TIMER.elapsed();
    node_t S = i;
    node_t T = n - 1 - i;
    dijkstra_shortest_paths(G, S, predecessor_map(&P[0]).distance_map(&D[0]));
    T_dist = D[T];
    fprintf(stdout, "Time %.4f Cost %" PRId64 "\n", TIMER.elapsed() - t0,
            T_dist);
  }
  fprintf(stdout, "Tot %.4f\n", TIMER.elapsed());

  return T_dist;
}

///------------------------------------------------------------------------------------------
/// Main function
int main(int argc, char **argv) {
  if (argc != 2) {
    fprintf(stdout, "usage: ./dijkstra <filename>\n");
    exit(EXIT_FAILURE);
  }
  /// Measure overall time
  timer TIMER;
  /// Invoke the different Dijkstra algorithm implementations
  cost_t T_dist = runDijkstra(argv);
  /// Print basic figures
  fprintf(stdout, "Cost %" PRId64 " - Time %.3f\n", T_dist, TIMER.elapsed());

  return 0;
}