#define cimg_display 0

#include <iostream>
#include <vector>
#include <exception>
#include <cmath>
#include <utility>
#include <assert.h>

#include <boost/program_options.hpp>

#include "Timer.hpp"

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wshadow"
#include "CImg.h"
#pragma GCC diagnostic pop

namespace zack{

template<typename T>
T clamp( T val, std::pair<T,T>range){
  assert( range.first <= range.second);

  return std::max( std::min( val, range.second), range.first);
}

template <typename T>
double normalize( T val, std::pair<T,T> range){
  assert( range.first <= range.second);

  auto diff = abs(range.second - range.first);
  return (val - range.first ) / (double)diff;
}

template <typename T>
T lerp( double val, std::pair<T,T> range){

  T diff = (range.second - range.first);
  T off = diff * val;
  return (range.first) + off;
}

template <typename T, typename R>
R remap( T val, std::pair<T,T> range1, std::pair<R,R> range2){
  assert( range1.first <= range1.second);

  double v = normalize( val, range1);
  return lerp( v, range2);
}

}

struct Matrix_exception: public std::exception{};

template <typename T>
struct Matrix{
    std::vector<T> arr;

    size_t rows, cols;

    Matrix(std::initializer_list<std::initializer_list<T>> lst){
        cols = lst.size();
        rows = lst.begin()->size();
        for( auto row_data : lst){
            if( row_data.size() != rows ){
                throw Matrix_exception();
            }

            for( auto val : row_data ){
                arr.push_back(val);
            }
        }
    }

    template <typename F>
    void do_power(F val){
#pragma omp parallel for simd
        for(size_t i=0; i<rows; i++){
            for(size_t j=0; j<cols;j++){
                T &cur = (*this)[i][j];
                cur = std::pow(cur, val);
            }
        }
    }

    Matrix(size_t rr, size_t cc){
        rows = rr;
        cols = cc;
        arr.resize(rr*cc);
    }

    static Matrix kronecker_product( Matrix<T> &a, Matrix<T> &b){
        Matrix ret(a.rows*b.rows, a.cols *b.cols);

#pragma omp parallel for
        for( size_t brow=0; brow < b.rows; brow++){
            for( size_t bcol=0; bcol < b.cols; bcol++){
                T mul = b[brow][bcol];
                for( size_t arow=0; arow < a.rows; arow++){
                    size_t ret_row = brow * a.cols+arow;
                    for( size_t acol=0; acol < a.cols; acol++){
                        size_t ret_col = bcol*a.cols+acol;
                        ret[ret_row][ret_col] = a[arow][acol] * mul;
                    }
                }
            }
        }
        return ret;
    }

    T& wrapped_get( size_t r, size_t c){
        return (*this)[r%rows][c%cols];
    }

    T* operator[](size_t r){
        return &arr[r*cols];
    }
};

using namespace std;

Matrix<float> kpower(Matrix<float> a, int power){
    Matrix<float>b=a;

    for(int i=0; i<power; i++){
        a = Matrix<float>::kronecker_product(a,b);
    }
    a.do_power(1/(float)power);
    return a;
}

uint8_t c2f(float val){
    return zack::remap( val, make_pair(0.0f, 1.0f), make_pair(0,255));
}

int main(int argc, char **argv){

    std::string out_name;
    int raise;

    namespace po = boost::program_options;

    po::options_description desc("allowed options");
    desc.add_options()
        ("help,h", "print help")
        ("display,d", "display the image with X")
        ("bmp", po::value<std::string>(&out_name)->default_value(""), "the file to write to")
        ("iterations,i", po::value<int>(&raise)->default_value(3), "iterations for multiplication")
        ;

    po::variables_map args;
    po::store(po::parse_command_line(argc, argv, desc), args);
    args.notify();

    if( args.count("iterations") ) raise = args["iterations"].as<int>();
    if( args.count("bmp") )out_name = args["bmp"].as<std::string>();

    if( args.count("help")){
        std::cout<<desc<<std::endl;
        return 0;
    }

    const float h = 0.8;
    const float l = 0.01;
    Matrix<float> a1 {{h,h,h},
                      {l,l,h},
                      {l,h,h}};
    Matrix<float> a2 {{l,l,h},
                      {l,h,l},
                      {l,l,l}};
    Matrix<float> a3 {{h,l,h},
                      {l,h,l},
                      {l,l,h}};

    Timer power_timer;
    power_timer.start();

    Matrix<float> r = kpower(a1, raise);
    Matrix<float> g = kpower(a2,raise);
    Matrix<float> b = kpower(a3,raise);
    power_timer.stop();

    std::cout<<"Time taken to generate data: "<<std::to_string(power_timer.getTime())<<" seconds"<<std::endl;

    using namespace cimg_library;
    CImg<uint8_t> image(r.rows, r.cols, 1, 3) ;

#pragma omp parallel for
    for(size_t i=0; i<r.rows; i++){
        for(size_t j=0; j<r.cols; j++){
            const uint8_t rr = c2f(r[i][j] );
            const uint8_t gg = c2f( g[i][j] );
            const uint8_t bb = c2f( b[i][j] );
            image(i,j,0,0)=rr;
            image(i,j,0,1)=gg;
            image(i,j,0,2)=bb;
        }
    }
    if(args.count("display")){
        //image.display();
    }

    if(args.count("bmp")){
        image.save_bmp(out_name.c_str());
    }
}


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

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/index/rtree.hpp>

#define DATASET_SIZE 500
#define ELIPSON 30
#define MIN_POINTS 10

using namespace std;

struct Point {
  int x, y;
};

struct Cluster {
  int id;
  vector<int> data;
};

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

typedef bg::model::point<float, 2, bg::cs::cartesian> dataPoint;
typedef bg::model::box<dataPoint> box;
typedef std::pair<box, int> value;

class DBSCAN {
 private:
  Point dataset[DATASET_SIZE];
  int elipson;
  int minPoints;
  int cluster;
  int clusters[DATASET_SIZE];
  int getDistance(int center, int neighbor);
  vector<int> findNeighbors(int pos);
  void expandCluster(int pointId, vector<int> &neighbors);
  bgi::rtree<value, bgi::quadratic<4>> rtree;

 public:
  DBSCAN(Point dataset[DATASET_SIZE]);
  void run();
  void results();
};

int main(int, char **) {

  // Generate random datasets
  Point dataset[DATASET_SIZE];

  for (int i = 0; i < DATASET_SIZE; i++) {
    int x = rand() % 50;
    int y = rand() % 50;
    dataset[i].x = x;
    dataset[i].y = y;
  }

  printf("Random Dataset created\n");
  printf("###############################\n");

  // Print dataset in an array structure
  printf("[");
  for (int i = 0; i < DATASET_SIZE; i++) {
    printf("[%d, %d], ", dataset[i].x, dataset[i].y);
  }
  printf("]\n");

  printf("###############################\n");

  // Initialize DBSCAN with dataset
  DBSCAN dbscan(dataset);

  // Run the DBSCAN algorithm
  dbscan.run();

  // Print the cluster results of DBSCAN
  dbscan.results();

  return 0;

}

DBSCAN::DBSCAN(Point loadData[DATASET_SIZE]) {

  elipson = ELIPSON;
  minPoints = MIN_POINTS;
  cluster = 0;

  for (int i = 0; i < DATASET_SIZE; i++) {
    dataset[i].x = loadData[i].x;
    dataset[i].y = loadData[i].y;
    clusters[i] = 0;
  }

  // Create an Rtree of the dataset
  for (int i = 0; i < DATASET_SIZE; i++) {
    // create a box for each points
    box b(dataPoint(dataset[i].x, dataset[i].y),
          dataPoint(dataset[i].x, dataset[i].y));
    // insert points to the rtree
    rtree.insert(std::make_pair(b, i));

  }
}

int DBSCAN::getDistance(int center, int neighbor) {

  int dist = (dataset[center].x - dataset[neighbor].x) *
                 (dataset[center].x - dataset[neighbor].x) +
             (dataset[center].y - dataset[neighbor].y) *
                 (dataset[center].y - dataset[neighbor].y);

  return sqrt(dist);

}

void DBSCAN::run() {
  // Neighbors of the point
  vector<int> neighbors;

  for (int i = 0; i < DATASET_SIZE; i++) {

    if (clusters[i] == 0) {

      // Find neighbors of point P
      neighbors = findNeighbors(i);

      // Mark noise points
      if (neighbors.size() < minPoints) {
        clusters[i] = -1;
      } else {
        // Increment cluster and initialize it will the current point
        cluster++;

        clusters[i] = cluster;

        // Expand the neighbors of point P
        for (int j = 0; j < neighbors.size(); j++) {

          // Mark neighbour as point Q
          int dataIndex = neighbors[j];

          if(clusters[dataIndex] == -1) {
            clusters[dataIndex] = cluster;
          } else if (clusters[dataIndex] == 0) {

            clusters[dataIndex] = cluster;

            // Expand more neighbors of point Q
            vector<int> moreNeighbors;
            moreNeighbors = findNeighbors(dataIndex);

            // Continue when neighbors point is higher than minPoint threshold

            if (moreNeighbors.size() >= minPoints) {
              // Check if neighbour of Q already exists in neighbour of P
              for (int x = 0; x < moreNeighbors.size(); x++) {
                bool doesntExist = true;
                for (int y = 0; y < neighbors.size(); y++) {
                  if (moreNeighbors[x] == neighbors[y]) {
                    doesntExist = false;
                    break;
                  }
                }

                // If neighbour doesn't exist, add to neighbor list
                if (doesntExist) {
                  neighbors.push_back(moreNeighbors[x]);
                }
              }
            }
          }
      }
    }
  }
}
}

void DBSCAN::results() {
  for(int x = 1; x <= cluster; x++) {
    printf("CLuster %d: \n[\n", x);
    for(int i = 0; i < DATASET_SIZE; i++) {
      if(clusters[i] == x) {
        printf("  [%d, %d]\n", dataset[i].x, dataset[i].y);
      }
    }
    printf("]\n");
  }

}

vector<int> DBSCAN::findNeighbors(int pos) {

  vector<int> neighbors;
  Point point = dataset[pos];
  vector<value> result_n;

  // Create a search box for the given poiny
  box searchBox(dataPoint(point.x - elipson, point.y - elipson),
                dataPoint(point.x + elipson, point.y + elipson));

  // Query the intersection of search box on Rtree
  rtree.query(bgi::intersects(searchBox), std::back_inserter(result_n));

  // collect the points of box
  vector<int> pointsInBox = {};
  for (value pair : result_n) pointsInBox.push_back(pair.second);

  // Compute the distance only with points in a box
  for (int x = 0; x < pointsInBox.size(); x++) {
    // Compute neighbor points
    int distance = getDistance(pos, pointsInBox[x]);
    if (distance <= elipson && pos != pointsInBox[x]) {
      neighbors.push_back(pointsInBox[x]);
    }
  }

  return neighbors;

}


#include "dynet/saxe-init.h"
#include "dynet/tensor.h"
#include "dynet/tensor-eigen.h"
#include "dynet/globals.h"

#include <random>
#include <cstring>

#include <Eigen/SVD>

using namespace std;

namespace dynet {

void orthonormal_random(unsigned dd, float g, Tensor& x) {
  Tensor t;
  t.d = Dim({dd, dd});
  t.v = new float[dd * dd];
  normal_distribution<float> distribution(0, 0.01);
  auto b = [&] () {return distribution(*rndeng);};
  generate(t.v, t.v + dd*dd, b);
  Eigen::JacobiSVD<Eigen::MatrixXf> svd(mat(t), Eigen::ComputeFullU);
  mat(x) = svd.matrixU();
  delete[] t.v;
}

}


#include <gtest/gtest.h>
#include <boost/math/distributions.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <stan/math/prim/mat.hpp>
#include <math/prim/mat/prob/vector_rng_test_helper.hpp>
#include <limits>
#include <vector>

class BetaProportionTestRig : public VectorRealRNGTestRig {
 public:
  BetaProportionTestRig()
      : VectorRealRNGTestRig(10000, 10, {0.3, 0.4, 0.5, 0.6, 0.7}, {1, 2, 3},
                             {-2.5, -1.7, -0.1, 0.0}, {-3, -2, -1, 0},
                             {0.35, 0.5, 0.9, 1.7, 2.1, 4.1}, {1, 2, 3, 4},
                             {-2.7, -1.5, -0.5, 0.0}, {-3, -2, -1, 0}) {}

