/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/*
Solutions for testdata were generated with Scilab line:

M=fscanfMat('nsm1.example');e=spec(M);e=gsort(e);rr=real(e);ii=imag(e);e=cat(1, rr, ii); s=strcat(string(e), ' ');write('tmp', s);
*/

#ifndef NDEBUG
  #define NDEBUG
#endif

//#define VIENNACL_DEBUG_ALL
#include <iostream>
#include <fstream>
#include <stdexcept>
#include <vector>

#include "viennacl/linalg/prod.hpp"
#include "viennacl/linalg/qr-method.hpp"

#include <examples/benchmarks/benchmark-utils.hpp>

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

namespace ublas = boost::numeric::ublas;

typedef float ScalarType;

const ScalarType EPS = 0.0001f;

void read_matrix_size(std::fstream& f, std::size_t& sz)
{
    if(!f.is_open())
    {
        throw std::invalid_argument("File is not opened");
    }

    f >> sz;
}

template <typename MatrixLayout>
void read_matrix_body(std::fstream& f, viennacl::matrix<ScalarType, MatrixLayout>& A)
{
    if(!f.is_open())
    {
        throw std::invalid_argument("File is not opened");
    }

    boost::numeric::ublas::matrix<ScalarType> h_A(A.size1(), A.size2());

    for(std::size_t i = 0; i < h_A.size1(); i++) {
        for(std::size_t j = 0; j < h_A.size2(); j++) {
            ScalarType val = 0.0;
            f >> val;
            h_A(i, j) = val;
        }
    }

    viennacl::copy(h_A, A);
}

void read_vector_body(std::fstream& f, std::vector<ScalarType>& v) {
    if(!f.is_open())
        throw std::invalid_argument("File is not opened");

    for(std::size_t i = 0; i < v.size(); i++)
    {
            ScalarType val = 0.0;
            f >> val;
            v[i] = val;
    }
}

template <typename MatrixLayout>
bool check_tridiag(viennacl::matrix<ScalarType, MatrixLayout>& A_orig)
{
    ublas::matrix<ScalarType> A(A_orig.size1(), A_orig.size2());
    viennacl::copy(A_orig, A);

    for (unsigned int i = 0; i < A.size1(); i++) {
        for (unsigned int j = 0; j < A.size2(); j++) {
            if ((std::abs(A(i, j)) > EPS) && ((i - 1) != j) && (i != j) && ((i + 1) != j))
            {
                // std::cout << "Failed at " << i << " " << j << " " << A(i, j) << "\n";
                return false;
            }
        }
    }
    return true;
}

template <typename MatrixLayout>
bool check_hessenberg(viennacl::matrix<ScalarType, MatrixLayout>& A_orig)
{
    ublas::matrix<ScalarType> A(A_orig.size1(), A_orig.size2());
    viennacl::copy(A_orig, A);

    for (std::size_t i = 0; i < A.size1(); i++) {
        for (std::size_t j = 0; j < A.size2(); j++) {
            if ((std::abs(A(i, j)) > EPS) && (i > (j + 1)))
            {
                // std::cout << "Failed at " << i << " " << j << " " << A(i, j) << "\n";
                return false;
            }
        }
    }
    return true;
}

ScalarType matrix_compare(ublas::matrix<ScalarType>& res,
                            ublas::matrix<ScalarType>& ref)
{
    ScalarType diff = 0.0;
    ScalarType mx = 0.0;

    for(std::size_t i = 0; i < res.size1(); i++)
    {
        for(std::size_t j = 0; j < res.size2(); j++)
        {
            diff = std::max(diff, std::abs(res(i, j) - ref(i, j)));
            mx = std::max(mx, res(i, j));
        }
    }

    return diff / mx;
}

ScalarType vector_compare(std::vector<ScalarType> & res,
                          std::vector<ScalarType> & ref)
{
    std::sort(ref.begin(), ref.end());
    std::sort(res.begin(), res.end());

    ScalarType diff = 0.0;
    ScalarType mx = 0.0;
    for(size_t i = 0; i < res.size(); i++)
    {
        diff = std::max(diff, std::abs(res[i] - ref[i]));
        mx = std::max(mx, res[i]);
    }

    return diff / mx;
}

template <typename MatrixLayout>
void matrix_print(viennacl::matrix<ScalarType, MatrixLayout>& A)
{
    for (unsigned int i = 0; i < A.size1(); i++) {
        for (unsigned int j = 0; j < A.size2(); j++)
           std::cout << std::fixed << A(i, j) << "\t";
        std::cout << "\n";
    }
}

template <typename MatrixLayout>
void test_eigen(const std::string& fn, bool is_symm)
{
    std::cout << "Reading..." << "\n";
    std::size_t sz;
    // read file
    std::fstream f(fn.c_str(), std::fstream::in);
    //read size of input matrix
    read_matrix_size(f, sz);

    if (viennacl::is_row_major<MatrixLayout>::value)
      std::cout << "Testing row-major matrix of size " << sz << "-by-" << sz << std::endl;
    else
      std::cout << "Testing column-major matrix of size " << sz << "-by-" << sz << std::endl;

    viennacl::matrix<ScalarType> A_input(sz, sz), A_ref(sz, sz), Q(sz, sz);
    // reference vector with reference values from file
    std::vector<ScalarType> eigen_ref_re(sz);
    // calculated real eigenvalues
    std::vector<ScalarType> eigen_re(sz);
    // calculated im. eigenvalues
    std::vector<ScalarType> eigen_im(sz);

    // read input matrix from file
    read_matrix_body(f, A_input);
    // read reference eigenvalues from file
    read_vector_body(f, eigen_ref_re);

    f.close();

    A_ref = A_input;

    std::cout << "Calculation..." << "\n";

    Timer timer;
    timer.start();
    // Start the calculation
    if(is_symm)
        viennacl::linalg::qr_method_sym(A_input, Q, eigen_re);
    else
        viennacl::linalg::qr_method_nsm(A_input, Q, eigen_re, eigen_im);
/*

    std::cout << "\n\n Matrix A: \n\n";
    matrix_print(A_input);
    std::cout << "\n\n";

    std::cout << "\n\n Matrix Q: \n\n";
    matrix_print(Q);
    std::cout << "\n\n";
*/

    double time_spend = timer.get();

    std::cout << "Verification..." << "\n";

    bool is_hessenberg = check_hessenberg(A_input);
    bool is_tridiag = check_tridiag(A_input);

    ublas::matrix<ScalarType> A_ref_ublas(sz, sz), A_input_ublas(sz, sz), Q_ublas(sz, sz), result1(sz, sz), result2(sz, sz);
    viennacl::copy(A_ref, A_ref_ublas);
    viennacl::copy(A_input, A_input_ublas);
    viennacl::copy(Q, Q_ublas);

    // compute result1 = ublas::prod(Q_ublas, A_input_ublas);   (terribly slow when using ublas directly)
    for (std::size_t i=0; i<result1.size1(); ++i)
      for (std::size_t j=0; j<result1.size2(); ++j)
      {
        ScalarType value = 0;
        for (std::size_t k=0; k<Q_ublas.size2(); ++k)
          value += Q_ublas(i, k) * A_input_ublas(k, j);
        result1(i,j) = value;
      }
    // compute result2 = ublas::prod(A_ref_ublas, Q_ublas);   (terribly slow when using ublas directly)
    for (std::size_t i=0; i<result2.size1(); ++i)
      for (std::size_t j=0; j<result2.size2(); ++j)
      {
        ScalarType value = 0;
        for (std::size_t k=0; k<A_ref_ublas.size2(); ++k)
          value += A_ref_ublas(i, k) * Q_ublas(k, j);
        result2(i,j) = value;
      }

    ScalarType prods_diff = matrix_compare(result1, result2);
    ScalarType eigen_diff = vector_compare(eigen_re, eigen_ref_re);

    bool is_ok = is_hessenberg;

    if(is_symm)
        is_ok = is_ok && is_tridiag;

    is_ok = is_ok && (eigen_diff < EPS);
    is_ok = is_ok && (prods_diff < EPS);

    // std::cout << A_ref << "\n";
    // std::cout << A_input << "\n";
    // std::cout << Q << "\n";
    // std::cout << eigen_re << "\n";
    // std::cout << eigen_im << "\n";
    // std::cout << eigen_ref_re << "\n";
    // std::cout << eigen_ref_im << "\n";

    // std::cout << result1 << "\n";
    // std::cout << result2 << "\n";
    // std::cout << eigen_ref << "\n";
    // std::cout << eigen << "\n";

    printf("%6s [%dx%d] %40s time = %.4f\n", is_ok?"[[OK]]":"[FAIL]", (int)A_ref.size1(), (int)A_ref.size2(), fn.c_str(), time_spend);
    printf("tridiagonal = %d, hessenberg = %d prod-diff = %f eigen-diff = %f\n", is_tridiag, is_hessenberg, prods_diff, eigen_diff);
    std::cout << std::endl << std::endl;

    if (!is_ok)
      exit(EXIT_FAILURE);

}

int main()
{

  test_eigen<viennacl::row_major>("../../examples/testdata/eigen/symm5.example", true);
 // test_eigen<viennacl::row_major>("../../examples/testdata/eigen/symm3.example", true);  // Computation of this matrix takes very long

  test_eigen<viennacl::column_major>("../../examples/testdata/eigen/symm5.example", true);
//  test_eigen<viennacl::column_major>("../../examples/testdata/eigen/symm3.example", true);

#ifdef VIENNACL_WITH_OPENCL
  test_eigen<viennacl::row_major>("../../examples/testdata/eigen/nsm2.example", false);
#endif
  //test_eigen<viennacl::row_major>("../../examples/testdata/eigen/nsm2.example", false);
  //test_eigen("../../examples/testdata/eigen/nsm3.example", false);
  //test_eigen("../../examples/testdata/eigen/nsm4.example", false); //Note: This test suffers from round-off errors in single precision, hence disabled

  std::cout << std::endl;
  std::cout << "------- Test completed --------" << std::endl;
  std::cout << std::endl;

  return EXIT_SUCCESS;
}


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

#ifndef ITL_PC_DIAGONAL_INCLUDE
#define ITL_PC_DIAGONAL_INCLUDE

#include <boost/numeric/linear_algebra/inverse.hpp>

#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/operation/resource.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>
#include <boost/numeric/itl/pc/solver.hpp>

namespace itl { namespace pc {

/// Diagonal Preconditioner
template <typename Matrix, typename Value= typename mtl::Collection<Matrix>::value_type>
class diagonal
{
  public:
    typedef Value                                         value_type;
    typedef typename mtl::Collection<Matrix>::size_type   size_type;
    typedef diagonal                                      self;

    /// Constructor takes matrix reference
    explicit diagonal(const Matrix& A) : inv_diag(num_rows(A))
    {
	mtl::vampir_trace<5050> tracer;
	MTL_THROW_IF(num_rows(A) != num_cols(A), mtl::matrix_not_square());
	using math::reciprocal;

	for (size_type i= 0; i < num_rows(A); ++i)
	    inv_diag[i]= reciprocal(A[i][i]);
    }

    /// Member function solve, better use free function solve
    template <typename Vector>
    Vector solve(const Vector& x) const
    {
	Vector y(resource(x));
	solve(x, y);
	return y;
    }

    template <typename VectorIn, typename VectorOut>
    void solve(const VectorIn& x, VectorOut& y) const
    {
	mtl::vampir_trace<5051> tracer;
	y.checked_change_resource(x);
	MTL_THROW_IF(size(x) != size(inv_diag), mtl::incompatible_size());
	for (size_type i= 0; i < size(inv_diag); ++i)
	    y[i]= inv_diag[i] * x[i];
    }

    /// Member function for solving adjoint problem, better use free function adjoint_solve
    template <typename Vector>
    Vector adjoint_solve(const Vector& x) const
    {
	Vector y(resource(x));
	adjoint_solve(x, y);
	return y;
    }

    template <typename VectorIn, typename VectorOut>
    void adjoint_solve(const VectorIn& x, VectorOut& y) const
    {
	using mtl::conj;
	y.checked_change_resource(x);
	MTL_THROW_IF(size(x) != size(inv_diag), mtl::incompatible_size());
	for (size_type i= 0; i < size(inv_diag); ++i)
	    y[i]= conj(inv_diag[i]) * x[i];
    }

 protected:
    mtl::dense_vector<value_type>    inv_diag;
};

/// Solve approximately a sparse system in terms of inverse diagonal
template <typename Matrix, typename Vector>
solver<diagonal<Matrix>, Vector, false>
inline solve(const diagonal<Matrix>& P, const Vector& x)
{
    return solver<diagonal<Matrix>, Vector, false>(P, x);
}

/// Solve approximately the adjoint of a sparse system in terms of inverse diagonal
template <typename Matrix, typename Vector>
solver<diagonal<Matrix>, Vector, true>
inline adjoint_solve(const diagonal<Matrix>& P, const Vector& x)
{
    return solver<diagonal<Matrix>, Vector, true>(P, x);
}

}} // namespace itl::pc