  template <typename T1, typename T2, typename T3, typename T_rng>
  auto generate_samples(const T1& mu, const T2& kappa, const T3&,
                        T_rng& rng) const {
    return stan::math::beta_proportion_rng(mu, kappa, rng);
  }

  std::vector<double> generate_quantiles(double mu, double kappa,
                                         double) const {
    // transform from location and precision parameterization
    // into shape1 (alpha) and shape2 (beta) parameterization
    double alpha = mu * kappa;
    double beta = kappa - alpha;
    std::vector<double> quantiles;
    double K = stan::math::round(2 * std::pow(N_, 0.4));
    boost::math::beta_distribution<> dist(alpha, beta);

    for (int i = 1; i < K; ++i) {
      double frac = i / K;
      quantiles.push_back(quantile(dist, frac));
    }
    quantiles.push_back(std::numeric_limits<double>::max());

    return quantiles;
  }
};

TEST(ProbDistributionsBetaProportion, errorCheck) {
  check_dist_throws_real_first_argument(BetaProportionTestRig());
}

TEST(ProbDistributionsBetaProportion, distributionTest) {
  check_quantiles_real_first_argument(BetaProportionTestRig());
}


//  (C) Copyright Nick Thompson 2018.
//  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)

#ifndef BOOST_MATH_TOOLS_SIGNAL_STATISTICS_HPP
#define BOOST_MATH_TOOLS_SIGNAL_STATISTICS_HPP

#include <algorithm>
#include <iterator>
#include <boost/math/tools/assert.hpp>
#include <boost/math/tools/complex.hpp>
#include <boost/math/tools/roots.hpp>
#include <boost/math/tools/header_deprecated.hpp>
#include <boost/math/statistics/univariate_statistics.hpp>

BOOST_MATH_HEADER_DEPRECATED("<boost/math/statistics/signal_statistics.hpp>");

namespace boost::math::tools {

template<class ForwardIterator>
auto absolute_gini_coefficient(ForwardIterator first, ForwardIterator last)
{
    using std::abs;
    using RealOrComplex = typename std::iterator_traits<ForwardIterator>::value_type;
    BOOST_MATH_ASSERT_MSG(first != last && std::next(first) != last, "Computation of the Gini coefficient requires at least two samples.");

    std::sort(first, last,  [](RealOrComplex a, RealOrComplex b) { return abs(b) > abs(a); });

    decltype(abs(*first)) i = 1;
    decltype(abs(*first)) num = 0;
    decltype(abs(*first)) denom = 0;
    for (auto it = first; it != last; ++it)
    {
        decltype(abs(*first)) tmp = abs(*it);
        num += tmp*i;
        denom += tmp;
        ++i;
    }

    // If the l1 norm is zero, all elements are zero, so every element is the same.
    if (denom == 0)
    {
        decltype(abs(*first)) zero = 0;
        return zero;
    }
    return ((2*num)/denom - i)/(i-1);
}

template<class RandomAccessContainer>
inline auto absolute_gini_coefficient(RandomAccessContainer & v)
{
    return boost::math::tools::absolute_gini_coefficient(v.begin(), v.end());
}

template<class ForwardIterator>
auto sample_absolute_gini_coefficient(ForwardIterator first, ForwardIterator last)
{
    size_t n = std::distance(first, last);
    return n*boost::math::tools::absolute_gini_coefficient(first, last)/(n-1);
}

template<class RandomAccessContainer>
inline auto sample_absolute_gini_coefficient(RandomAccessContainer & v)
{
    return boost::math::tools::sample_absolute_gini_coefficient(v.begin(), v.end());
}

// The Hoyer sparsity measure is defined in:
// https://arxiv.org/pdf/0811.4706.pdf
template<class ForwardIterator>
auto hoyer_sparsity(const ForwardIterator first, const ForwardIterator last)
{
    using T = typename std::iterator_traits<ForwardIterator>::value_type;
    using std::abs;
    using std::sqrt;
    BOOST_MATH_ASSERT_MSG(first != last && std::next(first) != last, "Computation of the Hoyer sparsity requires at least two samples.");

    if constexpr (std::is_unsigned<T>::value)
    {
        T l1 = 0;
        T l2 = 0;
        size_t n = 0;
        for (auto it = first; it != last; ++it)
        {
            l1 += *it;
            l2 += (*it)*(*it);
            n += 1;
        }

        double rootn = sqrt(n);
        return (rootn - l1/sqrt(l2) )/ (rootn - 1);
    }
    else {
        decltype(abs(*first)) l1 = 0;
        decltype(abs(*first)) l2 = 0;
        // We wouldn't need to count the elements if it was a random access iterator,
        // but our only constraint is that it's a forward iterator.
        size_t n = 0;
        for (auto it = first; it != last; ++it)
        {
            decltype(abs(*first)) tmp = abs(*it);
            l1 += tmp;
            l2 += tmp*tmp;
            n += 1;
        }
        if constexpr (std::is_integral<T>::value)
        {
            double rootn = sqrt(n);
            return (rootn - l1/sqrt(l2) )/ (rootn - 1);
        }
        else
        {
            decltype(abs(*first)) rootn = sqrt(static_cast<decltype(abs(*first))>(n));
            return (rootn - l1/sqrt(l2) )/ (rootn - 1);
        }
    }
}

template<class Container>
inline auto hoyer_sparsity(Container const & v)
{
    return boost::math::tools::hoyer_sparsity(v.cbegin(), v.cend());
}

template<class Container>
auto oracle_snr(Container const & signal, Container const & noisy_signal)
{
    using Real = typename Container::value_type;
    BOOST_MATH_ASSERT_MSG(signal.size() == noisy_signal.size(),
                     "Signal and noisy_signal must be have the same number of elements.");
    if constexpr (std::is_integral<Real>::value)
    {
        double numerator = 0;
        double denominator = 0;
        for (size_t i = 0; i < signal.size(); ++i)
        {
            numerator += signal[i]*signal[i];
            denominator += (noisy_signal[i] - signal[i])*(noisy_signal[i] - signal[i]);
        }
        if (numerator == 0 && denominator == 0)
        {
            return std::numeric_limits<double>::quiet_NaN();
        }
        if (denominator == 0)
        {
            return std::numeric_limits<double>::infinity();
        }
        return numerator/denominator;
    }
    else if constexpr (boost::math::tools::is_complex_type<Real>::value)

    {
        using std::norm;
        typename Real::value_type numerator = 0;
        typename Real::value_type denominator = 0;
        for (size_t i = 0; i < signal.size(); ++i)
        {
            numerator += norm(signal[i]);
            denominator += norm(noisy_signal[i] - signal[i]);
        }
        if (numerator == 0 && denominator == 0)
        {
            return std::numeric_limits<typename Real::value_type>::quiet_NaN();
        }
        if (denominator == 0)
        {
            return std::numeric_limits<typename Real::value_type>::infinity();
        }

        return numerator/denominator;
    }
    else
    {
        Real numerator = 0;
        Real denominator = 0;
        for (size_t i = 0; i < signal.size(); ++i)
        {
            numerator += signal[i]*signal[i];
            denominator += (signal[i] - noisy_signal[i])*(signal[i] - noisy_signal[i]);
        }
        if (numerator == 0 && denominator == 0)
        {
            return std::numeric_limits<Real>::quiet_NaN();
        }
        if (denominator == 0)
        {
            return std::numeric_limits<Real>::infinity();
        }

        return numerator/denominator;
    }
}

template<class Container>
auto mean_invariant_oracle_snr(Container const & signal, Container const & noisy_signal)
{
    using Real = typename Container::value_type;
    BOOST_MATH_ASSERT_MSG(signal.size() == noisy_signal.size(), "Signal and noisy signal must be have the same number of elements.");

    Real mu = boost::math::tools::mean(signal);
    Real numerator = 0;
    Real denominator = 0;
    for (size_t i = 0; i < signal.size(); ++i)
    {
        Real tmp = signal[i] - mu;
        numerator += tmp*tmp;
        denominator += (signal[i] - noisy_signal[i])*(signal[i] - noisy_signal[i]);
    }
    if (numerator == 0 && denominator == 0)
    {
        return std::numeric_limits<Real>::quiet_NaN();
    }
    if (denominator == 0)
    {
        return std::numeric_limits<Real>::infinity();
    }

    return numerator/denominator;

}

template<class Container>
auto mean_invariant_oracle_snr_db(Container const & signal, Container const & noisy_signal)
{
    using std::log10;
    return 10*log10(boost::math::tools::mean_invariant_oracle_snr(signal, noisy_signal));
}

// Follows the definition of SNR given in Mallat, A Wavelet Tour of Signal Processing, equation 11.16.
template<class Container>
auto oracle_snr_db(Container const & signal, Container const & noisy_signal)
{
    using std::log10;
    return 10*log10(boost::math::tools::oracle_snr(signal, noisy_signal));
}

// A good reference on the M2M4 estimator:
// D. R. Pauluzzi and N. C. Beaulieu, "A comparison of SNR estimation techniques for the AWGN channel," IEEE Trans. Communications, Vol. 48, No. 10, pp. 1681-1691, 2000.
// A nice python implementation:
// https://github.com/gnuradio/gnuradio/blob/master/gr-digital/examples/snr_estimators.py
template<class ForwardIterator>
auto m2m4_snr_estimator(ForwardIterator first, ForwardIterator last, decltype(*first) estimated_signal_kurtosis=1, decltype(*first) estimated_noise_kurtosis=3)
{
    BOOST_MATH_ASSERT_MSG(estimated_signal_kurtosis > 0, "The estimated signal kurtosis must be positive");
    BOOST_MATH_ASSERT_MSG(estimated_noise_kurtosis > 0, "The estimated noise kurtosis must be positive.");
    using Real = typename std::iterator_traits<ForwardIterator>::value_type;
    using std::sqrt;
    if constexpr (std::is_floating_point<Real>::value || std::numeric_limits<Real>::max_exponent)
    {
        // If we first eliminate N, we obtain the quadratic equation:
        // (ka+kw-6)S^2 + 2M2(3-kw)S + kw*M2^2 - M4 = 0 =: a*S^2 + bs*N + cs = 0
        // If we first eliminate S, we obtain the quadratic equation:
        // (ka+kw-6)N^2 + 2M2(3-ka)N + ka*M2^2 - M4 = 0 =: a*N^2 + bn*N + cn = 0
        // I believe these equations are totally independent quadratics;
        // if one has a complex solution it is not necessarily the case that the other must also.
        // However, I can't prove that, so there is a chance that this does unnecessary work.
        // Future improvements: There are algorithms which can solve quadratics much more effectively than the naive implementation found here.
        // See: https://stackoverflow.com/questions/48979861/numerically-stable-method-for-solving-quadratic-equations/50065711#50065711
        auto [M1, M2, M3, M4] = boost::math::tools::first_four_moments(first, last);
        if (M4 == 0)
        {
            // The signal is constant. There is no noise:
            return std::numeric_limits<Real>::infinity();
        }
        // Change to notation in Pauluzzi, equation 41:
        auto kw = estimated_noise_kurtosis;
        auto ka = estimated_signal_kurtosis;
        // A common case, since it's the default:
        Real a = (ka+kw-6);
        Real bs = 2*M2*(3-kw);
        Real cs = kw*M2*M2 - M4;
        Real bn = 2*M2*(3-ka);
        Real cn = ka*M2*M2 - M4;
        auto [S0, S1] = boost::math::tools::quadratic_roots(a, bs, cs);
        if (S1 > 0)
        {
            auto N = M2 - S1;
            if (N > 0)
            {
                return S1/N;
            }
            if (S0 > 0)
            {
                N = M2 - S0;
                if (N > 0)
                {
                    return S0/N;
                }
            }
        }
        auto [N0, N1] = boost::math::tools::quadratic_roots(a, bn, cn);
        if (N1 > 0)
        {
            auto S = M2 - N1;
            if (S > 0)
            {
                return S/N1;
            }
            if (N0 > 0)
            {
                S = M2 - N0;
                if (S > 0)
                {
                    return S/N0;
                }
            }
        }
        // This happens distressingly often. It's a limitation of the method.
        return std::numeric_limits<Real>::quiet_NaN();
    }
    else
    {
        BOOST_MATH_ASSERT_MSG(false, "The M2M4 estimator has not been implemented for this type.");
        return std::numeric_limits<Real>::quiet_NaN();
    }
}

template<class Container>
inline auto m2m4_snr_estimator(Container const & noisy_signal,  typename Container::value_type estimated_signal_kurtosis=1, typename Container::value_type estimated_noise_kurtosis=3)
{
    return m2m4_snr_estimator(noisy_signal.cbegin(), noisy_signal.cend(), estimated_signal_kurtosis, estimated_noise_kurtosis);
}

template<class ForwardIterator>
inline auto m2m4_snr_estimator_db(ForwardIterator first, ForwardIterator last, decltype(*first) estimated_signal_kurtosis=1, decltype(*first) estimated_noise_kurtosis=3)
{
    using std::log10;
    return 10*log10(m2m4_snr_estimator(first, last, estimated_signal_kurtosis, estimated_noise_kurtosis));
}

template<class Container>
inline auto m2m4_snr_estimator_db(Container const & noisy_signal,  typename Container::value_type estimated_signal_kurtosis=1, typename Container::value_type estimated_noise_kurtosis=3)
{
    using std::log10;
    return 10*log10(m2m4_snr_estimator(noisy_signal, estimated_signal_kurtosis, estimated_noise_kurtosis));
}

}
#endif


#pragma once
#ifndef NN_HPP
#define NN_HPP

#define EIGEN_NO_DEBUG

// c++ libraries
#include <iosfwd>
// eigen
#include <Eigen/Dense>
// ann - math
#include "src/math/random.hpp"
// ann - mem
#include "src/mem/serialize.hpp"
// ann - typedef
#include "src/util/typedef.hpp"

namespace NN{

//***********************************************************************
// COMPILER DIRECTIVES
//***********************************************************************

#ifndef NN_PRINT_FUNC
#define NN_PRINT_FUNC 0
#endif

#ifndef NN_PRINT_STATUS
#define NN_PRINT_STATUS 0
#endif

#ifndef NN_PRINT_DATA
#define NN_PRINT_DATA 0
#endif

//***********************************************************************
// FORWARD DECLARATIONS
//***********************************************************************

class ANN;
class ANNInit;
class Cost;
class DOutDVal;
class DOutDP;
class D2OutDPDVal;

//***********************************************************************
// INITIALIZATION METHOD
//***********************************************************************

class Init{
public:
	//enum
	enum Type{
		UNKNOWN=0,
		RAND=1,
		XAVIER=2,
		HE=3,
		MEAN=4
	};
	//constructor
	Init():t_(Type::UNKNOWN){}
	Init(Type t):t_(t){}
	//operators
	operator Type()const{return t_;}
	//member functions
	static Init read(const char* str);
	static const char* name(const Init& init);
private:
	Type t_;
	//prevent automatic conversion for other built-in types
	//template<typename T> operator T() const;
};
std::ostream& operator<<(std::ostream& out, const Init& init);

//***********************************************************************
// TRANSFER FUNCTIONS
//***********************************************************************

class Transfer{
public:
	//type
	enum Type{
		UNKNOWN=0,
		LINEAR=1,
		SIGMOID=2,
		TANH=3,
		ISRU=4,
		ARCTAN=5,
		SOFTSIGN=6,
		RELU=7,
		SOFTPLUS=8,
		ELU=9,
		GELU=10,
		SWISH=11,
		MISH=12,
		TANHRE=13
	};
	//constructor
	Transfer():t_(Type::UNKNOWN){}
	Transfer(Type t):t_(t){}
	//operators
	operator Type()const{return t_;}
	//member functions
	static Transfer read(const char* str);
	static const char* name(const Transfer& tf);
	//function
	static void tf_lin(VecXd& f, VecXd& d);
	static void tf_sigmoid(VecXd& f, VecXd& d);
	static void tf_tanh(VecXd& f, VecXd& d);
	static void tf_isru(VecXd& f, VecXd& d);
	static void tf_arctan(VecXd& f, VecXd& d);
	static void tf_softsign(VecXd& f, VecXd& d);
	static void tf_relu(VecXd& f, VecXd& d);
	static void tf_softplus(VecXd& f, VecXd& d);
	static void tf_elu(VecXd& f, VecXd& d);
	static void tf_gelu(VecXd& f, VecXd& d);
	static void tf_swish(VecXd& f, VecXd& d);
	static void tf_mish(VecXd& f, VecXd& d);
	static void tf_tanhre(VecXd& f, VecXd& d);
private:
	Type t_;
	//prevent automatic conversion for other built-in types
	//template<typename T> operator T() const;
};
std::ostream& operator<<(std::ostream& out, const Transfer& tf);

//***********************************************************************
// ANN
//***********************************************************************

/*
DEFINITIONS:
	ensemble - total set of all data (e.g. training "ensemble")
	element - single datum from ensemble
	c - "c" donotes the cost function, e.g. the gradient of the cost function w.r.t. the value of a node is dc/da
	z - "z" is the input to each node, e.g. the gradient of a node w.r.t. to its input is da/dz
	a - "a" is the value of a node, e.g. the gradient of a node w.r.t. to its input is da/dz
	o - "o" is the output of the network (i.e. out_), e.g. the gradient of the output w.r.t. the input is do/di
	i - "i" is the input of the network (i.e. in_), e.g. the gradient of the output w.r.t. the input is do/di
PRIVATE:
	VecXd in_ - raw input data for a single element of the ensemble (e.g. training set)
	VecXd inw_ - weight used to scale the input data
	VecXd inb_ - bias used to shift the input data
	VecXd out_ - raw output data given a single input element
	VecXd outw_ - weight used to scale the output data
	VecXd outb_ - bias used to shift the output data
	int nlayer_ -
		total number of hidden layers
		best thought of as the number of "connections" between layers
		nlayer_ must be greater than zero for an initialized network
		this is true even for a network with zero "hidden" layers
		an uninitialized network has nlayer_ = 0
		if we have just the input and output: nlayer_ = 1
			one set of weights,biases connecting input/output
		if we have one hidden layer: nlayer_ = 2
			two sets of weights,biases connecting input/layer0/output
		if we have two hidden layers: nlayer_ = 3
			three sets of weights,biases connecting input/layer0/layer1/output
		et cetera
	std::vector<VecXd> node_ -
		all nodes, including the input, output, and hidden layers
		the raw input and output (in_,out_) are separate from "node_"
		this is because the raw input/output may be shifted/scaled before being used
		thus, while in_/out_ are the "raw" input/output,
		the front/back of "node_" can be thought of the "scaled" input/output
		note that scaling is not necessary, but made optional with the use of in_/out_
		has a size of "nlayer_+1", as there are "nlayer_" connections between "nlayer_+1" nodes
	std::vector<VecXd> bias_ -
		the bias of each layer, best thought of as the bias "between" layers n,n+1
		bias_[n] must have the size node_[n+1] - we add this bias when going from node_[n] to node_[n+1]
		has a size of "nlayer_", as there are "nlayer_" connections between "nlayer_+1" nodes
	std::vector<MatXd> edge_ -
		the weights of each layer, best though of as transforming from layers n to n+1
		edge_[n] must have the size (node_[n+1],node_[n]) - matrix multiplying (node_[n]) to get (node_[n+1])
		has a size of "nlayer_", as there are "nlayer_" connections between "nlayer_+1" nodes
	std::vector<VecXd> dadz_ -
		the gradient of the value of a node (a) w.r.t. the input of the node (z) - da/dz
		practically, the gradient of the transfer function of each layer
		best thought of as the gradient associated with function transferring "between" layers n,n+1
		thus, dadz_[n] must have the size node_[n+1]
		has a size of "nlayer_", as there are "nlayer_" connections between "nlayer_+1" nodes
	tf_ -
		the type of the transfer function
		note the transfer function for the last layer is always linear
	tfp_ -
		(Transfer Function, Function Derivative, Vector)
		the transfer function for each layer, operates on entire vector at once
		computes both function and derivative simultaneously
*/
class ANN{
private:
	//typedefs
		typedef void (*TFP)(VecXd&,VecXd&);
	//layers
		int nlayer_;//number of layers (weights,biases)
	//transfer functions
		Transfer tf_;//transfer function type
		std::vector<TFP> tfp_;//transfer function - input for indexed layer (nlayer_)
	//input/output
		VecXd in_;//input layer
		VecXd out_;//output layer
		VecXd inw_,inb_;//input weight, bias
		VecXd outw_,outb_;//output weight, bias
	//gradients - nodes
		std::vector<VecXd> dadz_;//node derivative - not including input layer (nlayer_)
	//node weights and biases
		std::vector<VecXd> node_;//nodes (nlayer_+1)
		std::vector<VecXd> bias_;//bias (nlayer_)
		std::vector<MatXd> edge_;//edges (nlayer_)
public:
	//==== constructors/destructors ====
	ANN(){defaults();}
	~ANN(){}

	//==== operators ====
	friend std::ostream& operator<<(std::ostream& out, const ANN& n);
	friend FILE* operator<<(FILE* out, const ANN& n);
	friend VecXd& operator>>(const ANN& nn, VecXd& v);
	friend ANN& operator<<(ANN& nn, const VecXd& v);

	//==== access ====
	//network dimensions
		int nlayer()const{return nlayer_;}
	//nodes
		VecXd& in(){return in_;}
		const VecXd& in()const{return in_;}
		VecXd& out(){return out_;}
		const VecXd& out()const{return out_;}
		VecXd& node(int n){return node_[n];}
		const VecXd& node(int n)const{return node_[n];}
		int nNodes(int n)const{return node_[n].size();}
	//scaling
		VecXd& inw(){return inw_;}
		const VecXd& inw()const{return inw_;}
		VecXd& inb(){return inb_;}
		const VecXd& inb()const{return inb_;}
		VecXd& outw(){return outw_;}
		const VecXd& outw()const{return outw_;}
		VecXd& outb(){return outb_;}
		const VecXd& outb()const{return outb_;}
	//bias
		VecXd& bias(int l){return bias_[l];}
		const VecXd& bias(int l)const{return bias_[l];}
	//edge
		MatXd& edge(int l){return edge_[l];}
		const MatXd& edge(int l)const{return edge_[l];}
	//size
		int nIn()const{return in_.size();}
		int nOut()const{return out_.size();}
	//gradients - nodes
		VecXd& dadz(int n){return dadz_[n];}
		const VecXd& dadz(int n)const{return dadz_[n];}
	//transfer functions
		Transfer& tf(){return tf_;}
		const Transfer& tf()const{return tf_;}
		TFP tfp(int l){return tfp_[l];}
		const TFP tfp(int l)const{return tfp_[l];}

	//==== member functions ====
	//clearing/initialization
		void defaults();
		void clear();
	//info
		int size()const;
		int nBias()const;
		int nWeight()const;
	//resizing
		void resize(const ANNInit& init, int nInput, int nOutput);
		void resize(const ANNInit& init, int nInput, const std::vector<int>& nNodes, int nOutput);
		void resize(const ANNInit& init, const std::vector<int>& nNodes);
	//error
		double error_lambda()const;
		VecXd& grad_lambda(VecXd& grad)const;
	//execution
		const VecXd& execute();
		const VecXd& execute(const VecXd& in){in_=in;return execute();}

	//==== static functions ====
	static void write(FILE* writer, const ANN& nn);
	static void write(const char*, const ANN& nn);
	static void read(FILE* writer, ANN& nn);
	static void read(const char*, ANN& nn);
};

bool operator==(const ANN& n1, const ANN& n2);
inline bool operator!=(const ANN& n1, const ANN& n2){return !(n1==n2);}

//***********************************************************************
// ANNInit
//***********************************************************************

class ANNInit{
private:
	double bInit_;//initial value - bias
	double wInit_;//initial value - weight
	double sigma_;//distribution size parameter
	rng::dist::Name dist_;//distribution type
	Init init_;//initialization scheme
	int seed_;//random seed
public:
	//==== constructors/destructors ====
	ANNInit(){defaults();}
	~ANNInit(){}

	//==== operators ====
	friend std::ostream& operator<<(std::ostream& out, const ANNInit& init);

	//==== access ====
	double& bInit(){return bInit_;}
	const double& bInit()const{return bInit_;}
	double& wInit(){return wInit_;}
	const double& wInit()const{return wInit_;}
	double& sigma(){return sigma_;}
	const double& sigma()const{return sigma_;}
	rng::dist::Name& dist(){return dist_;}
	const rng::dist::Name& dist()const{return dist_;}
	Init& init(){return init_;}
	const Init& init()const{return init_;}
	int& seed(){return seed_;}
	const int& seed()const{return seed_;}

	//==== member functions ====
	void defaults();
	void clear(){defaults();}
};

//***********************************************************************
// Cost
//***********************************************************************

/*
dcdz_ -
	the gradient of the cost function (c) w.r.t. the node inputs (z) - dc/dz
*/
class Cost{
private:
	std::vector<VecXd> dcdz_;//derivative of cost function w.r.t. node inputs (nlayer_)
	VecXd grad_;//gradient of the cost function with respect to each parameter (bias + weight)
public:
	//==== constructors/destructors ====
	Cost(){}
	Cost(const ANN& nn){resize(nn);}
	~Cost(){}