#endif // ITL_PC_DIAGONAL_INCLUDE


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_FUNCTION_ATAN2D_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ATAN2D_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-trigonometric
    Function object implementing atan2d capabilities

    atan2d function : atan2 in degrees.

    @par Semantic:

    For every parameters of floating type T:

    @code
    T r = atan2d(x, y);
    @endcode

    is similar but not fully equivalent to:

    @code
    T r =  atand(y/x);;
    @endcode

    as it is quadrant aware.

    For any real arguments @c x and @c y not both equal to zero, <tt>atan2d(x, y)</tt>
    is the angle in degrees between the positive x-axis of a plane and the point
    given by the coordinates  <tt>(y, x)</tt>.

    It is also the angle in \f$[-180,180[\f$ for which
    \f$x/\sqrt{x^2+y^2}\f$ and \f$y/\sqrt{x^2+y^2}\f$
    are respectively the sine and the cosine.

    @see atand, atan2, atan

  **/
  const boost::dispatch::functor<tag::atan2d_> atan2d = {};
} }
#endif

#include <boost/simd/function/scalar/atan2d.hpp>
#include <boost/simd/function/simd/atan2d.hpp>

#endif


#ifndef __PROBABILITY_DISTRIBUTIONS__ASYMMETRIC_DISTRIBUTION_IMPL_HPP__
#define __PROBABILITY_DISTRIBUTIONS__ASYMMETRIC_DISTRIBUTION_IMPL_HPP__

#include "asymmetric_distribution.hpp"

#include "const_slice.hpp"
#include "slice.hpp"

#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/exponential_distribution.hpp>
#include <cmath>

namespace ProbabilityDistributions {
  template <class Dist, class D, class W, class T>
  AsymmetricDistribution<Dist,D,W,T>::AsymmetricDistribution(T p, T mu, T eps,
      T tol):
    fixed_mu_(false),
    fixed_p_(false),
    mu_(mu),
    eps_(eps),
    tol_(tol) {
      set_mu(mu);
      set_p(p);
    }

  template <class Dist, class D, class W, class T>
  void AsymmetricDistribution<Dist,D,W,T>::init() {
    set_p(p_);
  }

  template <class Dist, class D, class W, class T>
  void AsymmetricDistribution<Dist,D,W,T>::set_p(T p) {
    assert(p > 0);
    assert(p < 1);
    p_ = p;
    static_cast<Dist*>(this)->updated_p();
  }

  template <class Dist, class D, class W, class T>
  template <class RNG>
  void AsymmetricDistribution<Dist,D,W,T>::sample(MA::Array<D>& samples,
      size_t n_samples, RNG& rng) const {
    MA::Size::SizeType size(2);
    size[0] = n_samples;
    size[1] = 1;
    samples.resize(size);

    boost::random::uniform_real_distribution<T> dist(0, 1);
    auto gamma_plus = static_cast<Dist const*>(this)->create_gamma_plus();
    auto gamma_minus = static_cast<Dist const*>(this)->create_gamma_minus();

    D* ptr = samples.get_pointer();

    for (size_t j = 0; j < n_samples; j++) {
      if (dist(rng) < p_)
        ptr[j] = mu_ - gamma_minus(rng);
      else
        ptr[j] = mu_ + gamma_plus(rng);
    }
  }

  template <class Dist, class D, class W, class T>
  T AsymmetricDistribution<Dist,D,W,T>::log_likelihood(
      MA::ConstArray<D> const& data, MA::ConstArray<W> const& weight) const {
    check_data_and_weight(data, weight);

    D const* ptr = data.get_pointer();

    T ll = 0;
    T const_likelihood = static_cast<Dist const*>(this)->constant_likelihood() +
      (std::log(p_) + std::log(1-p_))/2;

    for (size_t j = 0; j < data.total_size(); j++) {
      T w = weight(j);
      T s = ptr[j];
      T local_likelihood = const_likelihood;
      if (s < mu_)
        local_likelihood += static_cast<Dist const*>(this)->negative_ll(s, mu_);
      else
        local_likelihood += static_cast<Dist const*>(this)->positive_ll(s, mu_);
      ll += w * local_likelihood;
    }

    return ll;
  }

  template <class Dist, class D, class W, class T>
  void AsymmetricDistribution<Dist,D,W,T>::MLE(MA::ConstArray<D> const& data,
      MA::ConstArray<W> const& weight, std::vector<size_t> const& indexes) {
    check_data_and_weight(data, weight);
    assert(data.size()[0] == indexes.size());

    static_cast<Dist*>(this)->init_MLE(data, weight, indexes);

    if (fixed_p_)
      static_cast<Dist*>(this)->MLE_fixed_p(data, weight, indexes);
    else {
      T step  = eps_;

      T center_p = p_, left_p = center_p - step, right_p = center_p + step;

      set_p(center_p);
      static_cast<Dist*>(this)->MLE_fixed_p(data, weight, indexes);
      T center_ll = log_likelihood(data, weight), left_ll, right_ll;

      if (left_p > 0) {
        set_p(left_p);
        static_cast<Dist*>(this)->MLE_fixed_p(data, weight, indexes);
        left_ll = log_likelihood(data, weight);
      }
      else
        left_ll = -INFINITY;

      if (right_p < 1) {
        set_p(right_p);
        static_cast<Dist*>(this)->MLE_fixed_p(data, weight, indexes);
        right_ll = log_likelihood(data, weight);
      }
        right_ll = -INFINITY;

      while (1) {
        if (std::abs(center_ll - left_ll)  < tol_ &&
            std::abs(right_ll  - left_ll)  < tol_ &&
            std::abs(center_ll - right_ll) < tol_)
          break;

        if (center_ll > left_ll && center_ll > right_ll) {
          if (step < tol_) {
            set_p(center_p);
            break;
          }

          step *= 1e-1;
          left_p = center_p - step;
          right_p = center_p + step;

          if (left_p > 0) {
            set_p(left_p);
            static_cast<Dist*>(this)->MLE_fixed_p(data, weight, indexes);
            left_ll = log_likelihood(data, weight);
          }
          else
            left_ll = -INFINITY;

          if (right_p < 1) {
            set_p(right_p);
            static_cast<Dist*>(this)->MLE_fixed_p(data, weight, indexes);
            right_ll = log_likelihood(data, weight);
          }
          else
            right_ll = -INFINITY;
        }
        else if (left_ll > right_ll) {
          right_p = center_p;
          center_p = left_p;
          right_ll = center_ll;
          center_ll = left_ll;

          left_p = center_p - step;

          if (left_p > 0) {
            set_p(left_p);
            static_cast<Dist*>(this)->MLE_fixed_p(data, weight, indexes);
            left_ll = log_likelihood(data, weight);
          }
          else
            left_ll = -INFINITY;
        }
        else {
          left_p = center_p;
          center_p = right_p;
          left_ll = center_ll;
          center_ll = right_ll;

          right_p = center_p + step;

          if (right_p < 1) {
            set_p(right_p);
            static_cast<Dist*>(this)->MLE_fixed_p(data, weight, indexes);
            right_ll = log_likelihood(data, weight);
          }
          else
            right_ll = -INFINITY;
        }
      }

      static_cast<Dist*>(this)->MLE_fixed_p(data, weight, indexes);
    }

    static_cast<Dist*>(this)->end_MLE();
  }

  template <class Dist, class D, class W, class T>
  T AsymmetricDistribution<Dist,D,W,T>::fix_step(T p, T step) const {
    while (p_ + step <= 0 || p_ + step >= 1)
      step *= 0.99;
    return step;
  }

  template <class Dist, class D, class W, class T>
  void AsymmetricDistribution<Dist,D,W,T>::check_data_and_weight(
      MA::ConstArray<D> const& data, MA::ConstArray<W> const& weight) const {
    assert(data.size().size() == 2);
    assert(data.size()[0] > 0);
    assert(data.size()[1] == 1);
    assert(weight.size().size() == 1);
    assert(weight.size()[0] == data.size()[0]);
  }
};

#endif


#include <iostream>
#include <vector>
#include <unordered_map>
#include <string>
#include <Eigen/Dense>
#include "../include/sample_network.h"

using namespace Eigen;

int main()
{
    using namespace MyDL;
    using std::cout;
    using std::endl;
    using std::vector;
    using std::string;
    using std::unordered_map;

    int batch_size = 4;
    int input_size = 2;
    vector<int> hidden_size = {3};
    int output_size = 2;
    double lambda = 2.0;

    MultiLayerNet net(input_size, hidden_size, output_size, lambda);

    cout << "Check Initialized Parameters" << endl;
    for (auto param : net.params)
    {
        cout << param.first << endl;
        cout << *(param.second) << endl;
    }

    cout << "Test Predict Method" << endl;
    vector<MatrixXd> inputs, outputs;
    MatrixXd X = MatrixXd::Random(batch_size, input_size);
    inputs.push_back(X);

    outputs = net.predict(inputs);

    cout << outputs[0] << endl;

    cout << "Test Loss Method" << endl;
    vector<MatrixXd> loss_output;
    MatrixXd t = MatrixXd::Zero(batch_size, output_size);

    t << 1, 0,
         1, 0,
         1, 0,
         0, 1;

    loss_output = net.loss(inputs, t);

    cout << loss_output[0] << endl;

    cout << "Test Accuracy Method" << endl;
    double accuracy;

    accuracy = net.accuracy(inputs, t);

    cout << accuracy << endl;

    cout << "Test Gradient Method" << endl;

    unordered_map<string, MatrixXd> grads;

    grads = net.gradient(inputs, t);

    for (auto grad : grads)
    {
        cout << grad.first << endl;
        cout << grad.second << endl;
    }

    cout << "Test Finished" << endl;

    return 0;
}


#include "alpha_shape_area.hpp"
#include <boost/lexical_cast.hpp>
#include <iostream>
#include <fstream>

#include "weight_data.hpp"
#include "weight_func_obj_min.hpp"
#include "create_alpha_shape.hpp"
#include "alpha_shape_weighted_area.hpp"

typedef float f_t;

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

	feenableexcept(FE_INVALID | FE_OVERFLOW);

	if(argc != 3){
		std::cout << "usage: " << argv[0] << " <alpha> <coord/radii file>" << std::endl;
		return 1;
	}
	f_t alpha = boost::lexical_cast<f_t>(argv[1]);
	const char* infilename = argv[2];

	std::vector<f_t> points, radii;
	f_t x, y, z, r;
	std::ifstream fs;
	fs.open(infilename, std::ifstream::in);
	while(fs >> x >> y >> z >> r){
		points.push_back(x);
		points.push_back(y);
		points.push_back(z);
		radii.push_back(r);
	}
	size_t numPoints = radii.size();
	std::cout << "number of points: " << numPoints << std::endl;

	std::vector<f_t> areas(numPoints);

	if(0){
		busv::alpha_shape_area<f_t, unsigned int>(numPoints, alpha, &(points[0]), &(radii[0]), &(areas[0]));
	}
	else{
		std::vector<busv::weight_data<f_t, size_t> > wd(numPoints);
		busv::weight_func_obj_min<f_t, size_t> wfo(&(wd[0]), 0);
		busv::AlphaShapeContainer<f_t, size_t> asc = busv::create_alpha_shape(numPoints, alpha,  &(points[0]), &(radii[0]));
		busv::alpha_shape_weighted_area<f_t, size_t>(asc, wfo, &(areas[0]));
	}

	for(unsigned int i = 0; i<numPoints; ++i){
		std::cout << i << "\t" << areas[i] << std::endl;
	}
	std::cout << std::endl;

	return 0;
}


// Copyright (c) 2011 libmv authors.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to
// deal in the Software without restriction, including without limitation the
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
// sell copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.

#include <Eigen/Geometry>

#include "libmv/multiview/affine.h"
#include "libmv/multiview/similarity.h"
#include "libmv/multiview/similarity_parameterization.h"

namespace libmv {

// Parametrization
// s*cos -s*sin  tx
// s*sin  s*cos  ty
// 0      0      1

// It gives the following system A x = B :
// |-Y1  Y1 1 0 | | s*sin |   | X2 |
// | X1  Y1 0 1 | | s*cos |   | Y2 |
//                | tx    | =
//                | ty    |
//
bool Similarity2DFromCorrespondencesLinear(const Mat &x1, const Mat &x2,
                                           Mat3 *M,
                                           double expected_precision) {
  assert(2 == x1.rows());
  assert(2 <= x1.cols());
  assert(x1.rows() == x2.rows());
  assert(x1.cols() == x2.cols());

  const int n = x1.cols();
  Mat A = Mat::Zero(2*n, 4);
  Mat b = Mat::Zero(2*n, 1);
  for (int i = 0; i < n; ++i) {
    const int j= i * 2;
    A(j,0) = -x1(1,i);
    A(j,1) =  x1(0,i);
    A(j,2) =  1.0;
    //A(j,3) =  0.0;

    A(j+1,0) = x1(0,i);
    A(j+1,1) = x1(1,i);
    //A(j+1,2) = 0.0;
    A(j+1,3) = 1.0;

    b(j,0)   = x2(0,i);
    b(j+1,0) = x2(1,i);
  }
  // Solve Ax=B
  Vec x = A.fullPivLu().solve(b);
  if ((A * x).isApprox(b, expected_precision))  {
    Similarity2DSCParameterization<double>::To(x, M);
    return true;
  } else {
    return false;
  }
}

bool Similarity3DFromCorrespondencesLinear(const Mat &x1,
                                          const Mat &x2,
                                          Mat4 *H,
                                          double expected_precision) {
   // TODO(julien) Compare to *H = umeyama (x1, x2, true);
   // and keep the best one (quality&speed)
  if (Affine3DFromCorrespondencesLinear(x1, x2, H, expected_precision)) {
    // Ensures that R is orthogonal (using SDV decomposition)
    Eigen::JacobiSVD<Mat> svd(H->block<3,3>(0, 0), Eigen::ComputeThinU |
                                                   Eigen::ComputeThinV);
    double scale = svd.singularValues()(0);
    Mat3 sI3 = scale * Mat3::Identity();
    H->block<3,3>(0, 0) = svd.matrixU() * sI3 * svd.matrixV().transpose();
    if (H->block<3,3>(0, 0).determinant() < 0)
      H->block<3,3>(0, 0) = -H->block<3,3>(0, 0);
    return true;
  }
  return false;
}

bool ExtractSimilarity2DCoefficients(const Mat3 &M,
                                     Vec2   *tr,
                                     double *angle,
                                     double *scale) {
  Vec4 p;
  Similarity2DSAParameterization<double>::From(M, &p);
  *scale = p(0);
  *angle = p(1);
  *tr << p(2), p(3);
  return true;
}
} // namespace libmv


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

namespace bs = boost::simd;

template <typename T, std::size_t N, typename Env>
void test(Env& $)
{
  using p_t = bs::pack<T, N>;

  T a1[N], c[N], s[N];
  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(i)/2 : -T(i)/2;
    std::tie(s[i], c[i])= bs::sinhcosh(a1[i]) ;
  }

  p_t aa1(&a1[0], &a1[0]+N);
  p_t ss (&s[0], &s[0]+N);
  p_t cc (&c[0], &c[0]+N);
  p_t ss1, cc1;
  std::tie(ss1, cc1)= bs::sinhcosh(aa1) ;
  STF_ULP_EQUAL(ss1, ss, 0.5);
  STF_ULP_EQUAL(cc1, cc, 0.5);
}

STF_CASE_TPL("Check sincosh on pack" , STF_IEEE_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;

  test<T, N>($);
  test<T, N/2>($);
  test<T, N*2>($);
}

STF_CASE_TPL("sinhcosh", STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using p_t = bs::pack<T>;
  using bs::sinhcosh;

  p_t a[] = {bs::Zero<p_t>(), bs::One<p_t>(), p_t(5), p_t(-5)};
  size_t N =  sizeof(a)/sizeof(p_t);
  STF_EXPR_IS( (sinhcosh(p_t()))
                  , (std::pair<p_t,p_t>)
                  );

  {
    for(size_t i=0; i < N; ++i)
    {
      std::pair<p_t,p_t> p = sinhcosh(a[i]);
      STF_ULP_EQUAL(p.first,  bs::sinh(a[i]), 1);
      STF_ULP_EQUAL(p.second, bs::cosh(a[i]), 1);
    }
  }

#ifndef BOOST_SIMD_NO_INVALIDS
  p_t b[] = {bs::Inf<p_t>(), bs::Minf<p_t>(), bs::Nan<p_t>()};
  N =  sizeof(b)/sizeof(p_t);

  {
    for(size_t i=0; i < N; ++i)
    {
      std::pair<p_t,p_t> p = sinhcosh(b[i]);
      STF_ULP_EQUAL(p.first,  bs::sinh(b[i]), 1);
      STF_ULP_EQUAL(p.second, bs::cosh(b[i]), 1);
    }
  }
#endif
}


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

TEST(MathFunctions, rising_factorial) {
  using stan::math::rising_factorial;

  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_FLOAT_EQ(120, rising_factorial(4.0, 3));
  EXPECT_FLOAT_EQ(360, rising_factorial(3.0, 4));
  EXPECT_THROW(rising_factorial(1, -4), std::domain_error);
  EXPECT_THROW(rising_factorial(nan, 1), std::domain_error);
}


/**
 * See https://gamedevelopment.tutsplus.com/tutorials/collision-detection-using-the-separating-axis-theorem--gamedev-169
 */

#include <cassert>
#include <limits>
#include <algorithm>
#include <set>

#include <boost/variant/static_visitor.hpp>

#include <common/Geometry.h>
#include <common/geometry/Circle.h>

#include <common/geometry/collision.h>

using namespace std;

namespace geometry {

bool collision(Circle const &c1, Circle const &c2) {
	return distance(c1.center, c2.center) <= (c1.radius + c2.radius);
}

MinMax minmaxProjection(Polygon const &polygon, Vector axis) {
	MinMax minmax = {numeric_limits<Scalar>::max(), numeric_limits<Scalar>::lowest()};
	for(Point const & point : polygon) {
		auto d = (point - Point{0,0}) * axis;
		minmax.min = min(minmax.min, d);
		minmax.max = max(minmax.max, d);
	}
	return minmax;
}

bool gapAlongAxis(Vector axis, Polygon const &p1, Polygon const &p2) {
	MinMax r1 = minmaxProjection(p1, axis);
	MinMax r2 = minmaxProjection(p2, axis);

	return (r1.max < r2.min) || (r2.max < r1.min);
}

void getNormals(std::set<Vector> &normals, Polygon const &polygon) {
	if (polygon.empty())
		return;

	Point prev = polygon.back();
	for(auto const & curr : polygon) {
		normals.insert(leftNormal(unit(curr - prev)));
		prev = curr;
	}
}

bool collision(Polygon const &p1, Polygon const &p2) {
	std::set<Vector> normals;
	getNormals(normals, p1);
	getNormals(normals, p2);

	for (auto const &n : normals) {
		if (gapAlongAxis(n, p1, p2))
			return false;
	}

	return true;
}

// TODO na zacatek nekam pridam kruh

// Does not detect full containing
bool collision(Polygon const &polygon, Circle const &circle) {

	// Near vertex
	for(Point const & vertex : polygon) {
		if (distance(vertex, circle.center) <= circle.radius)
			return true;
	}

	// Something is wrong down there
	assert(polygon.size() >= 3);
	Point const * lineFrom = &polygon.back();
	for (Point const & lineTo : polygon ) {

		Scalar const t = projection(circle.center, *lineFrom, lineTo);
		if (t >= 0 && t <= 1) {
			Point const projectedCenter = *lineFrom + t*(lineTo - *lineFrom);
			if (size(projectedCenter - circle.center) <= circle.radius)
					return true;
		}
		lineFrom = &lineTo;
	}

	return false;
}

bool collision(Circle const &circle, Polygon const &polygon) {
	return collision(polygon, circle);
}

namespace {

template < typename Object1 >
class CollisionVisitor1 : public boost::static_visitor<bool>{
	Object1 const & object1;
public:
	explicit CollisionVisitor1(Object1 const & object1) :
		object1(object1) {
	}

	template < typename Object2 >
	bool operator()(Object2 const & object2) {
		return collision(object1, object2);
	}
};

}

bool collision(Polygon const &polygon, Object2D const &object) {
	CollisionVisitor1<Polygon> visitor{polygon};
	return boost::apply_visitor(visitor, object);
}

bool collision(Circle const &circle, Object2D const &object) {
	CollisionVisitor1<Circle> visitor{circle};
	return boost::apply_visitor(visitor, object);
}

namespace {
class CollisionVisitor2 : public boost::static_visitor<bool>{
	Object2D const & object1;
public:
	explicit CollisionVisitor2(Object2D const & object1) :
		object1(object1) {
	}

	template < typename Object2 >
	bool operator()(Object2 const & object2) {
		CollisionVisitor1<Object2> visitor{object2};
		return boost::apply_visitor(visitor, object1);
	}
};
}

bool collision(Object2D const &object1, Object2D const &object2) {
	CollisionVisitor2 visitor{object1};
	return boost::apply_visitor(visitor, object2);
}

} // namespace geometry


#include "CEGO/CEGO.hpp"
#include <Eigen/Dense>
#include <atomic>

std::atomic_size_t Ncalls(0);

using CEGO::EArray;

/**
# Sadus, https://doi.org/10.1063/1.5041320, erratum: missing exponent of m
n is the repulsive exponent (the 12 of 12-6 LJ)
m is the attractive exponent (the 6 of 12-6 LJ)
*/
template <typename T>
auto B2_LennardJones(T Tstar, double n, double m){
    auto F = [&](auto y) {
        auto the_sum = 0.0;
        for (auto i = 0; i < 200; ++i) {
            auto my_factorial = [](auto k) { return tgamma(k + 1); };
            the_sum += tgamma((i * m - 3.0) / n) / my_factorial(i) * pow(y, i);
        }
        return pow(y, 3.0/(n - m)) * (tgamma((n - 3.0) / n) - 3.0/n*the_sum);
    };
    auto yn = pow(n/(n-m), n)*pow((n-m)/m,m)*pow(Tstar, -(n-m)); // y**n, Eq. 9
    auto y = pow(yn, 1.0/n);
    return 2*EIGEN_PI/3*F(y);
}

class FitClass {
public:
    EArray<double> m_T, B2, m_LHS, m_x;
    FitClass(const std::size_t Npts)
    {
        double Tmin = 0.1, Tmax = 10000;
        m_T = EArray<double>::LinSpaced(Npts, log10(Tmin), log10(Tmax)).exp();
        B2.resize(m_T.size());
        for (auto i = 0; i < m_T.size(); ++i) {
            B2(i) = B2_LennardJones(m_T(i), 12, 6);
        }
        // Set variables for fitting
        m_LHS = B2;
        m_x = 1/m_T;
    }
    template <typename TYPE>
    auto eval_RHS(const EArray<double>& x, const EArray<TYPE> &c)
    {
        std::decay_t<decltype(x)> val(x.size()); val.setZero();
        for (auto i = 0; i < c.size(); i += 2) {
            double a = c[i];
            double e = c[i + 1];
            val += a*x.pow(e);
        }
        return val.eval();
    }
    template <typename TYPE> TYPE objective(const EArray<TYPE>& c) {
        return ((eval_RHS(m_x, c) - m_LHS)).square().sum();
    }
    template <typename TYPE> EArray<TYPE> abs_rel_deviations(const EArray<TYPE>& c) {
        return (eval_RHS(m_x, c) - m_LHS).eval();
    }
    double objective(const CEGO::AbstractIndividual *pind) {
        const auto &c = dynamic_cast<const CEGO::NumericalIndividual<CEGO::numberish>*>(pind)->get_coeff_array<CEGO::numberish>();
        return objective(c);
    }
};

int do_one()
{
    //std::srand((unsigned int)time(0));
    std::size_t Nterms = 3;

    //EArray<CEGO::numberish> c1(3); c1 << 1.0, 2, 3;
    //EArray<CEGO::numberish> c2(3); c2 << 2.0, 3, 4;
    //EArray<CEGO::numberish> c3(3); c3 << 3.0, 4, 5;
    //auto oo = (c1 - c2).eval();
    //auto o2 = (c3 - c2).eval();
    //auto o3 = (o2*0.7).eval();

    // Construct the bounds
    std::vector<CEGO::Bound> bounds;
    for (auto i = 0; i < Nterms; ++i) {
        bounds.push_back(CEGO::Bound(std::make_pair(-1000.0, 1000.0)));
        bounds.push_back(CEGO::Bound(std::make_pair(0.0, 10.0)));
    }
    std::size_t Npts = 100;
    FitClass rp(Npts);

    auto Ncalls = 0;
    CEGO::CostFunction<CEGO::numberish> cost_wrapper = [&rp](const CEGO::AbstractIndividual*pind) {return rp.objective(pind); };
    auto Ntotal_individuals = 1000;
    auto Nlayers = 7;
    auto layers = CEGO::Layers<CEGO::numberish>(cost_wrapper, bounds.size(), Ntotal_individuals/Nlayers, Nlayers, 3);
    layers.parallel = false;
    layers.parallel_threads = 6;
    layers.set_bounds(bounds);
    layers.set_generation_mode(CEGO::GenerationOptions::LHS);
    layers.set_builtin_evolver(CEGO::BuiltinEvolvers::differential_evolution_best1bin);
    auto f = [&rp](const CEGO::EArray<double>& c) {return rp.objective<double>(c); };
    //auto f2 = [&rp](const CEGO::EArray<std::complex< double >>& c) {return rp.objective<std::complex<double>>(c); };
    //layers.add_gradient(f, f2);

    auto flags = layers.get_evolver_flags();
    flags["Nelite"] = 1;
    flags["Fmin"] = 0.1;
    flags["Fmax"] = 1.1;
    flags["CR"] = 0.9;
    layers.set_evolver_flags(flags);

    std::vector<double> best_costs;
    const double VTR = 2e-4;
    auto startTime = std::chrono::system_clock::now();
    bool success = false;
    for (auto counter = 0; counter < 15000; ++counter) {
        layers.do_generation();

        /*if (counter % 1000 == 0) {
            layers.gradient_minimizer();
        }*/

        auto [best_cost, best_coeffs] = layers.get_best();
        if (counter % 50 == 0) {
            std::cout << counter << ": best: " << best_cost << std::endl;
            //std::cout << counter << ": best coeffs: " << c << "||" << std::endl;
            //std::cout << counter << ": obj again: " << rp.objective(c) << "||" << std::endl;
        }
        if (best_cost < VTR) { success = true;  break; }
    }
    auto best_layer = layers.get_best();
    auto best_coeffs = std::get<1>(best_layer);
    //std::cout << rp.abs_rel_deviations(best_coeffs) << std::endl;
    auto endTime = std::chrono::system_clock::now();
    double elap = std::chrono::duration<double>(endTime - startTime).count();
    std::cout << "run:" << elap << " s\n";
    std::cout << "NFE:" << Ncalls << std::endl;
    return success;
}