	//==== access ====
	std::vector<VecXd>& dcdz(){return dcdz_;}
	const std::vector<VecXd>& dcdz()const{return dcdz_;}
	VecXd& grad(){return grad_;}
	const VecXd& grad()const{return grad_;}

	//==== member functions ====
	void clear();
	void resize(const ANN& nn);
	const VecXd& grad(const ANN& nn, const VecXd& dcdo);
};

//***********************************************************************
// DOutDVal
//***********************************************************************

/*
doda_ -
	the derivative of the output (o) w.r.t. the value of all nodes (a)
	has a size of "nlayer_+1" as we need to compute the gradient w.r.t. all nodes
	thus, doda_[n] must of the size node_[n]
	this includes the hidden layers as well as the input/ouput layers
	note these are the scaled inputs/outputs
dodi_ -
	the derivative of the output w.r.t. the raw input
	this is the first element of doda_ multiplied by the input scaling
*/
class DOutDVal{
private:
	MatXd dodi_;//derivative of out_ w.r.t. to in_ (out_.size() x in_.size())
	std::vector<MatXd> doda_;//derivative of out_ w.r.t. to the value "a" of all nodes (nlayer_+1)
public:
	//==== constructors/destructors ====
	DOutDVal(){}
	DOutDVal(const ANN& nn){resize(nn);}
	~DOutDVal(){}

	//==== access ====
	MatXd& dodi(){return dodi_;}
	const MatXd& dodi()const{return dodi_;}
	MatXd& doda(int n){return doda_[n];}
	const MatXd& doda(int n)const{return doda_[n];}
	std::vector<MatXd>& doda(){return doda_;}
	const std::vector<MatXd>& doda()const{return doda_;}

	//==== member functions ====
	void clear();
	void resize(const ANN& nn);
	void grad(const ANN& nn);
};

//***********************************************************************
// DOutDP
//***********************************************************************

class DOutDP{
private:
	std::vector<MatXd> dodz_;//derivative of output w.r.t. node inputs (nlayer_)
	std::vector<std::vector<VecXd> > dodb_;//derivative of output w.r.t. biases
	std::vector<std::vector<MatXd> > dodw_;//derivative of output w.r.t. weights
public:
	//==== constructors/destructors ====
	DOutDP(){}
	DOutDP(const ANN& nn){resize(nn);}
	~DOutDP(){}

	//==== access ====
	MatXd& dodz(int n){return dodz_[n];}
	const MatXd& dodz(int n)const{return dodz_[n];}
	std::vector<MatXd>& dodz(){return dodz_;}
	const std::vector<MatXd>& dodz()const{return dodz_;}
	MatXd& dodb(int n){return dodz_[n];}
	const MatXd& dodb(int n)const{return dodz_[n];}
	std::vector<std::vector<VecXd> >& dodb(){return dodb_;}
	const std::vector<std::vector<VecXd> >& dodb()const{return dodb_;}
	std::vector<std::vector<MatXd> >& dodw(){return dodw_;}
	const std::vector<std::vector<MatXd> >& dodw()const{return dodw_;}

	//==== member functions ====
	void clear();
	void resize(const ANN& nn);
	void grad(const ANN& nn);
};

//***********************************************************************
// D2OutDPDVal
//***********************************************************************

class D2OutDPDVal{
private:
	ANN nnc_;
	DOutDVal dOutDVal_;
	std::vector<MatXd> d2odpda_;
	MatXd pt1_,pt2_;
public:
	//==== constructors/destructors ====
	D2OutDPDVal(){}
	D2OutDPDVal(const ANN& nn){resize(nn);}
	~D2OutDPDVal(){}

	//==== access ====
	std::vector<MatXd>& d2odpda(){return d2odpda_;}
	const std::vector<MatXd>& d2odpda()const{return d2odpda_;}
	MatXd& d2odpda(int i){return d2odpda_[i];}
	const MatXd& d2odpda(int i)const{return d2odpda_[i];}

	//==== member functions ====
	void clear();
	void resize(const ANN& nn);
	void grad(const ANN& nn);
};

}

//**********************************************
// serialization
//**********************************************

namespace serialize{

	//**********************************************
	// byte measures
	//**********************************************

	template <> int nbytes(const NN::ANNInit& obj);
	template <> int nbytes(const NN::ANN& obj);

	//**********************************************
	// packing
	//**********************************************

	template <> int pack(const NN::ANNInit& obj, char* arr);
	template <> int pack(const NN::ANN& obj, char* arr);

	//**********************************************
	// unpacking
	//**********************************************

	template <> int unpack(NN::ANNInit& obj, const char* arr);
	template <> int unpack(NN::ANN& obj, const char* arr);

}

#endif


/*********************************************************************************/
/*                                                                               */
/*     Command-line tool to access support vector machine                        */
/*                          classification functionality.                        */
/*                                                                               */
/*     Alle Meije Wink                                                           */
/*                                                                               */
/*********************************************************************************/

/*
  Update history

  Who    When       What
  AMW    13-10-12   creation

*/

#define DLIB_PNG_SUPPORT

#include <dlib/image_io.h>                              //        -- be able to write png
#include <dlib/svm.h>                                   // n-dimensional vectors (n!=3) should use matrix in dlib
#include <dlib/matrix.h>                                //        -- see http://dlib.net/linear_algebra.html#vector
#include <dlib/svm/svm_c_linear_dcd_trainer.h>          // use svm trainer that supports "warm starting"
                                                        // see http://dlib.net/dlib/svm/active_learning.h.html

#include "combisDesign.hpp"

using namespace dlib;

int combisSVM(bis::bisnifti<value_type> *currentImage, std::string designfile)
{
    // test combis with
    // -i ~/work/documents/memorabel/PRNI2016/vumc/ECM/allmask_fmri.nii.gz --svm ~/work/documents/memorabel/PRNI2016/vumc/ECM/image_matrix.json -o ~/work/documents/memorabel/PRNI2016/vumc/ECM/weights.nii.gz

    ////////////////////////////////////////////////////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////////
    // main data types

    // classification table
    std::vector<value_type>                   y;
    std::vector<dlib::matrix<value_type,0,1>> x;

    ////////////////////////////////////////////////////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////////
    // data input from file

    // read the design
    auto design_isbinary=combisSVMdesign(designfile, &x, &y, currentImage);   // read text / binary design

    ////////////////////////////////////////////////////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////////
    // kernel design

    // type of sample (1 image) and kernel
    typedef dlib::matrix<value_type,0,1> sample_type;                              // one sample == one row of x
    typedef dlib::linear_kernel<sample_type> kernel_type;                         // kernel data type

    std::vector<sample_type> samples;                                             // sample set container
    std::vector<value_type> labels;                                                // label set container

    samples.assign(x.begin(), x.end());
    labels.assign(y.begin(), y.end());

    for (auto l: labels)
        std::cout << l << std::endl;
    if (!design_isbinary)
        for (auto s: samples)
            std::cout << s << std::endl;

    // trainer for this type of kernel
    // This trainer solves the "C" formulation of the SVM.  See the documentation for
    // details.
    dlib::svm_c_linear_dcd_trainer<kernel_type> linear_dcd_trainer;
    linear_dcd_trainer.set_c(1000);

    // normalise samples of x -- see http://dlib.net/svm_ex.cpp.html
    //dlib::vector_normalizer<sample_type> normalizer;
    //normalizer.train(samples);
    //for (auto sx:samples)
    //    sx = normalizer(sx);

    // preserve the state of the classifier for warm-starting (see active_learing.h)
    typedef typename dlib::svm_c_linear_dcd_trainer<kernel_type>::optimizer_state optimizer_state;
    optimizer_state state;

    ////////////////////////////////////////////////////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////////
    // construct decision / projection vector
    typedef decision_function< kernel_type > dftype;
    dftype decision_function = linear_dcd_trainer.train(samples, labels, state);

    sample_type m(2);
    m(0)=m(1)=0;
    for (long i = 0; i < decision_function.alpha.nr(); ++i) {
        std::cout << i << std::endl;
        std::cout << decision_function.alpha(i) << std::endl;
        std::cout << decision_function.basis_vectors(i) << std::endl;
        m += decision_function.alpha(i) * decision_function.basis_vectors(i);
    }
    std::cout << "b:" << std::endl;
    std::cout << decision_function.b << std::endl;

    #define RED     "\033[31m"      /* Red */
    #define GREEN   "\033[32m"      /* Green */
    #define YELLOW  "\033[33m"      /* Yellow */
    #define RESET   "\033[0m"

    dlib::array2d<float> map_image;
    float top=5,step=.49;
    map_image.set_size(top/step+1,top/step+1);
    for (float j=top; j>=0.; j-=step) {
        for (float i=0.1; i<=top; i+=step) {
            m(0)=i; m(1)=j;
            auto f = decision_function(m);
            map_image[i][j]=f;
            if (f<-1.)
                printf("%1.01f,%1.01f -> %s%5.02f%s  ",i,j,GREEN,-1.,RESET);
            else if (f>1.)
                printf("%1.01f,%1.01f -> %s%5.02f%s  ",i,j,RED,1.,RESET);
            else
                printf("%1.01f,%1.01f -> %s%5.02f%s  ",i,j,YELLOW,0.,RESET);
            }
        std::cout << std::endl; }
    //dlib::save_png(map_image, "/tmp/map_image.png");

    #undef RED
    #undef GREEN
    #undef YELLOW
    #undef RESET

    /*

    if (!my_machine.margin_set.empty())
        for (size_t k=0; k<my_machine.margin_set.size(); k++) {
            if (my_machine.output(my_machine.margin_key[k]))
                projection += (my_machine.weight[k] * x[my_machine.margin_key[k]]);
            else
                projection -= (my_machine.weight[k] * x[my_machine.margin_key[k]]);
        }
    if (!my_machine.error_set.empty())
        for (size_t i=0; i<my_machine.error_set.size(); i++) {
                projection -= (C * x[my_machine.every_key[my_machine.error_set[i]]]);
        }
    if (!my_machine.error_star_set.empty())
        for (size_t i=0; i<my_machine.error_star_set.size(); i++) {
                projection -= (C * x[my_machine.every_key[my_machine.error_star_set[i]]]);
        }
    auto projectionbias=my_machine.bias;

    ////////////////////////////////////////////////////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////////
    // demonstrate output

    if (y.size() == 4)
    {

        // test data with 0..5 x 0..5 grid and print classes
        double stp=1.0;//                                                             // sample the input space and show the classification
        for ( double y=5.; y>=0.; y-=stp )                                            // at all sampled points
        {
            for ( double x=0.; x<=5.; x+=stp )
            {
                ublas::vector <value_type> testxy(2);
                testxy[0]=x;
                testxy[1]=y;
                std::cout << "("
                        << std::fixed << std::setprecision(1) << x << ","
                        << std::fixed << std::setprecision(1) << y << ") -> "
                        << my_machine (testxy) << ", " ;
            }
            std::cout << std::endl;
        }
        std::cout << std::endl;

        // sample the input space and show the classification
        for ( double y=5.; y>=0.; y-=stp )                                            // at all sampled points
        {
            for ( double x=0.; x<=5.; x+=stp )
            {
                ublas::vector <value_type> testxy(2);
                testxy[0]=x;
                testxy[1]=y;
                std::cout << "("
                        << std::fixed << std::setprecision(1) << x << ","
                        << std::fixed << std::setprecision(1) << y << ") -> "
                        << projectionbias + blas::dot ( projection, testxy ) << ", " ;
            }
            std::cout << std::endl;
        }

        std::cout << "weights:\n";
        auto alpha=my_machine.weight;
        for (auto a: alpha) std::cout << a << " ";
        std::cout << std::endl;
        std::cout << "margin vectors:\n";
        {auto vecset=my_machine.margin_set;
        for (auto v: vecset) std::cout << v << " ";}
        std::cout << std::endl;
        std::cout << "bias:\n" << my_machine.bias << std::endl;
        std::cout << "C:\n" << my_machine.C << std::endl;

        std::cout << "error vectors:\n";
        {auto vecset=my_machine.error_set;
        for (auto v: vecset) std::cout << v << " ";}
        std::cout << std::endl;
        std::cout << "remaining vectors:\n";
        {auto vecset=my_machine.remaining_set;
        for (auto v: vecset) std::cout << v << " ";}
        std::cout << std::endl;

    }

    // if images -> make a projection image (weights map)
    // that shows the voting rights of each brain region
    if (design_isbinary) { // images design

        currentImage->bisArray::operator*=(0);
        {size_t i=0;
            for (auto m: mask)
                currentImage->my_data[m]=projection[i++]/mstd;
        }
        std::cout << "mask size: " << mask.size() << std::endl;

        std::cout << "projections of training images on weight map: " << std::endl;
        for (auto k:keys)
            std::cout << "specimen " << k << ", class " << y[k] << " SVM output " << my_machine(x[k]) << " projection " << projectionbias + blas::dot ( projection, x[k] ) << std::endl;

        auto iminfo=nifti_copy_nim_info(currentImage->getHeader());

        // std::cout << "storing bias in toffset " << std::endl;
        iminfo->toffset=float(projectionbias);
        //iminfo->scl_inter=float(projectionbias);

        // std::cout << "map in current image " << std::endl;
        currentImage->setHeader(iminfo);

        //nifti_image_infodump(iminfo);

    }

    ////////////////////////////////////////////////////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////////
    // cross-validation

    auto cv=1;

    if (cv) {

    std::vector<double> Cs = {1./16000., 1./8000, 1./4000., 1./2000., 1./1000.,};

    for (auto C2: Cs) {

        std::cout << "C value = " << std::setw(6) << C2 << std::endl;

        unsigned n=0,p=0,tn=0,tp=0,sv=0;
        for (auto k2:keys) {
            auto keys2 = keys;
            keys2.erase(keys2.begin()+k2);
            onlinesvm_machine_type my_machine2( C2, my_kernel, training_data );        // C = 1.0
            my_machine2.learn( keys2.begin(), keys2.end() );                           // start the learning
            auto in2=y[k2];
            auto out2=2*my_machine2(x[k2])-1;
            std::cout << "crossval " << k2 << ", class " << in2 << " SVM output " << out2
                      << ", #SV " << my_machine2.margin_set.size()+my_machine2.margin_set.size()
                      << ", bias " << my_machine2.bias << std::endl;
            if (in2<0)
                {n++; tn+=(out2<0);}
            else
                {p++; tp+=(out2>0);}
            sv=sv+my_machine2.margin_set.size();
        }

        sv/=(keys.size()-1);
        auto tnr=double(tn)/double(n);
        auto tpr=double(tp)/double(p);
        std::cout << "  true negative ratio = " << tnr
                  << ", true positive ratio = " << tpr
                  << ", balanced accuracy   = " << (tnr+tpr)/2.
                  << ", average #SV = " << sv << std::endl;

    }

    }
    */

	return 0;

}


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

// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_ALGORITHMS_ROTATION_HPP
#define BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_ALGORITHMS_ROTATION_HPP

#include <boost/geometry/util/math.hpp>

#include <boost/geometry/extensions/algebra/algorithms/detail.hpp>

#include <boost/geometry/extensions/algebra/geometries/concepts/rotation_quaternion_concept.hpp>

// TODO - for multiplication of coordinates
// if coordinate_type is_integral - use double as the result type

namespace boost { namespace geometry {

namespace detail { namespace rotation {

template <typename V1, typename V2, typename Rotation, typename Tag1, typename Tag2, std::size_t Dimension>
struct matrix
{
    BOOST_MPL_ASSERT_MSG(false, NOT_IMPLEMENTED_FOR_THIS_DIMENSION, (Rotation));
};

template <typename V1, typename V2, typename Rotation>
struct matrix<V1, V2, Rotation, vector_tag, vector_tag, 3>
{
    static const bool cs_check =
        ::boost::is_same<typename traits::coordinate_system<V1>::type, cs::cartesian>::value &&
        ::boost::is_same<typename traits::coordinate_system<V2>::type, cs::cartesian>::value;

    BOOST_MPL_ASSERT_MSG(cs_check, NOT_IMPLEMENTED_FOR_THOSE_SYSTEMS, (V1, V2));

    typedef typename geometry::select_most_precise<
        typename traits::coordinate_type<V1>::type,
        typename traits::coordinate_type<V2>::type
    >::type cv_type;

    typedef typename geometry::select_most_precise<
        cv_type,
        typename traits::coordinate_type<Rotation>::type
    >::type cr_type;

    typedef model::vector<cv_type, 3> vector_type;

    inline static void apply(V1 const& v1, V2 const& v2, Rotation & r)
    {
        namespace da = detail::algebra;

        // TODO - should store coordinates in more precise variables before the normalization?

        // angle
        cv_type d = da::dot<0, 0, 3>(v1, v2);
        cv_type l =
            math::sqrt(da::dot<0, 0, 3>(v1, v1) * da::dot<0, 0, 3>(v2, v2));
        cv_type c = d / l;

        // rotation angle == 0
        // not needed really, because in this case function still returns zero-rotation
        if ( 1 - std::numeric_limits<cv_type>::epsilon() <= c )
        {
            set<0, 0>(r, 1); set<0, 1>(r, 0); set<0, 2>(r, 0);
            set<1, 0>(r, 0); set<1, 1>(r, 1); set<1, 2>(r, 0);
            set<2, 0>(r, 0); set<2, 1>(r, 0); set<2, 2>(r, 1);
            return;
        }

        vector_type axis;

        // rotation angle = 180
        if ( c <= std::numeric_limits<cv_type>::epsilon() - 1 )
        {
            // find arbitrary rotation axis perpendicular to v1
            da::cross<0, 0, 0>(vector_type(1, 0, 0), v1, axis);
            if ( da::dot<0, 0, 3>(axis, axis) < std::numeric_limits<cr_type>::epsilon() )
                da::cross<0, 0, 0>(vector_type(0, 1, 0), v1, axis);
        }
        else
        {
            // rotation axis
            da::cross<0, 0, 0>(v1, v2, axis);
        }

        // sin
        cv_type s = math::sqrt(1 - c * c);
        cv_type t = 1 - c;
        // normalize axis
        da::normalize<0, 3>(axis);

        cv_type txx = t*get<0>(axis)*get<0>(axis);
        cv_type tyy = t*get<1>(axis)*get<1>(axis);
        cv_type tzz = t*get<2>(axis)*get<2>(axis);
        cv_type txy = t*get<0>(axis)*get<1>(axis);
        cv_type sx = s*get<0>(axis);
        cv_type txz = t*get<0>(axis)*get<2>(axis);
        cv_type sy = s*get<1>(axis);
        cv_type tyz = t*get<1>(axis)*get<2>(axis);
        cv_type sz = s*get<2>(axis);

        set<0, 0>(r, txx+c); set<0, 1>(r, txy-sz); set<0, 2>(r, txz+sy);
        set<1, 0>(r, txy+sz); set<1, 1>(r, tyy+c); set<1, 2>(r, tyz-sx);
        set<2, 0>(r, txz-sy); set<2, 1>(r, tyz+sx); set<2, 2>(r, tzz+c);
    }
};

template <typename V1, typename V2, typename Rotation>
struct matrix<V1, V2, Rotation, vector_tag, vector_tag, 2>
{
    static const bool cs_check =
        ::boost::is_same<typename traits::coordinate_system<V1>::type, cs::cartesian>::value &&
        ::boost::is_same<typename traits::coordinate_system<V2>::type, cs::cartesian>::value;

    BOOST_MPL_ASSERT_MSG(cs_check, NOT_IMPLEMENTED_FOR_THOSE_SYSTEMS, (V1, V2));

    typedef typename geometry::select_most_precise<
        typename traits::coordinate_type<V1>::type,
        typename traits::coordinate_type<V2>::type
    >::type cv_type;