int main() {
    int N = (CEGO::is_CI() ? 3 : 100);
    int good = 0;
    for (auto i = 0; i < N; ++i) {
        good += do_one();
    }
    std::cout << "success:" << good << "/" << N << std::endl;
}


#include "discretization.hpp"

#include <boost/numeric/odeint.hpp>

#include "eigenIntegration.hpp"
#include <eigen3/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h>

void eulerLinearDiscretization(Model &model,
                               double ts,
                               const Model::state_vector_t &x_eq,
                               const Model::input_vector_t &u_eq,
                               Model::state_matrix_t &A,
                               Model::control_matrix_t &B)
{
    Model::state_matrix_t A_c;
    Model::control_matrix_t B_c;
    model.computeJacobians(x_eq, u_eq, A_c, B_c);

    A = Model::state_matrix_t::Identity() + ts * A_c;
    B = ts * B_c;
}

void exactLinearDiscretization(Model &model,
                               double ts,
                               const Model::state_vector_t &x_eq,
                               const Model::input_vector_t &u_eq,
                               Model::state_matrix_t &A,
                               Model::control_matrix_t &B,
                               Model::state_vector_t &z)
{
    Model::state_matrix_t A_c;
    Model::control_matrix_t B_c;
    Model::state_vector_t f;
    model.computeJacobians(x_eq, u_eq, A_c, B_c);
    model.computef(x_eq, u_eq, f);

    Eigen::MatrixXd E;
    E.resize(Model::state_dim + Model::input_dim, Model::state_dim + Model::input_dim);
    E.setZero();
    E.topLeftCorner(Model::state_dim, Model::state_dim) << A_c;
    E.topRightCorner(Model::state_dim, Model::input_dim) << B_c;
    Eigen::MatrixXd expE = (E * ts).exp();

    A = expE.topLeftCorner(Model::state_dim, Model::state_dim);
    B = expE.topRightCorner(Model::state_dim, Model::input_dim);
    z = f - A * x_eq - B * u_eq;
}

class ODEMultipleShootingVariableTime
{
private:
    Model::input_vector_t u_t0, u_t1;
    double T, dt;
    Model &model;

public:
    using ode_matrix_t = Eigen::Matrix<double, Model::state_dim, 1 + Model::state_dim + 2 * Model::input_dim + 2>;

    ODEMultipleShootingVariableTime(
        const Model::input_vector_t &u_t0,
        const Model::input_vector_t &u_t1,
        const double &T,
        double dt,
        Model &model)
        : u_t0(u_t0), u_t1(u_t1), T(T), dt(dt), model(model) {}

    void operator()(const ode_matrix_t &V, ode_matrix_t &dVdt, const double t)
    {
        const Model::state_vector_t &x = V.col(0);
        const Model::input_vector_t u = u_t0 + t / dt * (u_t1 - u_t0);

        Model::state_vector_t f;
        Model::state_matrix_t A_bar;
        Model::control_matrix_t B_bar;
        model.computef(x, u, f);
        model.computeJacobians(x, u, A_bar, B_bar);
        A_bar *= T;
        B_bar *= T;

        const Model::state_matrix_t Phi_A_xi = V.block<Model::state_dim, Model::state_dim>(0, 1);
        const Model::state_matrix_t Phi_A_xi_inverse = Phi_A_xi.inverse();

        size_t cols = 0;

        // state
        dVdt.block<Model::state_dim, 1>(0, cols) = T * f;
        cols += 1;

        // A_bar
        dVdt.block<Model::state_dim, Model::state_dim>(0, cols) = A_bar * Phi_A_xi;
        cols += Model::state_dim;

        // B_bar
        const double alpha = (dt - t) / dt;
        dVdt.block<Model::state_dim, Model::input_dim>(0, cols) = Phi_A_xi_inverse * B_bar * alpha;
        cols += Model::input_dim;

        // C_bar
        const double beta = t / dt;
        dVdt.block<Model::state_dim, Model::input_dim>(0, cols) = Phi_A_xi_inverse * B_bar * beta;
        cols += Model::input_dim;

        // S_bar
        dVdt.block<Model::state_dim, 1>(0, cols) = Phi_A_xi_inverse * f;
        cols += 1;

        // z_bar
        dVdt.block<Model::state_dim, 1>(0, cols) = Phi_A_xi_inverse * (-A_bar * x - B_bar * u);

        assert(cols + 1 == size_t(dVdt.cols()));
    }
};

void multipleShootingVariableTime(
    Model &model,
    double T,
    const Eigen::MatrixXd &X,
    const Eigen::MatrixXd &U,
    Model::state_matrix_v_t &A_bar,
    Model::control_matrix_v_t &B_bar,
    Model::control_matrix_v_t &C_bar,
    Model::state_vector_v_t &S_bar,
    Model::state_vector_v_t &z_bar)
{
    const size_t K = X.cols();

    const double dt = 1. / double(K - 1);
    using namespace boost::numeric::odeint;
    runge_kutta4<ODEMultipleShootingVariableTime::ode_matrix_t, double, ODEMultipleShootingVariableTime::ode_matrix_t, double, vector_space_algebra> stepper;

    for (size_t k = 0; k < K - 1; k++)
    {
        ODEMultipleShootingVariableTime::ode_matrix_t V;
        V.setZero();
        V.col(0) = X.col(k);
        V.block<Model::state_dim, Model::state_dim>(0, 1).setIdentity();

        ODEMultipleShootingVariableTime odeMultipleShooting(U.col(k), U.col(k + 1), T, dt, model);

        integrate_adaptive(stepper, odeMultipleShooting, V, 0., dt, dt / 4.);

        size_t cols = 1;

        A_bar[k] = V.block<Model::state_dim, Model::state_dim>(0, cols);
        cols += Model::state_dim;

        B_bar[k] = A_bar[k] * V.block<Model::state_dim, Model::input_dim>(0, cols);
        cols += Model::input_dim;

        C_bar[k] = A_bar[k] * V.block<Model::state_dim, Model::input_dim>(0, cols);
        cols += Model::input_dim;

        S_bar[k] = A_bar[k] * V.block<Model::state_dim, 1>(0, cols);
        cols += 1;

        z_bar[k] = A_bar[k] * V.block<Model::state_dim, 1>(0, cols);
    }
}

class ODEMultipleShooting
{
private:
    Model::input_vector_t u_t0, u_t1;
    double dt;
    Model &model;

public:
    using ode_matrix_t = Eigen::Matrix<double, Model::state_dim, 1 + Model::state_dim + 2 * Model::input_dim + 1>;

    ODEMultipleShooting(
        const Model::input_vector_t &u_t0,
        const Model::input_vector_t &u_t1,
        double dt,
        Model &model)
        : u_t0(u_t0), u_t1(u_t1), dt(dt), model(model) {}

    void operator()(const ode_matrix_t &V, ode_matrix_t &dVdt, const double t)
    {
        const Model::state_vector_t &x = V.col(0);
        const Model::input_vector_t u = u_t0 + t / dt * (u_t1 - u_t0);

        Model::state_vector_t f;
        Model::state_matrix_t A_bar;
        Model::control_matrix_t B_bar;
        model.computef(x, u, f);
        model.computeJacobians(x, u, A_bar, B_bar);

        const Model::state_matrix_t Phi_A_xi = V.block<Model::state_dim, Model::state_dim>(0, 1);
        const Model::state_matrix_t Phi_A_xi_inverse = Phi_A_xi.inverse();

        size_t cols = 0;

        // state
        dVdt.block<Model::state_dim, 1>(0, cols) = f;
        cols += 1;

        // A_bar
        dVdt.block<Model::state_dim, Model::state_dim>(0, cols) = A_bar * Phi_A_xi;
        cols += Model::state_dim;

        // B_bar
        const double alpha = (dt - t) / dt;
        dVdt.block<Model::state_dim, Model::input_dim>(0, cols) = Phi_A_xi_inverse * B_bar * alpha;
        cols += Model::input_dim;

        // C_bar
        const double beta = t / dt;
        dVdt.block<Model::state_dim, Model::input_dim>(0, cols) = Phi_A_xi_inverse * B_bar * beta;
        cols += Model::input_dim;

        // z_bar
        dVdt.block<Model::state_dim, 1>(0, cols) = Phi_A_xi_inverse * (f - A_bar * x - B_bar * u);

        assert(cols + 1 == size_t(dVdt.cols()));
    }
};

void multipleShooting(
    Model &model,
    double T,
    const Eigen::MatrixXd &X,
    const Eigen::MatrixXd &U,
    Model::state_matrix_v_t &A_bar,
    Model::control_matrix_v_t &B_bar,
    Model::control_matrix_v_t &C_bar,
    Model::state_vector_v_t &z_bar)
{
    const size_t K = X.cols();

    const double dt = T / double(K - 1);
    using namespace boost::numeric::odeint;
    runge_kutta4<ODEMultipleShooting::ode_matrix_t, double, ODEMultipleShooting::ode_matrix_t, double, vector_space_algebra> stepper;

    for (size_t k = 0; k < K - 1; k++)
    {
        ODEMultipleShooting::ode_matrix_t V;
        V.setZero();
        V.col(0) = X.col(k);
        V.block<Model::state_dim, Model::state_dim>(0, 1).setIdentity();

        ODEMultipleShooting odeMultipleShooting(U.col(k), U.col(k + 1), dt, model);

        integrate_adaptive(stepper, odeMultipleShooting, V, 0., dt, dt / 4.);

        size_t cols = 1;

        A_bar[k] = V.block<Model::state_dim, Model::state_dim>(0, cols);
        cols += Model::state_dim;

        B_bar[k] = A_bar[k] * V.block<Model::state_dim, Model::input_dim>(0, cols);
        cols += Model::input_dim;

        C_bar[k] = A_bar[k] * V.block<Model::state_dim, Model::input_dim>(0, cols);
        cols += Model::input_dim;

        z_bar[k] = A_bar[k] * V.block<Model::state_dim, 1>(0, cols);
    }
}


#include <boost/math/special_functions/detail/igamma_inverse.hpp>

namespace boostswift {

double gamma_p_inv(double a, double p) {
    return boost::math::gamma_p_inv(a, p);
}

}


#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <Eigen/Eigen>

// http://ankokudan.org/d/dl/pdf/pdf-eigennote.pdf

Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> exp( Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> &output)
{
    std::cout << "output.rows() = " << output.rows() << std::endl;
    std::cout << "output.cols() = " << output.cols() << std::endl;
    Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> o = output;
    for(int i=0; i<output.cols(); i++)
      {
        o(0, i) = 1.0/(1.0 + exp(o(0, i)));
      }
    return o;
}