    inline static void apply(V1 const& v1, V2 const& v2, Rotation & r)
    {
        namespace da = detail::algebra;

        // TODO - should store coordinates in more precise variables before the normalization?

        // angle
        cv_type d = da::dot<0, 0, 2>(v1, v2);
        cv_type l =
            math::sqrt(da::dot<0, 0, 2>(v1, v1) * da::dot<0, 0, 2>(v2, v2));
        cv_type c = d / l;

        // TODO return also if l == 0;

        // rotation angle == 0
        // not needed really, because in this case function still returns zero-rotation
        if ( 1 - std::numeric_limits<cv_type>::epsilon() <= c )
        {
            set<0, 0>(r, 1); set<0, 1>(r, 0);
            set<1, 0>(r, 0); set<1, 1>(r, 1);
        }
        // rotation angle = 180
        else if ( c <= std::numeric_limits<cv_type>::epsilon() - 1 )
        {
            set<0, 0>(r, -1); set<0, 1>(r, 0);
            set<1, 0>(r, 0); set<1, 1>(r, -1);
        }
        else
        {
            // sin
            cv_type s = (get<0>(v1) * get<1>(v2) - get<1>(v1) * get<0>(v2)) / l;

            set<0, 0>(r, c); set<0, 1>(r, -s);
            set<1, 0>(r, s); set<1, 1>(r, c);
        }
    }
};

}} // namespace detail::rotation

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch {

template <typename V1, typename V2, typename Rotation,
          typename Tag1 = typename tag<V1>::type,
          typename Tag2 = typename tag<V2>::type,
          typename RTag = typename tag<Rotation>::type
>
struct rotation
{
    BOOST_MPL_ASSERT_MSG(false, NOT_IMPLEMENTED_FOR_THOSE_TAGS, (Tag1, Tag2, Rotation));
};

template <typename V1, typename V2, typename Rotation>
struct rotation<V1, V2, Rotation, vector_tag, vector_tag, rotation_quaternion_tag>
{
    static const bool cs_check =
        ::boost::is_same<typename traits::coordinate_system<V1>::type, cs::cartesian>::value &&
        ::boost::is_same<typename traits::coordinate_system<V2>::type, cs::cartesian>::value;

    BOOST_MPL_ASSERT_MSG(cs_check, NOT_IMPLEMENTED_FOR_THOSE_SYSTEMS, (V1, V2));

    typedef typename geometry::select_most_precise<
        typename traits::coordinate_type<V1>::type,
        typename traits::coordinate_type<V2>::type
    >::type cv_type;

    typedef typename geometry::select_most_precise<
        cv_type,
        typename traits::coordinate_type<Rotation>::type
    >::type cr_type;

    typedef model::vector<cv_type, 3> vector_type;

    inline static void apply(V1 const& v1, V2 const& v2, Rotation & r)
    {
        namespace da = detail::algebra;

        // TODO - should store coordinates in more precise variables before the normalization?

        cv_type d = da::dot<0, 0, 3>(v1, v2); // l1 * l2 * cos
        cv_type l = math::sqrt(da::dot<0, 0, 3>(v1, v1) * da::dot<0, 0, 3>(v2, v2)); // l1 * l2
        cv_type w = l + d; // l1 * l2 * ( 1 + cos )

        // rotation angle == 0
        // not needed really, because in this case function still returns zero-rotation
        if ( 2*l-std::numeric_limits<cv_type>::epsilon() <= w )
        {
            set<0>(r, 1); set<0>(r, 0); set<0>(r, 0); set<0>(r, 0);
        }
        // rotation angle == pi
        else if ( w <= std::numeric_limits<cv_type>::epsilon() )
        {
            set<0>(r, 0);
            // find arbitrary rotation axis perpendicular to v1
            da::cross<0, 0, 1>(vector_type(1, 0, 0), v1, r);
            if ( da::dot<1, 1, 3>(r, r) < std::numeric_limits<cr_type>::epsilon() )
                da::cross<0, 0, 1>(vector_type(0, 1, 0), v1, r);

            // normalize axis
            da::normalize<1, 3>(r);
        }
        else
        {
            set<0>(r, w); // l1 * l2 * ( 1 + cos )
            // rotation axis
            da::cross<0, 0, 1>(v1, v2, r); // l1 * l2 * sin * UNITA

            // normalize quaternion
            da::normalize<0, 4>(r);
        }
    }
};

template <typename V1, typename V2, typename Rotation>
struct rotation<V1, V2, Rotation, vector_tag, vector_tag, rotation_matrix_tag>
    : detail::rotation::matrix<V1, V2, Rotation, vector_tag, vector_tag, traits::dimension<Rotation>::value>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

template <typename V1, typename V2, typename Rotation>
inline void rotation(V1 const& v1, V2 const& v2, Rotation & r)
{
    concepts::check_concepts_and_equal_dimensions<V1 const, V2 const>();
    // TODO - replace the following by check_equal_dimensions
    concepts::check_concepts_and_equal_dimensions<V1 const, Rotation>();

    dispatch::rotation<V1, V2, Rotation>::apply(v1, v2, r);
}

template <typename Rotation, typename V1, typename V2>
inline Rotation return_rotation(V1 const& v1, V2 const& v2)
{
    Rotation r;
    translation(v1, v2, r);
    return r;
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_ALGORITHMS_ROTATION_HPP


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

#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>

int main (int argc, char** argv)
{
    if (argc != 2) {
        std::cout << "Usage: pose_graph_gtsam sphere.g2o" << std::endl;
        exit(EXIT_FAILURE);
    }

    std::ifstream fin(argv[1]);
    if (!fin) {
        std::cout << "file " << argv[1] << " does not exist." << std::endl;
        exit(EXIT_FAILURE);
    }

    gtsam::NonlinearFactorGraph::shared_ptr factor_graph_ptr(new gtsam::NonlinearFactorGraph);  // gtsam of factor graph
    gtsam::Values::shared_ptr initial_ptr(new gtsam::Values);                                   // values of initialized

    /**
     * get vertices & edges information form the sphere.g2o files.
     */
    int cntVertex = 0, cntEdge = 0;
    std::cout << "reading from g2o file." << std::endl;

    while (!fin.eof()) {
        std::string name;
        fin >> name;

        if (name == "VERTEX_SE3:QUAT") {
            // vertex
            gtsam::Key id;
            fin >> id;

            double data[7];
            for (int i = 0; i < 7; i++) {
                fin >> data[i];
            }
            gtsam::Rot3 R = gtsam::Rot3::quaternion(data[6], data[3], data[4], data[5]);
            gtsam::Point3 t(data[0], data[1], data[2]);
            initial_ptr->insert(id, gtsam::Pose3(R, t));        // add initial values
            cntVertex ++;

        } else if (name == "EDGE_SE3:QUAT") {
            gtsam::Matrix m = gtsam::I_6x6;             // information matrix
            gtsam::Key idx1, idx2;
            fin >>  idx1 >> idx2;
            double data[7];
            for (int i = 0; i < 7; i++)
                fin >> data[i];

            gtsam::Rot3 R = gtsam::Rot3::quaternion(data[6], data[3], data[4], data[5]);
            gtsam::Point3 t(data[0], data[1], data[2]);
            for (int i = 0; i < 6; i ++ ) {
                for (int j = i; j < 6; j ++) {
                    double mij;
                    fin >> mij;
                    m(i, j) = mij;
                    m(j, i) = mij;
                }
            }

            // information matrix
            gtsam::Matrix mgsam_information = gtsam::I_6x6;
            mgsam_information.block<3, 3>(0, 0) = m.block<3, 3>(3, 3);  // cov rotation
            mgsam_information.block<3, 3>(3, 3) = m.block<3, 3>(0, 0);  // cov translation
            mgsam_information.block<3, 3>(0, 3) = m.block<3, 3>(0, 3);  // off diagonal
            mgsam_information.block<3, 3>(3, 0) = m.block<3, 3>(3, 0);  // off diagonal

            gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgsam_information);    // Gaussian noise model
            gtsam::NonlinearFactor::shared_ptr factor(new gtsam::BetweenFactor<gtsam::Pose3>(idx1, idx2, gtsam::Pose3(R, t), model)); // add a factor
            factor_graph_ptr->push_back(factor);
            cntEdge ++;
        }

        if (!fin.good()) break;
    }

    std::cout << "read total " << cntVertex << " vertices, " << " cntEdge." << std::endl;

    /* 固定第一个顶点，在gtsam中相当于添加一个先验因子 */
    gtsam::NonlinearFactorGraph graph_with_prior = *factor_graph_ptr;
    gtsam::noiseModel::Diagonal::shared_ptr prior_model = gtsam::noiseModel::Diagonal::Variances(
        (gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6).finished()
    );

    gtsam::Key first_key = 0;

//     for(const gtsam::Values::ConstKeyValuePair& key_value : *initial_ptr) {
    for (auto key_value : *initial_ptr) {
        std::cout << "Adding prior to g2o file" << std::endl;
        graph_with_prior.add(gtsam::PriorFactor<gtsam::Pose3>(
            key_value.key, key_value.value.cast<gtsam::Pose3>(), prior_model)
        );
        break;
    }

    // 开始因子图优化，配置优化选项
    std::cout << "optimizing the factor graph." << std::endl;

    // use LM optimization
    gtsam::LevenbergMarquardtParams params_lm;
    params_lm.setVerbosity("ERROR");
    params_lm.setMaxIterations(20);
    params_lm.setLinearSolverType("MULTIFRONTAL_QR");
    gtsam::LevenbergMarquardtOptimizer optimizer_LM(graph_with_prior, *initial_ptr, params_lm);

    // try use GN
//     gtsam::GaussNewtonParams params_gn;
//     params_gn.setVerbosity("ERROR");
//     params_gn.setMaxIterations(20);
//     params_gn.setLinearSolverType("MULTIFRONTAL_QR");
//     gtsam::GaussNewtonOptimizer optimizer_GN(graph_with_prior, *initial_ptr, params_gn);

    gtsam::Values result = optimizer_LM.optimize();

    std::cout << "optimization complete." << std::endl;

    std::cout << "initial error: " << factor_graph_ptr->error(*initial_ptr) << std::endl;
    std::cout << "final error: " << factor_graph_ptr->error(result) << std::endl;

    std::cout << "done.\r\nwrite to g2o ..." << std::endl;

    std::ofstream fout("result_gtsam.g2o");
    // vertex
    for (auto key_value : result) {
//     for (const gtsam::Values::ConstKeyValuePair& key_value : result) {
        gtsam::Pose3 pose = key_value.value.cast<gtsam::Pose3>();
        gtsam::Point3 t = pose.translation();
        gtsam::Quaternion q = pose.rotation().toQuaternion();
        fout << "VERTEX_SE3:QUAT " << key_value.key << " "
             << t.x() << " " << t.y() << " " << t.z() << " "
             << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << std::endl;
    }
    // edge
    for (auto factor : *factor_graph_ptr) {
//     for (gtsam::NonlinearFactor::shared_ptr factor : *factor_graph_ptr) {
        gtsam::BetweenFactor<gtsam::Pose3>::shared_ptr f = boost::dynamic_pointer_cast<gtsam::BetweenFactor<gtsam::Pose3>>(factor);
//         gtsam::BetweenFactor<gtsam::Pose3>::shared_ptr f = std::dynamic_pointer_cast<gtsam::BetweenFactor<gtsam::Pose3>>(factor);    // this program is error !!!!!!
        if (f) {
            gtsam::SharedNoiseModel model = f->noiseModel();
            gtsam::noiseModel::Gaussian::shared_ptr gaussianModel = boost::dynamic_pointer_cast<gtsam::noiseModel::Gaussian>(model);
            if (gaussianModel) {
                gtsam::Matrix info = gaussianModel->R().transpose() * gaussianModel->R();
                gtsam::Pose3 pose = f->measured();
                gtsam::Point3 t = pose.translation();
                gtsam::Quaternion q = pose.rotation().toQuaternion();

                fout << "EDGE_SE3:QUAT " << f->key1() << " " << f->key2() << " "
                     << t.x() << " " << t.y() << " " << t.z() << " "
                     << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << " ";

                gtsam::Matrix infoG2O = gtsam::I_6x6;
                infoG2O.block(0, 0, 3, 3) = info.block(3, 3, 3, 3);     // cov translation
                infoG2O.block(3, 3, 3, 3) = info.block(3, 3, 3, 3);     // cov rotation
                infoG2O.block(0, 3, 3, 3) = info.block(0, 3, 3, 3);     // off diagonal
                infoG2O.block(3, 0, 3, 3) = info.block(3, 0, 3, 3);     // off diagonal

                for (int i = 0; i < 6; i++) {
                    for (int j = i; j < 6; j++) {
                        fout << infoG2O(i, j) << " ";
                    }
                }

                fout << std::endl;
            }
        }
    }

    fout.close();
    std::cout << "done." << std::endl;

    return 0;
}


/*

// Parameters.h defines Parameters of the hardware/Simulator which are not direct part of MSCKF Algorithm

*/

#ifndef PARAMETERS_H

#define PARAMETERS_H

#include <Eigen/Core>

#include <opencv2/core/core.hpp>

struct noiseParameters

{

	// define noise params	-> noiseParams

	double u_var_prime;

	double v_var_prime;

	/*

	double sigma_gc;					//rot vel var

	double sigma_ac;					//lin accel var

	double sigma_wgc;				//gyro bias change var

	double sigma_wac;				//accel bias change var

	*/

	Eigen::Vector3d var_gc;

	Eigen::Vector3d var_ac;

	Eigen::Vector3d var_wgc;

	Eigen::Vector3d var_wac;

	Eigen::Vector3d aBias;

	Eigen::Vector3d gBias;

	Eigen::Vector3d aScale;

	Eigen::Vector3d gScale;

	noiseParameters& operator=(const noiseParameters& old){

		u_var_prime = old.u_var_prime;

		v_var_prime = old.v_var_prime;

		var_ac = old.var_ac;

		var_gc = old.var_gc;

		var_wgc = old.var_wgc;

		var_wac = old.var_wac;

		return *this;

	};

};

struct msckfParameters

{

	// define msckf params	-> msckfParams

	int minTrackLength;				// Set to inf to dead-reckon only

	int maxTrackLength;				// Set to inf to wait for features to go out of view

	double maxGNCostNorm;			// Set to inf to allow any triangulation, no matter how bad

	double minRCOND;

	bool doNullSpaceTrick;

	bool doQRdecomp;

	double fDistanceMin ;

    double fDistanceMax ;

};

struct cameraParameters

{

	// define camera params	-> camera

	unsigned int width;

	unsigned int height;

	double c_u;						// Principal point [u pixels]

	double c_v;						// Principal point [v pixels]

	double f_u;						// Focal length [u pixels]

	double f_v;						// Focal length [v pixels]

	double w;

	double Wc0;

	double Wc1;

	double Wc2;

	double Tc0;

	double Tc1;

	double Tc2;

	Eigen::Matrix3d R_C_B;

	Eigen::Vector3d Bp_c0;

	cv::Mat  distortCoeff;

	cameraParameters &operator=(const cameraParameters &old){

		c_u = old.c_u;

		c_v = old.c_v;

		f_u = old.f_u;

		f_v = old.f_v;

		w = old.w;

		R_C_B = old.R_C_B;

		Bp_c0 = old.Bp_c0;

		Wc0 = old.Wc0;

		Wc1 = old.Wc1;

		Wc2 = old.Wc2;

		Tc0 = old.Tc0;

		Tc1 = old.Tc1;

		Tc2 = old.Tc2;

		return *this;

	};

};

struct IMUCalibrationParameters

{

	// define IMU Calibration params ->

	Eigen::Matrix3d Ta;

	Eigen::Matrix3d Tg;

	Eigen::Matrix3d Ts;

};

#endif


#include <iostream>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <mumpscpp/mumpscpp.h>
#include <Eigen/core>
#include <shiva/Environment.h>
#include <shiva/Communicator.h>
#include <mumpscpp/UblasCoordinateAdaptor.h>
#include <mumpscpp/UblasVectorAdaptor.h>
#include <mumpscpp/EigenVector.h>

shiva::environment mpi_env;

void assemble_symmetric(mumpscpp::UblasCoordinateSparseMatrix& k)
{
  // lower triangular part
  const size_t matrix_size = 3;
  k.resize(matrix_size, matrix_size, false);
  k.clear();
  k.append_element(0, 0, 2.0);
  k.append_element(1, 0, -1.0);
  k.append_element(1, 1, 2.0);
  k.append_element(2, 0, 0.0);
  k.append_element(2, 1, -1.0);
  k.append_element(2, 2, 2.0);

  k.sort();
}

int main()
{
  shiva::communicator world;
  const size_t matrix_size = 3;

  mumpscpp::UblasCoordinateSparseMatrix k;
  assemble_symmetric(k);
  mumpscpp::EigenVector f = Eigen::VectorXd::Zero(matrix_size);
  f[0] = 1.25;
  f[1] = -2.0;
  f[2] = 1.75;

  mumpscpp::Mumps<double> mumps(mumpscpp::MatrixType::symmetric, mumpscpp::HostParallelism::involved, world.fortran_mpi_communicator());
  mumps.set_output_level(mumpscpp::OutputLevel::error);
  mumps.setDistributedInput(k);

  mumps.analyzeFactorize();
  mumps.solve(f);

  std::cout << f[0] << " should be " << 0.375 << "\n";
  std::cout << f[1] << " should be " << -0.5  << "\n";
  std::cout << f[2] << " should be " << 0.625 << "\n";

  mumps.destroy();
}


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <BayesFilters/Gaussian.h>
#include <BayesFilters/GaussianMixture.h>
#include <BayesFilters/AdditiveMeasurementModel.h>
#include <BayesFilters/sigma_point.h>
#include <BayesFilters/utils.h>

#include <Eigen/Dense>

#include <iostream>

using namespace Eigen;
using namespace bfl;

class SimulatedMeasurement : public AdditiveMeasurementModel
{
public:
    SimulatedMeasurement(const Ref<VectorXd>& measurement, const Ref<VectorXd>& predicted_measurement) :
        measurement_(measurement), predicted_measurement_(predicted_measurement)
    { }

    bool freeze(const Data& data) override
    {
        return true;
    }

    std::pair<bool, Data> measure(const Data& data = Data()) const override
    {
        MatrixXd measurement(measurement_.size(), 1);
        measurement.col(0) = measurement_;
        return std::make_pair(true, measurement);
    }

    std::pair<bool, Data> predictedMeasure(const Ref<const MatrixXd>& cur_states) const override
    {
        MatrixXd predicted = MatrixXd::Zero(measurement_.size(), cur_states.cols());
        predicted.colwise() += predicted_measurement_;
        return std::make_pair(true, predicted);
    }

    std::pair<bool, Data> innovation(const Data& predicted_measurements, const Data& measurements) const override
    {
        MatrixXd innovation = -(any::any_cast<MatrixXd>(predicted_measurements).colwise() - any::any_cast<MatrixXd>(measurements).col(0));

        return std::make_pair(true, std::move(innovation));
    }

    std::pair<bool, MatrixXd> getNoiseCovarianceMatrix() const
    {
        VectorXd covariance(measurement_.size());
        for (std::size_t i = 0; i < measurement_.size(); i++)
            covariance(i) = 0.1 * (i + 1);

        MatrixXd covariance_matrix = covariance.asDiagonal();

        return std::make_pair(true, covariance_matrix);
    }

    std::pair<std::size_t, std::size_t> getOutputSize() const override
    {
        return std::pair<std::size_t, std::size_t>(measurement_.size(), 0);
    }

private:
    VectorXd measurement_;

    VectorXd predicted_measurement_;
};

int main()
{
    std::cout << "[Test] " << std::endl;
    {
        std::cout << "Constructing a simulated scenario..." << std::endl;

        // Unscented Transform parameters
        double alpha = 1.0;
        double beta = 2.0;
        double kappa = 0.0;
        sigma_point::UTWeight ut_weight(3, alpha, beta, kappa);

        // Predicted state
        Gaussian predicted_state(3);
        Matrix3d covariance;
        covariance << 0.01, 0.0,  0.0,
                      0.0,  0.01, 0.0,
                      0.0,  0.0,  0.01;
        predicted_state.mean() = Vector3d::Zero();
        predicted_state.covariance() = covariance;

        // Measurement model
        VectorXd measurement(6);
        measurement(0) = 1.05;
        measurement(1) = -0.05;
        measurement(2) = 0.05;
        measurement(3) = 0.95;
        measurement(4) = 1.1;
        measurement(5) = 0.8;

        VectorXd predicted(6);
        predicted(0) = 1.0;
        predicted(1) = 0.0;
        predicted(2) = 0.0;
        predicted(3) = 1.0;
        predicted(4) = 1.0;
        predicted(5) = 1.0;

        SimulatedMeasurement measurement_model(measurement, predicted);

        // Propagate belief
        GaussianMixture predicted_measurement(1, 6);
        MatrixXd cross_covariance;
        std::tie(std::ignore, predicted_measurement, cross_covariance) = sigma_point::unscented_transform(predicted_state, ut_weight, measurement_model);

        // Extract mean and predicted measurement covariance
        VectorXd my = predicted_measurement.mean(0);
        MatrixXd Py = predicted_measurement.covariance(0);

        std::cout << "done." << std::endl;

        // Evaluate likelihood using standard Gaussian
        VectorXd likelihood_0 = utils::multivariate_gaussian_density(measurement, my, Py);

        std::cout << "Evaluated likelihood using standard Gaussian evaluation is " << likelihood_0(0) << std::endl;

        // Evaluate likelihood using Gaussian having covariance in the form S = UV + R with R block diagonal

        // Evaluate the predicted measurement covariance using the form Py = YY^{T} + R with U = Y, V = Y^{T}
        MatrixXd input_sigma_points = sigma_point::sigma_point(predicted_state, ut_weight.c);

        bfl::Data prediction;
        std::tie(std::ignore, prediction) = measurement_model.predictedMeasure(input_sigma_points);
        MatrixXd propagated_sigma_points = bfl::any::any_cast<MatrixXd&&>(std::move(prediction));

        MatrixXd sqrt_ut_weight = ut_weight.covariance.array().sqrt().matrix().asDiagonal();
        MatrixXd Y = propagated_sigma_points.colwise() - my;
        Y *= sqrt_ut_weight;

        MatrixXd R_full;
        std::tie(std::ignore, R_full) = measurement_model.getNoiseCovarianceMatrix();

        // Compose the R matrix with adjacent diagonal blocks, as required by the method
        // utils::multivariate_gaussian_UVR()
        MatrixXd R(2, 6);
        for (std::size_t i = 0; i < R_full.cols() / 2; i++)
            R.block<2, 2>(0, 2 * i) = R_full.block<2, 2>(2 * i, 2 * i);

        VectorXd likelihood_1 = utils::multivariate_gaussian_density_UVR(measurement, my, Y, Y.transpose(), R);
        std::cout << "Evaluated likelihood using Gaussian with UVR factorized covariance matrix is " << likelihood_1(0) << std::endl;

        if (!(std::abs(likelihood_0(0) - likelihood_1(0)) < 0.00001))
        {
            std::cerr << "Evaluation of likelihood using Gaussian with UVR factorized covariance matrix failed."
                      << "Should be " << likelihood_0(0) << ", is " << likelihood_1(0) << std::endl;
            return EXIT_FAILURE;
        }
    }

    std::cout << "done." << std::endl;

    return EXIT_SUCCESS;
}


#include <Eigen/Dense>

#include <openbr/plugins/openbr_internal.h>
#include <openbr/core/eigenutils.h>

namespace br
{

/*!
 * \ingroup transforms
 * \brief Designed for use after eye detection + Stasm, this will
 * revert the detected landmarks to the original coordinate space
 * before affine alignment to the stasm mean shape. The storeAffine
 * parameter must be set to true when calling AffineTransform before this.
 * \author Brendan Klare \cite bklare
 */
class RevertAffineTransform : public UntrainableTransform
{
    Q_OBJECT

private:

    void project(const Template &src, Template &dst) const
    {
        QList<float> paramList = src.file.getList<float>("affineParameters");
        Eigen::MatrixXf points = EigenUtils::pointsToMatrix(src.file.points(), true);
        Eigen::MatrixXf affine = Eigen::MatrixXf::Zero(3, 3);
        for (int i = 0, cnt = 0; i < 2; i++)
            for (int j = 0; j < 3; j++, cnt++)
                affine(i, j) = paramList[cnt];
        affine(2, 2) = 1;
        affine = affine.inverse();
        Eigen::MatrixXf affineInv = affine.block(0, 0, 2, 3);
        Eigen::MatrixXf pointsT = points.transpose();
        points =  affineInv * pointsT;
        dst = src;
        dst.file.clearPoints();
        dst.file.setPoints(EigenUtils::matrixToPoints(points.transpose()));
    }
};

BR_REGISTER(Transform, RevertAffineTransform)

} // namespace br

#include "imgproc/revertaffine.moc"


#include <cinttypes>
#include <cmath>
#include <complex>
#include <iostream>

#define png_infopp_NULL nullptr
#define int_p_NULL      nullptr
#include <boost/gil/gil_all.hpp>
#include <boost/gil/extension/io/png_io.hpp>

namespace gil = boost::gil;

struct mandelbrot_fn {
    using const_t           = mandelbrot_fn;
    using value_type        = gil::gray8_pixel_t;
    using reference         = value_type;
    using const_reference   = value_type;
    using point_t           = gil::point2<int>;
    using result_type       = value_type;
    using argument_type     = point_t;
    static constexpr bool is_mutable = false;

    explicit mandelbrot_fn(point_t const& size) :
        m_size{ size }
    {}

    auto operator()(point_t const& p) const -> result_type {
        auto map = [](double val, double r1_from, double r1_to, double r2_from, double r2_to) -> double {
            return ((val - r1_from) / (r1_to - r1_from)) * (r2_to - r2_from) + r2_from;
        };

        // map x to [-2, 1] and y to [1.5, -1.5]
        std::complex<double> c{ map(p.x, 0, m_size.x, -2, 1), map(p.y, 0, m_size.y, 1.5, -1.5) };
        auto lc = c;
        for (auto i = 0; i < 100; ++i) {
            if (std::pow(lc.real(), 2) + std::pow(lc.imag(), 2) > 4) {
                return result_type{ static_cast<gil::bits8>(i / 100.0 * 255) };
            }
            lc = std::pow(lc, 2) + c;
        }

        return result_type{ 0 };
    }

private:
    point_t m_size;
};

int main() {
    using point_t = mandelbrot_fn::point_t;
    using locator_t = gil::virtual_2d_locator<mandelbrot_fn, false>;
    using image_view_t = gil::image_view<locator_t>;

    point_t size{ 5000, 5000 };
    image_view_t view{ size, locator_t{ point_t{ 0, 0 }, point_t{ 1, 1 }, mandelbrot_fn{ size } } };
    gil::png_write_view("mandelbrot.png", view);
}


#pragma once
#include <Eigen/Core>
#include <cassert>
#include <stdexcept>
#include <tuple>

//! The gradient of the shape function (on the reference element)
//!
//! We have three shape functions
//!
//! @param i integer between 0 and 2 (inclusive). Decides which shape function to return.
//! @param x x coordinate in the reference element.
//! @param y y coordinate in the reference element.
inline Eigen::Vector2d gradientLambda(const int i, double x, double y) {
	// (write your solution here)
	std::ignore = x;
	std::ignore = y;
	assert(0 <= i && i <= 2);
	switch (i) {
		case 0:
			return Eigen::Vector2d(-1, -1);
		case 1:
			return Eigen::Vector2d(1, 0);
		case 2:
			return Eigen::Vector2d(0, 1);
		default:
			throw std::domain_error("i not in {0,1,2}");
	}

	return Eigen::Vector2d(0, 0); //remove when implemented
}


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

  @copyright 2015 NumScale SAS

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

**/
//==================================================================================================
#ifndef BOOST_SIMD_ARITHMETIC_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_HPP_INCLUDED

namespace boost { namespace simd
{
  /*!
    @ingroup group-functions
    @defgroup group-arithmetic Arithmetic functions

    Those functions provides scalar and SIMD algorithms for classical arithmetic operators and
    functions provided by the C and C++ standard library. Other functions like, in particular,
    provision for saturated operations are also provided.

  **/

  /*!
    @ingroup group-callable
    @defgroup group-callable-arithmetic Arithmetic Callable Objects
    Callable objects version of @ref group-arithmetic

    Their specific semantic limitations are similar to those of their function
    equivalents as described in the @ref group-arithmetic section.
  **/
} }

#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/abss.hpp>
#include <boost/simd/function/adds.hpp>
#include <boost/simd/function/angle.hpp>
#include <boost/simd/function/arg.hpp>
#include <boost/simd/function/average.hpp>
#include <boost/simd/function/ceil.hpp>
#include <boost/simd/function/conj.hpp>
#include <boost/simd/function/correct_fma.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/decs.hpp>
#include <boost/simd/function/dist.hpp>
#include <boost/simd/function/dists.hpp>
#include <boost/simd/function/div.hpp>
#include <boost/simd/function/divs.hpp>
#include <boost/simd/function/drem.hpp>
#include <boost/simd/function/extract.hpp>
#include <boost/simd/function/fabs.hpp>
#include <boost/simd/function/fix.hpp>
#include <boost/simd/function/floor.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/fmod.hpp>
#include <boost/simd/function/fms.hpp>
#include <boost/simd/function/fnma.hpp>
#include <boost/simd/function/fnms.hpp>
#include <boost/simd/function/hypot.hpp>
#include <boost/simd/function/iceil.hpp>
#include <boost/simd/function/idiv.hpp>
#include <boost/simd/function/ifix.hpp>
#include <boost/simd/function/ifloor.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/incs.hpp>
#include <boost/simd/function/iround2even.hpp>
#include <boost/simd/function/iround.hpp>
#include <boost/simd/function/itrunc.hpp>
#include <boost/simd/function/max.hpp>
#include <boost/simd/function/meanof.hpp>
#include <boost/simd/function/min.hpp>
#include <boost/simd/function/minmod.hpp>
#include <boost/simd/function/minusone.hpp>
#include <boost/simd/function/mod.hpp>
#include <boost/simd/function/modulo.hpp>
#include <boost/simd/function/modulus.hpp>
#include <boost/simd/function/muls.hpp>
#include <boost/simd/function/neg.hpp>
#include <boost/simd/function/negs.hpp>
#include <boost/simd/function/oneminus.hpp>
#include <boost/simd/function/oneplus.hpp>
#include <boost/simd/function/rdivide.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/remainder.hpp>
#include <boost/simd/function/remfix.hpp>
#include <boost/simd/function/rem.hpp>
#include <boost/simd/function/remquo.hpp>
#include <boost/simd/function/remround.hpp>
#include <boost/simd/function/rint.hpp>
#include <boost/simd/function/round2even.hpp>
#include <boost/simd/function/round.hpp>
#include <boost/simd/function/rsqrt.hpp>
#include <boost/simd/function/signbit.hpp>
#include <boost/simd/function/sqr_abs.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/sqrs.hpp>
#include <boost/simd/function/sqrt1pm1.hpp>
#include <boost/simd/function/sqrt.hpp>
#include <boost/simd/function/subs.hpp>
#include <boost/simd/function/tenpower.hpp>
#include <boost/simd/function/tofloat.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/function/toints.hpp>
#include <boost/simd/function/touint.hpp>
#include <boost/simd/function/touints.hpp>
#include <boost/simd/function/trunc.hpp>
#include <boost/simd/function/two_add.hpp>
#include <boost/simd/function/two_prod.hpp>
#include <boost/simd/function/two_split.hpp>
#include <boost/simd/function/unary_minus.hpp>
#include <boost/simd/function/unary_plus.hpp>

#endif


//
// Created by riku on 1/3/18.
//
#include <NTL/ZZ.h>
#include <HElib/NumbTh.h>
#include <HElib/FHEContext.h>
#include <iostream>
void MiniONN(long m) {
    long p = NTL::RandomPrime_long(16, 20);
    while (true) {
        long d = multOrd(p, m);
        if (d == 1)
            break;
        do {
            p = NTL::NextPrime(p + 2);
        } while (m % p == 0);
    };
    std::cout << m << " " << p << std::endl;
}

bool check(NTL::ZZX const& factor) {
	for (long i = 1; i < NTL::deg(factor); i++) {
		if (NTL::coeff(factor, i) != 0)
			return false;
	}
	return true;
}

void DoublePacking(long m, long slots) {
	long p = NTL::RandomPrime_long(17, 20);
	long phim = phi_N(m);
	assert(phim == (m >> 1));
	long count = 0;
	while (count < 10) {
		long d = multOrd(p, m);
		long s = phim / d;
		if (s == slots) {
			FHEcontext context(m, p, 1);
			const auto &ftrs = context.alMod.getFactorsOverZZ();
			bool ok = true;
			for (const auto& f : ftrs)
				ok &= check(f);
			if (ok) {
				printf("%ld %ld %f\n", m, p, std::log(p) / std::log(2.));
				count += 1;
			}
		}
		do {
            p = NTL::NextPrime(p + 2);
        } while (m % p == 0);
	};
}

int main() {
    //MiniONN(8192);
	DoublePacking(16384, 64);
	//DoublePacking(8192, 64);
	// DoublePacking(8192, 256);
	// DoublePacking(8192, 512);
	// DoublePacking(8192, 1024);
	// DoublePacking(8192, 2048);
    return 0;
}


#include <k52/optimization/conjugate_gradient_method.h>

#ifdef BUILD_WITH_MPI

#include <boost/mpi.hpp>
#include <k52/parallel/mpi/constants.h>

#endif

#include <cmath>
#include <stdexcept>
#include <iostream>

#include <k52/common/floating_point.h>
#include <k52/optimization/params/i_continuous_parameters.h>

using ::std::vector;
using ::k52::common::FloatingPoint;

namespace k52
{
namespace optimization
{

ConjugateGradientMethod::ConjugateGradientMethod(
    double precision,
    double increment_of_the_argument,
    size_t number_of_iterations)
{
    precision_ = precision;
    increment_of_the_argument_ = increment_of_the_argument;
    number_of_iterations_ = number_of_iterations;
}

ConjugateGradientMethod* ConjugateGradientMethod::Clone() const
{
    return new ConjugateGradientMethod(precision_, increment_of_the_argument_, number_of_iterations_);
}

std::string ConjugateGradientMethod::get_name() const
{
    return "Conjugate Gradient Method";
}

#ifdef BUILD_WITH_MPI
void ConjugateGradientMethod::Send(boost::mpi::communicator* communicator, int target) const
{
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, precision_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, increment_of_the_argument_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, number_of_iterations_);
}

void ConjugateGradientMethod::Receive(boost::mpi::communicator* communicator, int source)
{
    communicator->recv(source,
        k52::parallel::mpi::constants::kCommonTag,
        precision_);
    communicator->recv(source,
        k52::parallel::mpi::constants::kCommonTag,
        increment_of_the_argument_);
    communicator->recv(source,
        k52::parallel::mpi::constants::kCommonTag,
        number_of_iterations_);
}
#endif

vector<double> ConjugateGradientMethod::FindOptimalParameters(
    const vector<double>& initial_parameters)
{
    vector<double> parameters = initial_parameters;

    vector<double> gradient = CalculateGradient(parameters);
    vector<double> previous_gradient(parameters.size());

    vector<double> search_direction(parameters.size());
    vector<double> previous_search_direction(parameters.size());

    double weighting_coefficient = 0;
    double exit = 0;
    size_t iteration = 1;

    //http://en.wikipedia.org/wiki/Nonlinear_conjugate_gradient_method
    do
    {
        previous_search_direction = search_direction;
        search_direction = FindNextSearchDirection(gradient, previous_search_direction, weighting_coefficient, iteration);

        double minimizing_parameter = PerformOneDimensionalSearch(parameters, search_direction);
        //TODO use vector math
        for (size_t i = 0; i<parameters.size(); i++)
        {
            parameters[i] += minimizing_parameter * search_direction[i];
        }

        previous_gradient = gradient;
        gradient = CalculateGradient(parameters);

        weighting_coefficient = CalculateWeightingCoefficient(gradient, previous_gradient);

        //TODO use vector math
        exit = 0;
        for (size_t i = 0; i<parameters.size(); i++)
        {
            exit += pow(gradient[i], 2);
        }
        exit = sqrt(exit);

        iteration++;
        if (iteration == number_of_iterations_)
        {
            std::cout << "Solution not found" << std::endl;
            break;
        }
    } while (exit >= precision_);

    return parameters;
}

double ConjugateGradientMethod::CountCorrectedObjectiveFunctionValue(
    const vector<double>& parameters)
{
    //Searching for minimum in the method
    return CountObjectiveFunctionValueToMinimize(parameters);
}

double ConjugateGradientMethod::CalculateDerivative(
    const vector<double>& parameters,
    size_t index)
{
    vector<double> decrement_function (parameters);
    vector<double> increment_function (parameters);
    decrement_function[index] = parameters[index] - increment_of_the_argument_/2;
    increment_function[index] = parameters[index] + increment_of_the_argument_/2;
    double increment_function_value = CountCorrectedObjectiveFunctionValue(increment_function);
    double decrement_function_value = CountCorrectedObjectiveFunctionValue(decrement_function);
    return (increment_function_value - decrement_function_value)/increment_of_the_argument_;
}

vector<double> ConjugateGradientMethod::FindNextSearchDirection(
    const vector<double>& gradient,
    const vector<double>& previous_search_direction,
    double weighting_coefficient,
    int iteration)
{
    vector<double> search_direction = gradient;

    if (iteration > 1)
    {
        //TODO use vector math
        for (size_t i=0; i<gradient.size(); i++)
        {
            search_direction[i] += weighting_coefficient * previous_search_direction[i];
        }
    }

    return search_direction;
}

vector<double> ConjugateGradientMethod::CalculateGradient(
    const vector<double>& parameters)
{
    vector<double> gradient(parameters.size());

    //TODO use vector math
    for (size_t i=0; i<parameters.size(); i++)
    {
        gradient[i] = CalculateDerivative(parameters, i);
    }

    return gradient;
}

double ConjugateGradientMethod::PerformOneDimensionalSearch(
    const vector<double>& parameters,
    const vector<double>& search_direction)
{
    double x = 0, previous_x=0;

    //Using Newton method to find one-dim minimum
    do
    {
        previous_x = x;

        //Counting derivatives
        //TODO use vector math
        vector<double> point = parameters;
        vector<double> incremented = parameters;
        vector<double> decremented = parameters;
        for(size_t i=0; i<parameters.size(); i++)
        {
            point[i] += x*search_direction[i];
            incremented[i] += (x + increment_of_the_argument_/2)*search_direction[i];
            decremented[i] += (x - increment_of_the_argument_/2)*search_direction[i];
        }

        double f = CountCorrectedObjectiveFunctionValue(point);
        double f_incremented = CountCorrectedObjectiveFunctionValue(incremented);
        double f_decremented = CountCorrectedObjectiveFunctionValue(decremented);

        double dirivative = (f_incremented - f_decremented)/increment_of_the_argument_;
        double secound_derivative = (f_incremented - 2*f + f_decremented) / (increment_of_the_argument_*increment_of_the_argument_/4);

        x = previous_x - dirivative/secound_derivative;
    } while(std::abs(previous_x - x) > precision_);
    return x;
}

double ConjugateGradientMethod::CalculateWeightingCoefficient(
    const vector<double>& gradient,
    const vector<double>& previous_gradient)
{
    //Fletcher-Reeves coefficient
    double gradient_square=0;
    double previous_gradient_square=0;

    //TODO replace pow 2
    //TODO use vector math
    for (size_t i=0; i<gradient.size(); i++)
    {
        gradient_square += pow(gradient[i], 2);
        previous_gradient_square += pow(previous_gradient[i], 2);
    }

    if(FloatingPoint::IsZero(previous_gradient_square))
    {
        throw std::runtime_error("previous_gradient_square == 0");
    }

    return gradient_square / previous_gradient_square;
}

}/* namespace optimization */
}/* namespace k52 */


#pragma once

////////////////////////////////////////////////////////////////////////////////
#include <polysolve/LinearSolver.hpp>

#include <nlohmann/json.hpp>
using json = nlohmann::json;

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

namespace polysolve
{