int main(void)
{
  Eigen::Matrix<float, 2,2> m1 = Eigen::Matrix<float, 2, 2>::Random(2,2);
  Eigen::Matrix<float, 2,2> m2;
  Eigen::Matrix<float, 2,2> m3;
  Eigen::Matrix<float, 2, 1> v1;
  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> A;
  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> B;

  m2 <<
    1, 0,
    1, 2;

  m3 <<
    0, 1,
    2, 0;

  v1 << 1, 4;

  std::cout << "m1 = " << m1 << std::endl;
  std::cout << "m2 = " << m2 << std::endl;
  std::cout << "m3 = " << m3 << std::endl;
  std::cout << "m2 - m3 = " << m2 - m3 << std::endl;
  std::cout << "m2 * m3 = " << m2 * m3 << std::endl;
  std::cout << "v1 = " << v1 << std::endl;

  std::cout << "m2.size() = " << m2.size() << std::endl;
  std::cout << "v1.rows() = " << v1.rows() << std::endl;
  std::cout << "v1.cols() = " << v1.cols() << std::endl;

  std::cout << "v1 * m3 = " << v1.transpose() * m3 << std::endl;
  A = v1.transpose() * m3;

  std::cout << "v1.rows() = " << v1.rows() << std::endl;
  std::cout << "v1.cols() = " << v1.cols() << std::endl;
  std::cout << "A.rows() = " << A.rows() << std::endl;
  std::cout << "A.cols() = " << A.cols() << std::endl;

  B = exp(A);

  std::cout << "B.rows() = " << B.rows() << std::endl;
  std::cout << "B.cols() = " << B.cols() << std::endl;
  std::cout << "B = " << B << std::endl;

  return 0;
}


#ifndef SHAPE_RETRIEVAL__HISTOGRAM_H_
#define SHAPE_RETRIEVAL__HISTOGRAM_H_

#include <Eigen/Core>
#include <map>
#include <vector>
#include <math.h>
#include <boost/algorithm/string.hpp>
#include "Vocabulary.hpp"

using namespace Eigen;

class Histogram
{
public:

	static Vocabulary vocabulary;

	Histogram() = default;
    Histogram(MatrixXd const& bagOfFeatures);

    void setValue(MatrixXd const& bagOfFeatures);
    void writeToFile(string path);
    void writeToFile(ofstream &file);
    void setFromFile(string path);
    void setFromStream(ifstream &file);

    double distance(Histogram const& hist2);

    std::map<int, double> weights;

private:

    VectorXi computeCentroids(MatrixXd const& bagOfFeatures);
    void computeWeights(VectorXi const& bagOfWords);
};

#endif //SHAPE_RETRIEVAL__HISTOGRAM_H_


#ifndef LATTICE_BETHELATTICE_HPP
#define LATTICE_BETHELATTICE_HPP

#include <string>
#include <vector>
#include <boost/lexical_cast.hpp>

namespace lattice {

class BetheLattice {
public:
  static std::string name() { return "Bethe Lattice";}
  BetheLattice(unsigned int LatticeSize , unsigned int NeighboringNumber) : num_stage(LatticeSize), n_adj(NeighboringNumber) {}
  void create_table(std::vector<std::vector < int > >& table){
    for(int stage = 0; stage < num_stage+1 ; ++stage){
      int num_each_stage = calc_num_each_stage(stage);
      for(int tagg = 0; tagg < num_each_stage ; ++tagg){
        int number  = numberize(stage,tagg);
        if(stage == num_stage){
          for(int i = 0; i < n_adj - 1; ++i){
            table[number][i] = number;
          }
        }
        else if(stage == 0){
          for(int i = 0; i < n_adj; ++i){
            table[number][i] = i+1;
            table[i+1][n_adj-1] = number;
          }
        }
        else{
          for(int i = 0; i < n_adj - 1; ++i){
            int pair_number = numberize(stage+1, tagg*(n_adj-1)+i);
            table[number][i] = pair_number;
            table[pair_number][n_adj-1] = number;
          }
        }
      }
    }
  }

  int numberize(const int stage, const int tagg){// j <= std::pow(n_adj-1,i-1)* n_adj - 1
    if(stage ==0) return 0;
    else return BetheCalc(stage) + 1 + tagg;
  }

  int latticize(int d, const int l){ //d selects stage_number or tagg_number (0, 1) ex. 0 denotes stage, 1 denotes tagg_number
    int stage = num_stage;
    int tagg = 1;
    int result = 0;
    while(tagg != 0){
      tagg = l % BetheCalc(stage);
      --stage;
      if(stage < 2) break;
    }
    if(stage < 2){
      if(l == 0){
        if(d==0) result = 0;
        else result = 0;
      }
      else{
        if(d==0)result = stage;
        else result= l;
      }
    }
   return result;
  }

  int set_num_particles(int Ns){
    if(Ns == 0) return  1;
    else return BetheCalc(Ns+1) + 1;
  }

  int BetheCalc(int stage ){ //N_t => 0
    int num_t = 0;
    for(int k = 1 ; k <= stage-1; ++k){
     num_t += calc_num_each_stage(k);
    }
    return num_t;
  }

  int calc_num_each_stage(int stage){
    double temp;
    double stage_t = boost::lexical_cast<double>(stage);
    double n_adj_t = boost::lexical_cast<double>(n_adj);
    if(stage == 0) temp = 1;
    else temp = n_adj_t * std::pow(n_adj_t-1, stage_t-1) ;
    return boost::lexical_cast<int>(temp);
 }

  int number_adjacent() {return n_adj;}

private:
  unsigned int num_stage;
  unsigned int n_adj;

};//Bethe lattice end

} // end namespace

#endif //LATTICE_BETHELATTICE_HPP


/**
 * \file metric_space_concept.hpp
 *
 * This library defines the traits and concepts that pertain to what can be considered
 * a metric-space, as used in ReaK::pp. Metric-spaces are based on the Topology concept
 * from the Boost.Graph library, but with additional requirements which are needed
 * in algorithms tailored for a metric-space (see metric_space_search.hpp). Basically,
 * the concept of a metric-space in ReaK::pp corresponds to the mathematical concept of
 * a metric-space (see wikipedia or any decent math book).
 *
 * \author Sven Mikael Persson <mikael.s.persson@gmail.com>
 * \date March 2011
 */

/*
 *    Copyright 2011 Sven Mikael Persson
 *
 *    THIS SOFTWARE IS DISTRIBUTED UNDER THE TERMS OF THE GNU GENERAL PUBLIC LICENSE v3 (GPLv3).
 *
 *    This file is part of ReaK.
 *
 *    ReaK is free software: you can redistribute it and/or modify
 *    it under the terms of the GNU General Public License as published by
 *    the Free Software Foundation, either version 3 of the License, or
 *    (at your option) any later version.
 *
 *    ReaK is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *    GNU General Public License for more details.
 *
 *    You should have received a copy of the GNU General Public License
 *    along with ReaK (as LICENSE in the root folder).
 *    If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef METRIC_SPACE_CONCEPT_HPP
#define METRIC_SPACE_CONCEPT_HPP

#include <boost/config.hpp>
#include <cmath>
#include <boost/concept_check.hpp>

/**
 * This traits class defines the types and constants associated to a metric-space.
 * \tparam Topology The topology type for which the metric-space traits are sought.
 */
template <typename Topology>
struct metric_topology_traits {
  /** The type that describes a point in the space. */
  typedef typename Topology::point_type point_type;
  /** The type that describes a difference between points in the space. */
  typedef typename Topology::point_difference_type point_difference_type;

  /** The dimensions of the space (0 if unknown at compile-time). */
  BOOST_STATIC_CONSTANT(std::size_t, dimensions = Topology::dimensions);

};

/**
 * This concept defines the requirements to fulfill in order to model a distance-metric
 * as used in ReaK::pp. A distance-metric is essentially a callable type that can compute
 * both the distance between two points and the corresponding norm of a difference between
 * two points.
 *
 * Required concepts:
 *
 * Topology should model the Topology concept of the BGL.
 *
 * Valid expressions:
 *
 * dist = d(p1, p2, s);  The distance (dist) can be obtained by calling the distance metric (d) on two points (p1,p2) and providing a const-ref to the topology (or space) (s).
 *
 * dist = d(pd, s);  The distance (dist) can be obtained by calling the distance metric (d) on a point-difference (pd) and providing a const-ref to the topology (or space) (s).
 *
 * \tparam DistanceMetric The distance metric type to be checked for this concept.
 * \tparam Topology The topology to which the distance metric should apply.
 */
template <typename DistanceMetric, typename Topology>
struct DistanceMetricConcept {
  DistanceMetric d;
  Topology s;
  typename metric_topology_traits<Topology>::point_type p1, p2;
  typename metric_topology_traits<Topology>::point_difference_type pd;
  double dist;

  BOOST_CONCEPT_USAGE(DistanceMetricConcept)
  {
    dist = d(p1, p2, s);
    dist = d(pd, s);
  };

};

/**
 * This concept defines the requirements to fulfill in order to model a metric-space
 * as used in ReaK::pp. A metric-space is a special kind of topology which has a
 * distance metric (in theory, satisfying triangular inequality).
 *
 * Valid expressions:
 *
 * d  = space.distance(p1, p2);  The distance between two points (p1,p2) can be obtained as a double (d).
 *
 * d  = space.norm(pd);  The norm of the difference (pd) between two points can be obtained as a double (d).
 *
 * p1 = space.random_point();  A random-point in the metric-space can be obtained.
 *
 * pd = space.difference(p1,p2);  The difference (pd) between two points (p1,p2) can be obtained.
 *
 * p1 = space.move_position_toward(p1,d,p2);  A point can be obtained by moving a fraction (d) away from one point (p1) to another (p2).
 *
 * p1 = space.origin();  The origin of the space can be obtained.
 *
 * p1 = space.adjust(p1,d * pd + pd - pd);  A point-difference can be scaled (d * pd), added / subtracted to another point-difference and added to a point (p1) to obtain an adjusted point.
 *
 * pd = -pd;  A point-difference can be negated (reversed) and is assignable.
 *
 * pd -= pd;  A point-difference can be subtracted-and-assigned.
 *
 * pd += pd;  A point-difference can be added-and-assigned.
 *
 * \tparam Topology The topology type to be checked for this concept.
 */
template <typename Topology>
struct MetricSpaceConcept {
  typename metric_topology_traits<Topology>::point_type p1, p2;
  typename metric_topology_traits<Topology>::point_difference_type pd;
  Topology space;
  double d;

  BOOST_CONCEPT_USAGE(MetricSpaceConcept)
  {
    d  = space.distance(p1, p2);
    d  = space.norm(pd);
    p1 = space.random_point();
    pd = space.difference(p1,p2);
    p1 = space.move_position_toward(p1,d,p2);
    p1 = space.origin();
    p1 = space.adjust(p1,d * pd + pd - pd);
    pd = -pd;
    pd -= pd;
    pd += pd;
  };

};

#endif


#pragma once

/**
 * Reduced product of a numerical domain and the congruence domain.
 **/

#include <crab/domains/abstract_domain.hpp>
#include <crab/domains/abstract_domain_specialized_traits.hpp>
#include <crab/domains/combined_domains.hpp>
#include <crab/domains/congruences.hpp>
#include <crab/domains/interval.hpp>
#include <crab/support/stats.hpp>

#include <boost/optional.hpp>

namespace crab {
namespace domains {

/*
 *  The reduce operator based on "Static Analysis of Arithmetical
 *  Congruences" by P. Granger published in International Journal of
 *  Computer Mathematics, 1989.
 */
template <typename Number> class interval_congruence {
public:
  using interval_congruence_t = interval_congruence<Number>;

private:
  using interval_t = ikos::interval<Number>;
  using congruence_t = ikos::congruence<Number>;
  using bound_t = ikos::bound<Number>;

private:
  interval_t m_first;
  congruence_t m_second;

private:
  interval_congruence(bool is_bottom)
      : m_first(is_bottom ? interval_t::bottom() : interval_t::top()),
        m_second(is_bottom ? congruence_t::bottom() : congruence_t::top()) {}

public:
  static interval_congruence_t top() { return interval_congruence(false); }

  static interval_congruence_t bottom() { return interval_congruence(true); }

private:
  inline Number abs(Number x) { return x < 0 ? -x : x; }

  // operator % can return a negative number
  // mod(a, b) always returns a positive number
  inline Number mod(Number a, Number b) {
    Number m = a % b;
    if (m < 0)
      return m + b;
    else
      return m;
  }

  // R(c,a) is the least element of c greater or equal than a
  inline Number R(congruence_t c, Number a) {
    Number m = c.get_modulo();
    Number p = c.get_remainder();
    return a + mod(p - a, abs(m));
  }

  // L(c,a) is the greatest element of c smaller or equal than a
  inline Number L(congruence_t c, Number a) {
    Number m = c.get_modulo();
    Number p = c.get_remainder();
    return a - mod(a - p, abs(m));
  }

public:
  interval_congruence(Number n)
      : m_first(interval_t(n)), m_second(congruence_t(n)) {}

  interval_congruence(interval_t i, congruence_t c) : m_first(i), m_second(c) {
    reduce();
  }

  interval_congruence(interval_t i)
      : m_first(i), m_second(congruence_t::top()) {
    reduce();
  }

  interval_congruence(congruence_t c)
      : m_first(interval_t::top()), m_second(c) {
    reduce();
  }

  interval_congruence(const interval_congruence &other)
      : m_first(other.m_first), m_second(other.m_second) {}

  interval_congruence(interval_congruence &&other)
      : m_first(std::move(other.m_first)), m_second(std::move(other.m_second)) {
  }

  interval_congruence_t &operator=(const interval_congruence_t &other) {
    if (this != &other) {
      m_first = other.m_first;
      m_second = other.m_second;
    }
    return *this;
  }

  interval_congruence_t &operator=(interval_congruence_t &&other) {
    if (this != &other) {
      m_first = std::move(other.m_first);
      m_second = std::move(other.m_second);
    }
    return *this;
  }

  bool is_bottom() { return m_first.is_bottom() || m_second.is_bottom(); }

  bool is_top() { return m_first.is_top() && m_second.is_top(); }

  interval_t &first() { return m_first; }
  const interval_t &first() const { return m_first; }

  congruence_t &second() { return m_second; }
  const congruence_t &second() const { return m_second; }

  /*
     Let (i,c) be a pair of interval and congruence these are the
     main rules described by Granger:

     if (c.is_bottom() || i.is_bottom()) (bottom(), bottom());
     if (c = 0Z+a and a \notin i)        (bottom(), bottom());
     if (c = 0Z+a)                       ([a,a]   , c);
     if (i=[a,b] and R(c,a) > L(c,b))    (bottom(), bottom());
     if (i=[a,b])                        ([R(c,a), L(c,b)], c);
     if (i=[a,+oo])                      ([R(c,a), +oo], c);
     if (i=[-oo,b])                      ([-oo, L(c,b)], c);
     otherwise                           (i,c)
   */

  void reduce() {
    interval_t &i = first();
    congruence_t &c = second();

    if (i.is_bottom() || c.is_bottom()) {
      i = interval_t::bottom();
      c = congruence_t::bottom();
    }

    // congruence is top and interval is a singleton
    if (c.is_top()) {
      boost::optional<Number> n = i.singleton();
      if (n) {
        c = congruence_t(*n);
      }
      return;
    }

    Number modulo = c.get_modulo();
    if (modulo == 0) {
      // congruence is a singleton so we refine the interval
      interval_t a(c.get_remainder());
      if (!(a <= i)) {
        i = interval_t::bottom();
        c = congruence_t::bottom();
      } else {
        i = a;
      }
    } else {
      // refine lower and upper bounds of the interval using
      // congruences
      bound_t lb = i.lb();
      bound_t ub = i.ub();

      if (lb.is_finite() && ub.is_finite()) {
        Number x = R(c, *(lb.number()));
        Number y = L(c, *(ub.number()));
        if (x > y) {
          i = interval_t::bottom();
          c = congruence_t::bottom();
        } else if (x == y) {
          i = interval_t(x);
          c = congruence_t(x);
        } else {
          i = interval_t(bound_t(x), bound_t(y));
        }
      } else if (lb.is_finite()) {
        Number x = R(c, *(lb.number()));
        i = interval_t(bound_t(x), bound_t::plus_infinity());
      } else if (ub.is_finite()) {
        Number y = L(c, *(ub.number()));
        i = interval_t(bound_t::minus_infinity(), bound_t(y));
      } else {
        // interval is top
      }
    }
  }

  void write(crab_os &o) const {
    o << "(" << m_first << ", " << m_second << ")";
  }

public:
  interval_congruence_t operator+(interval_congruence_t x) {
    return interval_congruence_t(m_first.operator+(x.first()),
                                 m_second.operator+(x.second()));
  }

  interval_congruence_t operator-(interval_congruence_t x) {
    return interval_congruence_t(m_first.operator-(x.first()),
                                 m_second.operator-(x.second()));
  }

  interval_congruence_t operator*(interval_congruence_t x) {
    return interval_congruence_t(m_first.operator*(x.first()),
                                 m_second.operator*(x.second()));
  }

  interval_congruence_t operator/(interval_congruence_t x) {
    return interval_congruence_t(m_first.operator/(x.first()),
                                 m_second.operator/(x.second()));
  }

  interval_congruence_t operator|(interval_congruence_t other) {
    return interval_congruence_t(m_first | other.m_first,
                                 m_second | other.m_second);
  }

  interval_congruence_t operator&(interval_congruence_t other) {
    return interval_congruence_t(m_first & other.m_first,
                                 m_second & other.m_second);
  }

public:
  // division and remainder operations

  interval_congruence_t SDiv(interval_congruence_t x) {
    return interval_congruence_t(m_first.SDiv(x.first()),
                                 m_second.SDiv(x.second()));
  }

  interval_congruence_t UDiv(interval_congruence_t x) {
    return interval_congruence_t(m_first.UDiv(x.first()),
                                 m_second.UDiv(x.second()));
  }

  interval_congruence_t SRem(interval_congruence_t x) {
    return interval_congruence_t(m_first.SRem(x.first()),
                                 m_second.SRem(x.second()));
  }

  interval_congruence_t URem(interval_congruence_t x) {
    return interval_congruence_t(m_first.URem(x.first()),
                                 m_second.URem(x.second()));
  }

  // bitwise operations

  interval_congruence_t Trunc(unsigned width) {
    return interval_congruence_t(m_first.Trunc(width), m_second.Trunc(width));
  }

  interval_congruence_t ZExt(unsigned width) {
    return interval_congruence_t(m_first.ZExt(width), m_second.ZExt(width));
  }

  interval_congruence_t SExt(unsigned width) {
    return interval_congruence_t(m_first.SExt(width), m_second.SExt(width));
  }

  interval_congruence_t And(interval_congruence_t x) {
    return interval_congruence_t(m_first.And(x.first()),
                                 m_second.And(x.second()));
  }

  interval_congruence_t Or(interval_congruence_t x) {
    return interval_congruence_t(m_first.Or(x.first()),
                                 m_second.Or(x.second()));
  }

  interval_congruence_t Xor(interval_congruence_t x) {
    return interval_congruence_t(m_first.Xor(x.first()),
                                 m_second.Xor(x.second()));
  }

  interval_congruence_t Shl(interval_congruence_t x) {
    return interval_congruence_t(m_first.Shl(x.first()),
                                 m_second.Shl(x.second()));
  }

  interval_congruence_t LShr(interval_congruence_t x) {
    return interval_congruence_t(m_first.LShr(x.first()),
                                 m_second.LShr(x.second()));
  }

  interval_congruence_t AShr(interval_congruence_t x) {
    return interval_congruence_t(m_first.AShr(x.first()),
                                 m_second.AShr(x.second()));
  }
};

template <typename Number>
inline crab::crab_os &operator<<(crab::crab_os &o,
                                 const interval_congruence<Number> &v) {
  v.write(o);
  return o;
}

// Reduced product of a numerical domain with interval x congruences.
template <typename NumAbsDom>
class numerical_congruence_domain final
    : public abstract_domain_api<numerical_congruence_domain<NumAbsDom>> {

  using rnc_domain_t = numerical_congruence_domain<NumAbsDom>;
  using abstract_domain_t = abstract_domain_api<rnc_domain_t>;

public:
  using typename abstract_domain_t::disjunctive_linear_constraint_system_t;
  using typename abstract_domain_t::interval_t;
  using typename abstract_domain_t::linear_constraint_system_t;
  using typename abstract_domain_t::linear_constraint_t;
  using typename abstract_domain_t::linear_expression_t;
  using typename abstract_domain_t::reference_constraint_t;
  using typename abstract_domain_t::variable_or_constant_t;
  using typename abstract_domain_t::variable_t;
  using typename abstract_domain_t::variable_vector_t;
  using typename abstract_domain_t::variable_or_constant_vector_t;
  using number_t = typename NumAbsDom::number_t;
  using varname_t = typename NumAbsDom::varname_t;

  using congruence_domain_t = ikos::congruence_domain<number_t, varname_t>;
  using interval_congruence_t = interval_congruence<number_t>;

private:
  using reduced_domain_product2_t =
      reduced_domain_product2<number_t, varname_t, NumAbsDom, congruence_domain_t>;

  reduced_domain_product2_t m_product;

  numerical_congruence_domain(const reduced_domain_product2_t &product)
      : m_product(product) {}

  void reduce_variable(const variable_t &v) {
    crab::CrabStats::count(domain_name() + ".count.reduce");
    crab::ScopedCrabStats __st__(domain_name() + ".reduce");

    if (is_bottom()) {
      return;
    }

    auto i = m_product.first()[v]; // project on intervals
    auto c = m_product.second().to_congruence(v);
    interval_congruence_t val(i, c);

    if (val.is_bottom()) {
      set_to_bottom();
    } else {
      if (val.first() != i) {
        // FIXME: method set is not part of the abstract_domain API so
        // it might not compile.
        m_product.first().set(v, val.first());
      }

      if (val.second() != c) {
        // FIXME: method set is not part of the abstract_domain API so
        // it might not compile.
        m_product.second().set(v, val.second());
      }
    }
  }

public:
  rnc_domain_t make_top() const override {
    reduced_domain_product2_t dom_prod;
    return rnc_domain_t(dom_prod.make_top());
  }

  rnc_domain_t make_bottom() const override {
    reduced_domain_product2_t dom_prod;
    return rnc_domain_t(dom_prod.make_bottom());
  }

  void set_to_top() override {
    reduced_domain_product2_t dom_prod;
    rnc_domain_t abs(dom_prod.make_top());
    std::swap(*this, abs);
  }

  void set_to_bottom() override {
    reduced_domain_product2_t dom_prod;
    rnc_domain_t abs(dom_prod.make_bottom());
    std::swap(*this, abs);
  }

  numerical_congruence_domain() : m_product() {}

  numerical_congruence_domain(const rnc_domain_t &other)
      : m_product(other.m_product) {}

  rnc_domain_t &operator=(const rnc_domain_t &other) {
    if (this != &other)
      m_product = other.m_product;

    return *this;
  }

  bool is_bottom() const override { return m_product.is_bottom(); }

  bool is_top() const override { return m_product.is_top(); }

  bool operator<=(const rnc_domain_t &other) const override {
    return m_product <= other.m_product;
  }

  bool operator==(const rnc_domain_t &other) const {
    return m_product == other.m_product;
  }

  void operator|=(const rnc_domain_t &other) override {
    m_product |= other.m_product;
  }

  rnc_domain_t operator|(const rnc_domain_t &other) const override {
    return rnc_domain_t(m_product | other.m_product);
  }

  rnc_domain_t operator&(const rnc_domain_t &other) const override {
    return rnc_domain_t(m_product & other.m_product);
  }

  rnc_domain_t operator||(const rnc_domain_t &other) const override {
    return rnc_domain_t(m_product || other.m_product);
  }

  rnc_domain_t widening_thresholds(
      const rnc_domain_t &other,
      const iterators::thresholds<number_t> &ts) const override {
    return rnc_domain_t(m_product.widening_thresholds(other.m_product, ts));
  }

  rnc_domain_t operator&&(const rnc_domain_t &other) const override {
    return rnc_domain_t(m_product && other.m_product);
  }

  // pre: x is already reduced
  void set(const variable_t &v, interval_congruence_t x) {
    m_product.first().set(v, x.first());
    m_product.second().set(v, x.second());
  }

  interval_congruence_t get(const variable_t &v) {
    return interval_congruence_t(m_product.first()[v],
                                 m_product.second().to_congruence(v));
  }

  interval_t operator[](const variable_t &v) override {
    interval_congruence_t x = get(v);
    return x.first();
  }

  void operator+=(const linear_constraint_system_t &csts) override {
    m_product += csts;

    if (!is_bottom()) {
      for (auto const &cst : csts) {
        for (auto const &v : cst.variables()) {
          reduce_variable(v);
          if (is_bottom()) {
            return;
          }
        }
      }
    }
  }

  void operator-=(const variable_t &v) override { m_product -= v; }

  void assign(const variable_t &x, const linear_expression_t &e) override {
    m_product.assign(x, e);
    reduce_variable(x);
  }

  void apply(arith_operation_t op, const variable_t &x, const variable_t &y,
             const variable_t &z) override {
    m_product.apply(op, x, y, z);
    reduce_variable(x);
  }

  void apply(arith_operation_t op, const variable_t &x, const variable_t &y,
             number_t k) override {
    m_product.apply(op, x, y, k);
    reduce_variable(x);
  }

  void backward_assign(const variable_t &x, const linear_expression_t &e,
                       const rnc_domain_t &invariant) override {
    m_product.backward_assign(x, e, invariant.m_product);
    // reduce the variables in the right-hand side
    for (auto const &v : e.variables())
      reduce_variable(v);
  }

  void backward_apply(arith_operation_t op, const variable_t &x,
                      const variable_t &y, number_t k,
                      const rnc_domain_t &invariant) override {
    m_product.backward_apply(op, x, y, k, invariant.m_product);
    // reduce the variables in the right-hand side
    reduce_variable(y);
  }

  void backward_apply(arith_operation_t op, const variable_t &x,
                      const variable_t &y, const variable_t &z,
                      const rnc_domain_t &invariant) override {
    m_product.backward_apply(op, x, y, z, invariant.m_product);
    // reduce the variables in the right-hand side
    reduce_variable(y);
    reduce_variable(z);
  }

  // cast operators

  void apply(int_conv_operation_t op, const variable_t &dst,
             const variable_t &src) override {
    m_product.apply(op, dst, src);
    reduce_variable(dst);
  }

  // bitwise operators

  void apply(bitwise_operation_t op, const variable_t &x, const variable_t &y,
             const variable_t &z) override {
    m_product.apply(op, x, y, z);
    reduce_variable(x);
  }

  void apply(bitwise_operation_t op, const variable_t &x, const variable_t &y,
             number_t k) override {
    m_product.apply(op, x, y, k);
    reduce_variable(x);
  }

  void select(const variable_t &lhs, const linear_constraint_t &cond,
              const linear_expression_t &e1,
              const linear_expression_t &e2) override {
    m_product.select(lhs, cond, e1, e2);
    reduce_variable(lhs);
  }

  /// numerical_congruence_domain implements only standard abstract
  /// operations of a numerical domain so it is intended to be used as
  /// a leaf domain in the hierarchy of domains.
  BOOL_OPERATIONS_NOT_IMPLEMENTED(rnc_domain_t)
  ARRAY_OPERATIONS_NOT_IMPLEMENTED(rnc_domain_t)
  REGION_AND_REFERENCE_OPERATIONS_NOT_IMPLEMENTED(rnc_domain_t)

  void forget(const variable_vector_t &variables) override {
    m_product.forget(variables);
  }

  void project(const variable_vector_t &variables) override {
    m_product.project(variables);
  }

  void expand(const variable_t &var, const variable_t &new_var) override {
    m_product.expand(var, new_var);
  }

  void normalize() override { m_product.normalize(); }

  void minimize() override { m_product.minimize(); }

  void write(crab_os &o) const override { m_product.write(o); }

  linear_constraint_system_t to_linear_constraint_system() const override {
    return m_product.to_linear_constraint_system();
  }

  disjunctive_linear_constraint_system_t
  to_disjunctive_linear_constraint_system() const override {
    return m_product.to_disjunctive_linear_constraint_system();
  }

  std::string domain_name() const override { return m_product.domain_name(); }

  void rename(const variable_vector_t &from,
              const variable_vector_t &to) override {
    m_product.rename(from, to);
  }

  /* begin intrinsics operations */
  void intrinsic(std::string name,
		 const variable_or_constant_vector_t &inputs,
                 const variable_vector_t &outputs) override {
    m_product.intrinsic(name, inputs, outputs);
  }

  void backward_intrinsic(std::string name,
			  const variable_or_constant_vector_t &inputs,
                          const variable_vector_t &outputs,
                          const rnc_domain_t &invariant) override {
    m_product.backward_intrinsic(name, inputs, outputs, invariant.m_product);
  }
  /* end intrinsics operations */

}; // class numerical_congruence_domain

template <typename NumAbsDom>
struct abstract_domain_traits<numerical_congruence_domain<NumAbsDom>> {
  using number_t = typename NumAbsDom::number_t;
  using varname_t = typename NumAbsDom::varname_t;
};

} // end namespace domains
} // namespace crab