    ///
    /// @brief         { Solve a linear system Ax = b with Dirichlet boundary
    ///                conditions. For each Dirichlet node i, we want to ensure that
    ///                x[i] = b[i]. The implementation follows
    ///                http://www.math.colostate.edu/~bangerth/videos.676.21.65.html
    ///
    ///                For memory efficiency, this function updates in place the
    ///                matrix of the linear system. We also return the modified rhs
    ///                to the user }
    ///
    /// @param[in]     solver           { Linear solver class to use for solving the
    ///                                 system }
    /// @param[in,out] A                { Matrix of the linear system without
    ///                                 boundary conditions. Output: modified
    ///                                 matrix. }
    /// @param[in,out] b                { Right-hand side of the linear system.
    ///                                 Output: modified rhs for the equivalent
    ///                                 system. }
    /// @param[in]     dirichlet_nodes  { List of ids of Dirichlet nodes }
    /// @param[in,out] x                { Unknown vector }
    ///
    Eigen::Vector4d dirichlet_solve(LinearSolver &solver, StiffnessMatrix &A,
                                    Eigen::VectorXd &b, const std::vector<int> &dirichlet_nodes, Eigen::VectorXd &x,
                                    const int precond_num,
                                    const std::string &save_path = "", bool compute_spectrum = false,
                                    const bool remove_zero_cols = false,
                                    const bool skip_last_cols = false);

    void prefactorize(LinearSolver &solver, StiffnessMatrix &A,
                                 const std::vector<int> &dirichlet_nodes, const int precond_num,
                                 const std::string &save_path = "");

    void dirichlet_solve_prefactorized(LinearSolver &solver, const StiffnessMatrix &A, Eigen::VectorXd &f,
                                                  const std::vector<int> &dirichlet_nodes, Eigen::VectorXd &u);

} // namespace polysolve