// All content Copyright (C) 2018 Genomics plc
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "readrecalibration/commonTypes.hpp"
#include "stats/functions.hpp"

BOOST_AUTO_TEST_CASE( testPhredToPCache )
{
    for ( auto i = 0; i < 100; ++i )
    {
        auto cacheNumber = wecall::corrector::phred_to_p( i );
        auto computedNumber = wecall::stats::fromPhredQ( i );
        BOOST_CHECK_CLOSE( cacheNumber, computedNumber, 1e-8 );
    }
}


#include "multivariate_guassian.hpp"

#include "multinomial.hpp"
#include "utils.hpp"

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

namespace FilterModel {

MultivariateGuassian::MultivariateGuassian(const std::vector<double> mean_in,
                                           const std::vector<std::vector<double>> covariance_in)
    : mean(mean_in.size()), covariance(covariance_in.size(), covariance_in.size()) {
    for (int i = 0; i < mean_in.size(); ++i) {
        mean[i] = mean_in.at(i);
    }

    for (int i = 0; i < covariance_in.size(); ++i) {
        const std::vector<double>& row = covariance_in.at(i);
        for (int j = 0; j < row.size(); ++j) {
            covariance(i, j) = row.at(j);
            covariance(j, i) = row.at(j);
        }
    }

    covariance_inverse = covariance.inverse();
};

MultivariateGuassian MultivariateGuassian::from_multinomial(int n, const std::vector<double>& p) {
    std::vector<double> mean;
    for (double pi : p) {
        mean.push_back(n * pi);
    }

    std::vector<std::vector<double>> covariance;
    for (int row = 0; row < p.size(); ++row) {
        std::vector<double> covariance_row;
        for (int column = 0; column < row; ++column) {
            covariance_row.push_back(-n * p.at(row) * p.at(column));
        }
        covariance_row.push_back(n * p.at(row) * (1.0 - p.at(row)));
        covariance.push_back(covariance_row);
    }

    MultivariateGuassian out(mean, covariance);

    return out;
}

MultivariateGuassian MultivariateGuassian::from_multinomial(const Multinomial& m) {
    std::vector<double> new_p(m.p);
    new_p.pop_back();
    return MultivariateGuassian::from_multinomial(m.n, new_p);
}

double MultivariateGuassian::density(std::vector<double> point) const {
    if (point.size() != mean.size()) {
        BOOST_LOG_TRIVIAL(fatal) << "MultivariateGuassian density input must have the same number "
                                    "of dimensions as the distribution.";
        assert(false);
    }

    // TODO(joschnei): There has to be a better way to do this copy.
    Eigen::VectorXd x(point.size());
    for (int i = 0; i < point.size(); ++i) {
        x[i] = point.at(i);
    }

    Eigen::VectorXd diff = (x - mean).eval();
    Eigen::RowVectorXd mult1 = (diff.transpose() * covariance_inverse).eval();
    double mult2 = (mult1 * diff).eval()(0, 0);
    double det = covariance.determinant();
    double val = std::exp(-1.0 / 2.0 * mult2) / std::sqrt(std::pow(2 * M_PI, mean.size()) * det);
    return val;
}

void MultivariateGuassian::shift_hyperplanes(std::vector<std::vector<double>>& hyperplanes) {
    for (std::vector<double>& hyperplane : hyperplanes) {
        double constant = hyperplane.back();
        for (int i = 0; i < hyperplane.size() - 1; ++i) {
            constant -= hyperplane.at(i) * mean[i];
        }
        hyperplane[hyperplane.size() - 1] = constant;
    }
}

std::vector<std::vector<double>> MultivariateGuassian::get_covariance() const {
    std::vector<std::vector<double>> out;
    for (int row_index = 0; row_index < covariance.rows(); ++row_index) {
        std::vector<double> row;
        for (int column_index = 0; column_index < covariance.cols(); ++column_index) {
            row.push_back(covariance(row_index, column_index));
        }
        out.push_back(row);
    }
    return out;
}

std::vector<double> MultivariateGuassian::get_mean() const {
    std::vector<double> out;
    for (int i = 0; i < mean.size(); ++i) {
        out.push_back(mean[i]);
    }
    return out;
}

}  // namespace FilterModel


#pragma once
#include "profile.hpp"
#include "mesh.hpp"
#include <Eigen/Sparse>
#include <Eigen/Core>
#include <Eigen/Eigen>
#include "transformation.hpp"
#undef TEST_SPARSE

namespace gd {

USING_PART_OF_NAMESPACE_EIGEN;

template<class Mesh>
class PoissonSolver1d;

const double ProfileNumerical1d_epsilon = 1e-5;
template<class Mesh>
	class ProfileNumerical1d : public Profile {
	int n;
	Mesh mesh;
	Density* density;
	PoissonSolver1d<Mesh> solver;
	VectorXd solution;
	Transformation1d_in_3d* transformation;
	double G;
public:

	ProfileNumerical1d(int n, Density* density, Transformation1d_in_3d* transformation, double G, double u1, double u2) : mesh(u1, u2, n, transformation), density(density), solver(density, &mesh, G), solution(mesh.get_dof()), transformation(transformation), G(G) {
		solver.solve_(solution, 1., 1., 0.);
	}
	virtual double densityr(double r) {
		return density->densityr(r);
	}
	virtual double densityR(double) {
		return 0;
	}
	virtual double I(double, double) {
		return 0;
	}
	virtual double dphidr(double r) {
		double u = transformation->inverse_transform(r);
		//double u = atan(r) * 2 / M_PI;
		//double jacobian = M_PI/2 / pow(cos(u*M_PI/2), 2);
		//double du
		//return mesh.gradient(solution, u) / jacobian;
		return mesh.gradient(solution, u) / transformation->drdu(u); //jacobian;
	}
	virtual double potentialr(double r) {
		//double u = atan(r) * 2 / M_PI;
		double u = transformation->inverse_transform(r);
		return mesh.eval(solution, u);
	}
};

template<class Mesh>
class PoissonSolver1d {
public:
	typedef Mesh mesh_type;
	Density* density;
	Mesh* mesh;
	VectorXd lasta;
	double G;

	PoissonSolver1d(Density* density, Mesh* mesh, double G) : density(density), mesh(mesh), lasta(mesh->get_dof()), G(G) {
	}

	double operator()(double x) {
		return mesh->eval(lasta, x);
	}

	double gradient(double x) {
		return mesh->gradient(lasta, x);
	}

	void solve(double_vector v, double scale1, double scale2, double boundary_value=0) {
		VectorXd v_copy = VectorXd::Map(v.data().begin(), v.size());
		solve_(v_copy, scale1, scale2, boundary_value);
	}
	void solve_(VectorXd& v, double scale1, double scale2, double boundary_value=0) {

		/*
		solve 'a' from the linear system Ma=x using FEM (Galerkin method)
		such that \Phi(r) = \sum_i a_i \phi_i(r) is the solution to the
		poisson eq: \delta^2 \Phi(r) = 4 pi G rho(r)
		*/
		int dof = mesh->get_dof();
#ifdef TEST_SPARSE
		Eigen::DynamicSparseMatrix<double> Ms(dof, dof);
#else
		MatrixXd M = MatrixXd::Zero(dof, dof);
#endif
		VectorXd x = VectorXd::Zero(dof);

		int dof_per_cell = Mesh::dof_per_cell;
		for(int cell_index = 0; cell_index < mesh->get_n_cells(); cell_index++) {
			for(int i = 0; i < dof_per_cell; i++) {
				for(int j = 0; j < dof_per_cell; j++) {
					//M(cell_index*(dof_per_cell-1)+i, cell_index*(dof_per_cell-1)+j) = integrate dphi_i * dphi_j
#ifdef TEST_SPARSE
					Ms.coeffRef(mesh->dof_index(cell_index, i), mesh->dof_index(cell_index, j)) +=
#else
					M(mesh->dof_index(cell_index, i), mesh->dof_index(cell_index, j)) +=
#endif
						mesh->integrate_gradshape(cell_index, i, j) * scale1; // * r * r;
						/*integrate dphi_i * dphi_j*/
				}
				//x(mesh->index(cell_index, i)) += integrate phi_i * 4 * M_PI * density->densityr(r) dr;
				//auto f = [&](double r) { return this->density->densityr(r) * r * r; }; //*/ };
				auto f = [&](double u) {
					double r = this->mesh->transformation->transform(u);
					//double r = tan(u*M_PI/2);
					//double s = sin(u*M_PI/2);
					//double c = cos(u*M_PI/2);
					//return this->density->densityr(r) * 2 * M_PI*M_PI * s*s/pow(c,4);
					return this->density->densityr(r) * this->mesh->transformation->d3xdu(u);
				};
				//double G = 1;
				//cout << mesh->dof_index(cell_index, i) << " = " << (-4 * M_PI * G * mesh->integrate_shape(cell_index, i, f)) << endl;
				x(mesh->dof_index(cell_index, i)) += -(4 * M_PI) * G * mesh->integrate_shape(cell_index, i, f) * scale2;
			}
		}
		// set boundary condition, Phi(r_end) = 0
		for(int i = 1; i < Mesh::dof_per_cell; i++) {
#ifdef TEST_SPARSE
			Ms.coeffRef(dof-1-i,dof-1) = boundary_value;
			Ms.coeffRef(dof-1,dof-1-i) = boundary_value;
#else
			M(dof-1-i,dof-1) = boundary_value;
			M(dof-1,dof-1-i) = boundary_value;
#endif
		}

#ifdef TEST_SPARSE
		Ms.coeffRef(dof-1,dof-1) = 1;
#else
		M(dof-1,dof-1) = 1;
#endif

#ifdef TEST_SPARSE
		typedef Eigen::SparseMatrix<double> SparseMatrixType;
		SparseMatrixType M(Ms);
#endif
		x(dof-1) = 0;

		//cout << M << endl;
		//cout << "next" << endl << x << endl;
		// solve 'a'
#ifdef TEST_SPARSE
		VectorXd a = x;
		Eigen::SparseLLT<SparseMatrixType,Eigen::Cholmod> sparseLLT(M);
		sparseLLT.solveInPlace(a);
#else
		//VectorXd a = M.inverse() * x;
		VectorXd a(dof);
		M.llt().solve(x, &a);
#endif
		lasta = a;
		// copy to v
		//VectorXd::Map(v.data().begin(), v.size()) = a;
		v = a;
		//cout << "x: " << endl <<  (x) << endl;

		//cout << "solution: " << endl <<  (a) << endl;
		//cout << "test" << endl <<  (M * a) << endl;
	}
};

}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/function/fast.hpp>
#include <boost/simd/function/sqrt.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>

namespace bs = boost::simd;

template <typename T, std::size_t N, typename Env>
void test_fast(Env& $)
{
  using p_t = bs::pack<T, N>;

  T a1[N], b[N];

  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] =  T(N-i);
    b[i] = bs::fast_(bs::sqrt)(a1[i]) ;
  }

  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb (&b[0], &b[0]+N);
  STF_ULP_EQUAL(bs::fast_(bs::sqrt)(aa1), bb, 2048);
}

STF_CASE_TPL("Check fast(sqrt) on pack", STF_IEEE_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;

  test_fast<T, N>($);
  test_fast<T, N/2>($);
  test_fast<T, N*2>($);
}

template <typename T, std::size_t N, typename Env>
void test(Env& $)
{
  using p_t = bs::pack<T, N>;

  T a1[N], b[N];
  a1[0] = bs::Inf<T>();
  b[0] = bs::sqrt(a1[0]);

  for(std::size_t i = 1; i < N; ++i)
  {
    a1[i] =  T(N-i);
    b[i] = bs::sqrt(a1[i]) ;
  }
  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb (&b[0], &b[0]+N);
  STF_ULP_EQUAL(bs::sqrt(aa1), bb, 0.5);
}

STF_CASE_TPL("Check sqrt on pack", STF_IEEE_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;

  test<T, N>($);
  test<T, N/2>($);
  test<T, N*2>($);
}


#include <Eigen/Dense>

#include <boost/preprocessor/repetition/repeat.hpp>

#include <chrono>
#include <iostream>
#include <cstdlib>

// Creates a test matrix, as a diagonal square matrix with positive > 0 random
// values.
template <typename T, int N>
void create_test_matrix(Eigen::Matrix<T, N, N>& M)
{
  for (size_t i = 0; i < N; ++i)
  {
    M(i, i) = std::rand() + 1e-3;
  }
}

// Runs a self-adjoint eigensolver to obtain the eigenvalues and eigenvectors
// of a square matrix.
template <typename T, int N>
bool selfadjoint_eigensolver_test(const Eigen::Matrix<T, N, N>& M)
{
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, N, N> > eigensolver(M);

  return eigensolver.info() == Eigen::Success;
}

template <typename T, int N>
void test()
{
  typedef std::chrono::high_resolution_clock Time;
  typedef std::chrono::duration<double> Duration;

  Eigen::Matrix<T, N, N> M;
  create_test_matrix(M);

  const auto start = Time::now();

  selfadjoint_eigensolver_test(M);

  const auto end = Time::now();

  const auto dt = Duration(end - start).count();

  std::cout << N << " " << dt << std::endl;
}

#define TEST(z, n, _) test<double, n+1>();

int main(int argc, char** argv)
{
  std::cout << "# N = NxN Matrix size\n"
            << "# t = Time [s] to compute SelfAdjointEigenSolver\n"
            << "#\n"
            << "# N t" << std::endl;

  BOOST_PP_REPEAT(15, TEST, _)

  return EXIT_SUCCESS;
}


/* ROS-CHOMP.
 *
 * Copyright (C) 2015 Jafar Qutteineh. All rights reserved.
 * License (3-Cluase BSD): https://github.com/j3sq/ROS-CHOMP/blob/master/LICENSE
 *
 * **
 * \file ros_chomp.cpp
 *
 * ROS support for chomp path adaptor for Cargo-ANTS project http://cargo-ants.eu/
 *
 *
 */
#include <iostream>
#include <Eigen/Dense>
#include "chomp.hpp"
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <cargo_ants_msgs/Path.h>       //received from path_planner
#include <cargo_ants_msgs/Goal.h>       // member of Path msg
#include <cargo_ants_msgs/ReferenceTrajectory.h>
#include <cargo_ants_msgs/ReferenceTrajectoryPoint.h>
#include <cargo_ants_msgs/ObstacleMap.h>
#include <cargo_ants_msgs/Obstacle.h>

using namespace std;
typedef Eigen::VectorXd Vector;
typedef Eigen::MatrixXd Matrix;
typedef cargo_ants_msgs::ObstacleMap ObstacleMap;
typedef cargo_ants_msgs::ReferenceTrajectory Trajectory;
typedef cargo_ants_msgs::ReferenceTrajectoryPoint TrajectoryPoint;
Trajectory trajectory;
ObstacleMap obstacleMap;
Vector qs(2), qe(2), xi;

Trajectory generateTrajectory(Vector const &xi)
{
	Trajectory trajectory;
	trajectory.dt = 1.0;
	Vector xi_copy(xi.size()+qs.size()+qe.size());
	xi_copy<<qs,xi,qe; //copy xi and add starting point and end point
	vector <TrajectoryPoint> points(xi_copy.size() / 2);
	for (size_t ii = 0; ii < xi_copy.size() / 2; ii++){
		points[ii].xx = xi_copy[ii*2];
		points[ii].yy = xi_copy[ii*2+1];
		//finite backward difference,mind the indicies!
		if (ii>0){
			points[ii].xd = (points[ii].xx-points[ii-1].xx)/trajectory.dt;
			points[ii].yd = (points[ii].yy-points[ii-1].yy)/trajectory.dt;
		}
		if (ii>1){
			points[ii].xdd = (points[ii].xd-points[ii-1].xd)/trajectory.dt;
			points[ii].ydd = (points[ii].yd-points[ii-1].yd)/trajectory.dt;
		}
		//ds
	}
	trajectory.points.insert(trajectory.points.end(), points.begin(), points.end());
	return trajectory;
}
void pathPlannerCallback(const cargo_ants_msgs::Path::ConstPtr &msg)
{
        vector <cargo_ants_msgs::Goal> goals = msg->goals;
        if (goals.size() < 2) {
                ROS_INFO("At least 2 goal points are required, received %lu", goals.size());
                return;
        }
        int n = goals.size();
        qs << goals[0].gx, goals[0].gy;
        qe << goals[n].gx, goals[n].gy;
        xi[0] = goals[0].gx;
        for (size_t ii = 0; ii < goals.size() + 1; ii++) {
                xi[ii * 2 + 1] = goals[ii].gy;
                xi[ii * 2 + 2] = goals[ii + 1].gx;
        }
}

void obstaclesCallback(const cargo_ants_msgs::ObstacleMap::ConstPtr &msg)
{
	obstacleMap.obstacles = msg->obstacles;
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "path_adaptor");
	ros::NodeHandle node;
	ros::Subscriber path_sub = node.subscribe("path_planner", 1000, pathPlannerCallback);
	ros::Subscriber obs_sub = node.subscribe("obstacles", 1000, obstaclesCallback);
	ros::Publisher trajectory_pub = node.advertise<Trajectory> ( "/trajectory", 10);
	ros::Rate loop_rate (10);
	ROS_INFO("path_adaptor Started!");
	while ( ros::ok() ){
		ros::spinOnce();
		Matrix obs(3,obstacleMap.obstacles.size());
		for (size_t ii = 0; ii < obstacleMap.obstacles.size() ; ++ii) {
			obs.col(ii) << obstacleMap.obstacles[ii].origin.ox,
			 								obstacleMap.obstacles[ii].origin.oy,
											obstacleMap.obstacles[ii].origin.oth;
		}
		chomp::generatePath(qs, qe, xi, obs);
		trajectory = generateTrajectory(xi);
		trajectory_pub.publish(trajectory);
		loop_rate.sleep();
}
	return 0;
	// VectorXd qs(2);  //Start goal  coordinates (x,y)
	// VectorXd qe(2);  //End goal  coordinates (x,y)
	// VectorXd xi;     //Trajectory points (x0,y0,x1,y1,....)
	// MatrixXd obs;    //obstacles |x0,y0,R0;x1,y1,R1...|
	//
	// qs << 0, 0;
	// qe << 3, 5;
	//
	//
	// generatePath(qs, qe, xi, obs);
	// cout << xi << std::endl;
}


/*! \file demo_boxplot.cpp
    \brief Demonstration of boxplots.
    \details Contains Quickbook Markup to be included in documentation.

    \author Jacob Voytko and Paul A. Bristow
*/

// Copyright Jacob Voytko 2007
// Copyright Paul A. Bristow 2008, 2012

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

// An example to demonstrate some use of boxplot options.
// See also boxplot_full.cpp for a even wider range of use.

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.

// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

//[demo_boxplot_1

/*`
Boxplot is a  convenient way of graphically depicting groups of numerical data
through their five-number summaries.
Show 1st quartile, median and 3rd quartile as a box,
95% confidence interval as whiskers,
and outliers and extreme outliers.

See [@http://en.wikipedia.org/wiki/Boxplot boxplot] and

Some Implementations of the Boxplot
Michael Frigge, David C. Hoaglin and Boris Iglewicz
The American Statistician, Vol. 43, No. 1 (Feb., 1989), pp. 50-54

First we need a few includes to use Boost.Plot.
*/

#include <vector>
using std::vector;
#include <cmath>
using ::sin;
#include <boost/svg_plot/svg_boxplot.hpp>

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

/*`Use two functions, 1/x and sin(x), to simulate distributions.
*/

double f(double x)
{ // Effectively 1/x.
  return 50 / x;
}

double g(double x)
{ // Effectively sin(x).
  return 40 + 25 * sin(x * 50);
}
//] [demo_boxplot_1]

int main()
{
  using namespace boost::svg;
  try
  {
//[demo_boxplot_2]
/*`10 values are computed and stored in two std:: vectors.
*/
  std::vector<double> data1;
  std::vector<double> data2;

  cout.precision(2);
  for(double i = 0.1; i < 10; i += 0.1)
  {   // Fill our vectors with 100 values:
    double fv = f(i);
    double gv = g(i);
    // cout << i << ' ' << fv << ' ' << gv << endl;
    data1.push_back(fv);
    data2.push_back(gv);
  }

/*`A new boxplot is contructed and several settings added.
*/
  svg_boxplot my_boxplot;

  my_boxplot.background_border_color(darkblue);
  my_boxplot.background_color(azure);

  my_boxplot  // Title and axes labels.
    .title("Boxplots of 1/x and sin(x) Functions")
    .x_label("Functions")
    .y_label("Population Size");

  my_boxplot.y_range(0, 100)  // Y-Axis information.
    .y_minor_tick_length(10)
    .y_major_interval(20);

   // box'n'whiskers options apply to the plot, AND for each data series boxplot
   // so can be set for all boxplots or separately for each plot.
   my_boxplot.plot(data1, "test").box_width(10).whisker_length(5).box_style().fill_color(pink).stroke_color(green);
   // TODO these should be chainable like: .box_fill(pink), box_stroke(green)...
   //my_boxplot.plot(data1, "test").box_width(10).whisker_length(5).median_style().stroke_color(purple);

/*`Add the two data series containers, and their labels, to the plot.
*/

  my_boxplot.plot(data1, "[50 / x]");
  my_boxplot.plot(data2, "[40+25*sin(x*50)]");

/*  cout << "my_boxplot.title " << my_boxplot.title() << endl;
  cout << "my_boxplot.x_label_text "<< my_boxplot.x_label_text() << endl;
  cout << "my_boxplot.y_label_text " << my_boxplot.y_label_text() << endl;

  cout << "my_boxplot.background_color " << my_boxplot.background_color() << endl;
  cout << "my_boxplot.background_border_color " << my_boxplot.background_border_color() << endl;
  cout << "my_boxplot.plot_background_color " << my_boxplot.plot_background_color() << endl;
 cout << "my_boxplot.plot_border_color " << my_boxplot.plot_border_color() << endl;
*/

//`Finally write the SVG plot to a file.
  my_boxplot.write("demo_boxplot.svg");
/*`You can view the plot at demo_boxplot.svg."
*/

//] [demo_boxplot_2]
  }
  catch(const std::exception& e)
  {
    std::cout <<
      "\n""Message from thrown exception was:\n  " << e.what() << std::endl;
  }
  return 0;
} // int main()

/*

Output:

Compiling...
demo_boxplot.cpp
Linking...
Embedding manifest...
Autorun "j:\Cpp\SVG\debug\demo_boxplot.exe"
Build Time 0:02

*/