#pragma once

#include <cstdint>
#include <Eigen/Dense>

/** @class RobotEstimator
 *  This class implements a Kalman Filter in order to estimate a robot's current state.
 *  The estimated state is then used in motion control to assign wheel velocities accordingly
 *
 *  Explanations:
 *  - https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
 *  - https://en.wikipedia.org/wiki/Kalman_filter
 *  - https://www.mathworks.com/videos/series/understanding-kalman-filters.html
 */
class RobotEstimator {
private:
    /**
     * Number of tracked states (number of states in state vector, x_hat)
     *
     * 3 Total:
     * - X and Y: linear velocities of robot body (m/s)
     * - W: angular velocity of robot body around z axis (rad/s)
     */
    static constexpr int numStates = 3;

    /**
     * Number of applied inputs (number of control inputs in control vector, u)
     *
     * 4 total:
     * - Motor duty cycle for each motor (% max), controls average applied voltage
     */
    static constexpr int numInputs = 4;

    /**
     * Number of tracked measurable outputs (number of outputs in output vector, y)
     *
     * 5 total:
     * - 4 wheel motor encoders (rad/s)
     * - Gyro angular velocity (rad/s)
     */
    static constexpr int numOutputs = 5;

public:
    /**
     * @param dt_us Expected period of the controller in us
     */
    RobotEstimator(uint32_t dt_us);

    /**
     * Using the previous state and the next input
     * We can guess where we are this time step
     *
     * @param u Last motor command
     */
    void predict(Eigen::Matrix<float, numInputs, 1> u);

    /**
     * Using the next measurements, we can move our prediction
     * closer to the true target
     *
     * @param z Encoders 1-4 then gyro
     */
    void update(Eigen::Matrix<float, numOutputs, 1> z);

    /**
     * @param state Matrix that the current guess will be saved into
     */
    void getState(Eigen::Matrix<float, numStates, 1>& state);

private:

    static constexpr float processNoise = 0.05;  /**< State noise from unmodeled influences (model errors/omissions) */
    static constexpr float encoderNoise = 0.04;  /**< Measurement noise in the encoder */
    static constexpr float gyroNoise = 0.005;    /**< Measurement noise in the gyro */
    static constexpr float initCovariance = 0.1; /**< The initial covariance value in each entry of `P` */

    /**
     * State Transition Matrix/Prediction Matrix
     *
     * Predict our next state based on our current state using dynamics
     */
    Eigen::Matrix<float, numStates,  numStates>  F;

    /**
     * Control Input Matrix
     *
     * Corrects our next state prediction by mapping control input (known influences, `u`) to our next state
     */
    Eigen::Matrix<float, numStates,  numInputs>  B;

    /**
     * Observation Matrix
     *
     * Predicts the measurements from our sensors (`z`) from our current state (`x_hat`).
     */
    Eigen::Matrix<float, numOutputs, numStates>  H;

    /**
     * Covariance Matrix for Process Noise
     *
     * A matrix whose entries are the estimated covariances between any two state variables.
     * Entries represent uncertainty in our state vector (`x_hat`) due to unmodeled influences.
     */
    Eigen::Matrix<float, numStates,  numStates>  Q;

    /**
     * Covariance Matrix for Observation Noise
     *
     * A matrix whose entries are the estimated covariances between any two state variables.
     * Entries represent uncertainty in our measurements (`z`) due to random noise
     */
    Eigen::Matrix<float, numOutputs, numOutputs> R;

    /**
     * Covariance Estimate Matrix
     *
     * A matrix whose entries are the estimated covariances between any two state variables.
     * Entries represent our overall uncertainty in our state vector (`x_hat`).
     */
    Eigen::Matrix<float, numStates,  numStates>  P;

    /**
     * Identity Matrix
     *
     * A square matrix with only 1s on the diagonals
     */
    Eigen::Matrix<float, numStates, numStates> I;

    /**
     * Current State Vector Estimate
     *
     * What the robot estimator believes our current state to be
     */
    Eigen::Matrix<float, numStates, 1> x_hat;
};


/**
 *          Copyright Matthias Walter 2010.
 * Distributed under the Boost Software License, Version 1.0.
 *    (See accompanying file LICENSE_1_0.txt or copy at
 *          http://www.boost.org/LICENSE_1_0.txt)
 **/

#include "../config.h"
#include <map>
#include <vector>

#include <boost/numeric/ublas/lu.hpp>
#include <boost/dynamic_bitset.hpp>

#include "total_unimodularity.hpp"
#include "combinations.hpp"

namespace unimod
{

  /**
   * Calculates a subdeterminant of the given matrix.
   *
   * @param matrix A given integer matrix
   * @param submatrix Matrix-indices describing a submatrix
   * @return The submatrix' determinant
   */

  int submatrix_determinant(const integer_matrix& matrix, const submatrix_indices& submatrix)
  {
    typedef boost::numeric::ublas::matrix_indirect <const integer_matrix, submatrix_indices::indirect_array_type> indirect_matrix_t;

    assert (submatrix.rows.size() == submatrix.columns.size());

    const indirect_matrix_t indirect_matrix(matrix, submatrix.rows, submatrix.columns);

    boost::numeric::ublas::matrix <float> float_matrix(indirect_matrix);
    boost::numeric::ublas::permutation_matrix <size_t> permutation_matrix(float_matrix.size1());

    /// LU-factorize
    int result = boost::numeric::ublas::lu_factorize(float_matrix, permutation_matrix);
    if (result != 0)
      return 0;

    /// Calculate the determinant as a product of the diagonal entries, taking the permutation matrix into account.
    float det = 1.0f;
    for (size_t i = 0; i < float_matrix.size1(); ++i)
    {
      det *= float_matrix(i, i);
      if (i != permutation_matrix(i))
        det = -det;
    }

    return (int) det;
  }

  /**
   * Checks Camion's criterion for total unimodularity.
   *
   * @param matrix A given integer matrix
   * @param submatrix Matrix-indices describing a submatrix B of A
   * @return true iff B is not Eulerian or 1^T * B * 1 = 0 (mod 4)
   */

  bool submatrix_camion(const integer_matrix& matrix, const submatrix_indices& submatrix)
  {
    typedef boost::numeric::ublas::matrix_indirect <const integer_matrix, submatrix_indices::indirect_array_type> indirect_matrix_t;

    assert (submatrix.rows.size() == submatrix.columns.size());

    const indirect_matrix_t indirect_matrix(matrix, submatrix.rows, submatrix.columns);

    // Test whether submatrix is eulerian.
    for (size_t row = 0; row < indirect_matrix.size1(); ++row)
    {
      size_t count = 0;
      for (size_t column = 0; column < indirect_matrix.size2(); ++column)
        count += indirect_matrix(row, column);
      if (count % 2 == 1)
        return true;
    }
    for (size_t column = 0; column < indirect_matrix.size2(); ++column)
    {
      size_t count = 0;
      for (size_t row = 0; row < indirect_matrix.size1(); ++row)
        count += indirect_matrix(row, column);
      if (count % 2 == 1)
        return true;
    }

    // Matrix is eulerian.
    size_t count = 0;
    for (size_t row = 0; row < indirect_matrix.size1(); ++row)
    {
      for (size_t column = 0; column < indirect_matrix.size2(); ++column)
        count += indirect_matrix(row, column);
    }
    return (count % 4 == 0);
  }

  /**
   * Checks all subdeterminants to test a given matrix for total unimodularity.
   *
   * @param matrix The given matrix
   * @return true if and only if this matrix is totally unimdular
   */

  bool determinant_is_totally_unimodular(const integer_matrix& matrix)
  {
    submatrix_indices indices;

    return determinant_is_totally_unimodular(matrix, indices);
  }

  /**
   * Checks all subdeterminants to test a given matrix for total unimodularity.
   * If this is not the case, violator describes a violating submatrix.
   *
   * @param matrix The given matrix
   * @param violator The violating submatrix, if the result is false
   * @return true if and only if the this matrix is totally unimodular
   */

  bool determinant_is_totally_unimodular(const integer_matrix& matrix, submatrix_indices& violator)
  {
    for (size_t size = 1; size <= matrix.size1(); ++size)
    {
      combination row_combination(matrix.size1(), size);
      while (true)
      {
        combination column_combination(matrix.size2(), size);
        while (true)
        {
          submatrix_indices sub;
          submatrix_indices::vector_type indirect_array(size);
          for (size_t i = 0; i < size; ++i)
            indirect_array[i] = row_combination[i];
          sub.rows = submatrix_indices::indirect_array_type(size, indirect_array);
          for (size_t i = 0; i < size; ++i)
          {
            indirect_array[i] = column_combination[i];
          }
          sub.columns = submatrix_indices::indirect_array_type(size, indirect_array);

          /// Examine the determinant
          bool camion = submatrix_camion(matrix, sub);

#ifndef NDEBUG
          int det = submatrix_determinant(matrix, sub);
          bool is_violator = det < -1 || det > +1;
          assert((is_violator && !camion) || (!is_violator && camion));
#pragma message("\n\n\nWARNING: Submatrix Test uses subdeterminants for verification and is much slower!\n\n ")
#endif
          if (!camion)
          {
            violator = sub;
            return false;
          }

          if (column_combination.is_last())
            break;
          column_combination.next();
        }
        if (row_combination.is_last())
          break;
        row_combination.next();
      }
    }
    return true;
  }

}


/*=============================================================================

  SKSURGERYOPENCVCPP: Image-guided surgery functions, in C++, using OpenCV.

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

=============================================================================*/

#include "sksTriangulate.h"
#include "sksExceptionMacro.h"

#include <opencv2/calib3d.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <iostream>

namespace sks
{

//-----------------------------------------------------------------------------
double Norm(const cv::Point3d& p1)
{
  return std::sqrt(p1.x * p1.x + p1.y * p1.y + p1.z * p1.z);
}

//-----------------------------------------------------------------------------
cv::Point3d CrossProduct(const cv::Point3d& p1, const cv::Point3d& p2)
{
  cv::Point3d cp;
  cp.x = p1.y * p2.z - p1.z * p2.y;
  cp.y = p1.z * p2.x - p1.x * p2.z;
  cp.z = p1.x * p2.y - p1.y * p2.x;
  return cp;
}

//-----------------------------------------------------------------------------
double DotProduct(const cv::Point3d& p1, const cv::Point3d& p2)
{
  return p1.x * p2.x + p1.y * p2.y + p1.z * p2.z;
}

//-----------------------------------------------------------------------------
double DistanceToLine(const std::pair<cv::Point3d, cv::Point3d>& line, const cv::Point3d& x0 )
{
  // Courtesy of Wolfram Mathworld
  cv::Point3d x1;
  cv::Point3d x2;

  x1 = line.first;
  x2 = line.second;

  cv::Point3d d1 = x1-x0;
  cv::Point3d d2 = x2-x1;

  return sks::Norm(sks::CrossProduct(d2, d1)) / (sks::Norm(d2));
}

//-----------------------------------------------------------------------------
double DistanceBetweenLines(const cv::Point3d& P0, const cv::Point3d& u,
                            const cv::Point3d& Q0, const cv::Point3d& v,
                            cv::Point3d& midpoint)
{
  // Method 1. Solve for shortest line joining two rays, then get midpoint.
  // Taken from: http://geomalgorithms.com/a07-_distance.html
  double sc, tc, a, b, c, d, e;
  double distance;

  cv::Point3d Psc;
  cv::Point3d Qtc;
  cv::Point3d W0;

  // Difference of two origins
  W0.x = P0.x - Q0.x;
  W0.y = P0.y - Q0.y;
  W0.z = P0.z - Q0.z;

  a = u.x*u.x + u.y*u.y + u.z*u.z;
  b = u.x*v.x + u.y*v.y + u.z*v.z;
  c = v.x*v.x + v.y*v.y + v.z*v.z;
  d = u.x*W0.x + u.y*W0.y + u.z*W0.z;
  e = v.x*W0.x + v.y*W0.y + v.z*W0.z;
  sc = (b*e - c*d) / (a*c - b*b);
  tc = (a*e - b*d) / (a*c - b*b);

  if ( boost::math::isnan(sc) || boost::math::isnan(tc) || boost::math::isinf(sc) || boost::math::isinf(tc) )
  {
    // Lines are parallel
    distance = sks::DistanceToLine ( std::pair<cv::Point3d, cv::Point3d> ( P0, P0 + u ), Q0 );
    midpoint.x = std::numeric_limits<double>::quiet_NaN();
    midpoint.y = std::numeric_limits<double>::quiet_NaN();
    midpoint.z = std::numeric_limits<double>::quiet_NaN();
    return distance;
  }
  Psc.x = P0.x + sc*u.x;
  Psc.y = P0.y + sc*u.y;
  Psc.z = P0.z + sc*u.z;
  Qtc.x = Q0.x + tc*v.x;
  Qtc.y = Q0.y + tc*v.y;
  Qtc.z = Q0.z + tc*v.z;

  distance = std::sqrt((Psc.x - Qtc.x)*(Psc.x - Qtc.x)
                        +(Psc.y - Qtc.y)*(Psc.y - Qtc.y)
                        +(Psc.z - Qtc.z)*(Psc.z - Qtc.z));

  midpoint.x = (Psc.x + Qtc.x)/2.0;
  midpoint.y = (Psc.y + Qtc.y)/2.0;
  midpoint.z = (Psc.z + Qtc.z)/2.0;

  return distance;
}

//-----------------------------------------------------------------------------
double ComputeRMSBetweenCorrespondingPoints(const cv::Mat& a, const cv::Mat& b)
{
  double rms = 0;
  double diff = 0;
  double squaredDiff = 0;

  if (a.rows != b.rows)
  {
    sksExceptionThrow() << "a has " << a.rows << " rows, but b has " << b.rows;
  }

  if (a.cols != 3)
  {
    sksExceptionThrow() << "a does not have 3 columns.";
  }

  if (b.cols != 3)
  {
    sksExceptionThrow() << "b does not have 3 columns.";
  }

  for (unsigned int r = 0; r < a.rows; r++)
  {
    for (unsigned int c = 0; c < a.cols; c++)
    {
      diff = (b.at<double>(r, c) - a.at<double>(r, c));
      squaredDiff = diff * diff;
      rms += squaredDiff;
    }
  }
  rms /= static_cast<double>(a.rows);
  rms = std::sqrt(rms);
  return rms;
}

} // end namespace


/*
Copyright 2020 Standard Cyborg

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/
#include "standard_cyborg/algorithms/PointToPointAlignment.hpp"

#include "standard_cyborg/util/IncludeEigen.hpp"

#include "standard_cyborg/math/Vec3.hpp"
#include "standard_cyborg/math/Mat3x4.hpp"

#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wdocumentation"
#include <Eigen/Jacobi>
#pragma clang diagnostic pop

namespace standard_cyborg {

namespace algorithms {

standard_cyborg::math::Mat3x4 PointToPointAlignment(const std::vector<standard_cyborg::math::Vec3>& sourcePositions, const std::vector<standard_cyborg::math::Vec3>& targetPositions)
{
    int N = (int)sourcePositions.size();

    Eigen::Matrix3Xf p = Eigen::Matrix3Xf(3, N);
    Eigen::Matrix3Xf q = Eigen::Matrix3Xf(3, N);
    Eigen::MatrixXf avgP, avgQ, pNorm, qNorm, C, R, t;

    for (int iv = 0; iv < N; ++iv) {
        p.col(iv) = Eigen::Vector3f(sourcePositions[iv].x, sourcePositions[iv].y, sourcePositions[iv].z);
        q.col(iv) = Eigen::Vector3f(targetPositions[iv].x, targetPositions[iv].y, targetPositions[iv].z);
    }

    avgP = p.rowwise().mean();
    avgQ = q.rowwise().mean();

    pNorm = p - avgP.replicate(1, p.cols());
    qNorm = q - avgQ.replicate(1, q.cols());

    C = pNorm * qNorm.transpose();
    Eigen::JacobiSVD<Eigen::MatrixXf> svd(C, Eigen::ComputeThinU | Eigen::ComputeThinV);

    // rotation
    R = svd.matrixV() * svd.matrixU().transpose();

    // translation
    t = avgQ - R * avgP;

    standard_cyborg::math::Mat3x4 r = {
        R(0, 0), R(0, 1), R(0, 2), t(0),
        R(1, 0), R(1, 1), R(1, 2), t(1),
        R(2, 0), R(2, 1), R(2, 2), t(2),
    };

    return r;
}

}

}


/**
 * \file Chebyshev1Filter.cpp
 */

#include <boost/math/special_functions/asinh.hpp>

#include "Chebyshev1Filter.h"
#include "helpers.h"
#include "IIRFilter.h"

namespace
{
  template<typename DataType>
  void create_chebyshev1_analog_coefficients(int order, DataType ripple, std::vector<std::complex<DataType> >& z, std::vector<std::complex<DataType> >& p, DataType& k)
  {
    z.clear(); // no zeros for this filter type
    p.clear();
    if(ripple == 0)
    {
      return;
    }
    if(order == 0)
    {
      k = static_cast<DataType>(std::pow(10, (-ripple / 20)));
      return;
    }

    DataType eps = static_cast<DataType>(std::sqrt(std::pow(10, (0.1 * ripple)) - 1.0));
    DataType mu = static_cast<DataType>(1.0 / order * boost::math::asinh(1 / eps));

    for(int i = -order+1; i < order; i += 2)
    {
      DataType theta = boost::math::constants::pi<DataType>() * i / (2*order);
      p.push_back(-std::sinh(std::complex<DataType>(mu, theta)));
    }

    std::complex<DataType> f = 1;

    for(int i = 0; i < p.size(); ++i)
    {
      f *= -p[i];
    }
    k = f.real();
    if(order % 2 == 0)
    {
      k = k / std::sqrt((1 + eps * eps));
    }
  }

  template<typename DataType>
  void create_default_chebyshev1_coeffs(size_t order, DataType ripple, DataType Wn, std::vector<DataType>& coefficients_in, std::vector<DataType>& coefficients_out)
  {
    std::vector<std::complex<DataType> > z;
    std::vector<std::complex<DataType> > p;
    DataType k;

    int fs = 2;
    create_chebyshev1_analog_coefficients(static_cast<int>(order), ripple, z, p, k);
    DataType warped = 2 * fs * std::tan(boost::math::constants::pi<DataType>() *  Wn / fs);
    zpk_lp2lp(warped, z, p, k);
    zpk_bilinear(fs, z, p, k);

    boost::math::tools::polynomial<DataType> b;
    boost::math::tools::polynomial<DataType> a;

    zpk2ba(fs, z, p, k, b, a);

    for(size_t i = 0; i < std::min(order + 1, b.size()); ++i)
    {
      coefficients_in[i] = b[i];
    }
    for(size_t i = 0; i < std::min(order, a.size()-1); ++i)
    {
      coefficients_out[i] = -a[i];
    }
  }

  template<typename DataType>
  void create_bp_chebyshev1_coeffs(size_t order, DataType ripple, DataType wc1, DataType wc2, std::vector<DataType>& coefficients_in, std::vector<DataType>& coefficients_out)
  {
    std::vector<std::complex<DataType> > z;
    std::vector<std::complex<DataType> > p;
    DataType k;

    int fs = 2;
    create_chebyshev1_analog_coefficients(static_cast<int>(order/2), ripple, z, p, k);
    wc1 = 2 * fs * std::tan(boost::math::constants::pi<DataType>() * wc1 / fs);
    wc2 = 2 * fs * std::tan(boost::math::constants::pi<DataType>() * wc2 / fs);

    zpk_lp2bp(std::sqrt(wc1 * wc2), wc2 - wc1, z, p, k);
    zpk_bilinear(fs, z, p, k);

    boost::math::tools::polynomial<DataType> b;
    boost::math::tools::polynomial<DataType> a;

    zpk2ba(fs, z, p, k, b, a);

    for(size_t i = 0; i < std::min(order + 1, b.size()); ++i)
    {
      coefficients_in[i] = b[i];
    }
    for(size_t i = 0; i < std::min(order, a.size()-1); ++i)
    {
      coefficients_out[i] = -a[i];
    }
  }

  template<typename DataType>
  void create_bs_chebyshev1_coeffs(size_t order, DataType ripple, DataType wc1, DataType wc2, std::vector<DataType>& coefficients_in, std::vector<DataType>& coefficients_out)
  {
    std::vector<std::complex<DataType> > z;
    std::vector<std::complex<DataType> > p;
    DataType k;

    int fs = 2;
    create_chebyshev1_analog_coefficients(static_cast<int>(order/2), ripple, z, p, k);
    wc1 = 2 * fs * std::tan(boost::math::constants::pi<DataType>() * wc1 / fs);
    wc2 = 2 * fs * std::tan(boost::math::constants::pi<DataType>() * wc2 / fs);

    zpk_lp2bs(std::sqrt(wc1 * wc2), wc2 - wc1, z, p, k);
    zpk_bilinear(fs, z, p, k);

    boost::math::tools::polynomial<DataType> b;
    boost::math::tools::polynomial<DataType> a;

    zpk2ba(fs, z, p, k, b, a);

    for(size_t i = 0; i < std::min(order + 1, b.size()); ++i)
    {
      coefficients_in[i] = b[i];
    }
    for(size_t i = 0; i < std::min(order, a.size()-1); ++i)
    {
      coefficients_out[i] = -a[i];
    }
  }
}

namespace ATK
{
  template <typename DataType>
  Chebyshev1LowPassCoefficients<DataType>::Chebyshev1LowPassCoefficients(int nb_channels)
  :Parent(nb_channels, nb_channels), cut_frequency(0), ripple(0), in_order(1), out_order(1)
  {
  }

  template <typename DataType_>
  void Chebyshev1LowPassCoefficients<DataType_>::set_ripple(DataType_ ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev1LowPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType_>
  void Chebyshev1LowPassCoefficients<DataType_>::set_cut_frequency(DataType_ cut_frequency)
  {
    this->cut_frequency = cut_frequency;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev1LowPassCoefficients<DataType_>::get_cut_frequency() const
  {
    return cut_frequency;
  }

  template <typename DataType>
  void Chebyshev1LowPassCoefficients<DataType>::set_order(int order)
  {
    in_order = out_order = order;
    setup();
  }

  template <typename DataType>
  void Chebyshev1LowPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    create_default_chebyshev1_coeffs(in_order, ripple, 2 * cut_frequency / input_sampling_rate, coefficients_in, coefficients_out);
  }

  template <typename DataType>
  Chebyshev1HighPassCoefficients<DataType>::Chebyshev1HighPassCoefficients(int nb_channels)
  :Parent(nb_channels, nb_channels), cut_frequency(0), ripple(0), in_order(1), out_order(1)
  {
  }

  template <typename DataType_>
  void Chebyshev1HighPassCoefficients<DataType_>::set_cut_frequency(DataType_ cut_frequency)
  {
    this->cut_frequency = cut_frequency;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev1HighPassCoefficients<DataType_>::get_cut_frequency() const
  {
    return cut_frequency;
  }

  template <typename DataType_>
  void Chebyshev1HighPassCoefficients<DataType_>::set_ripple(DataType_ ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev1HighPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev1HighPassCoefficients<DataType>::set_order(int order)
  {
    in_order = out_order = order;
    setup();
  }

  template <typename DataType>
  void Chebyshev1HighPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    create_default_chebyshev1_coeffs(in_order, ripple, (input_sampling_rate - 2 * cut_frequency) / input_sampling_rate, coefficients_in, coefficients_out);
    for(int i = in_order - 1; i >= 0; i -= 2)
    {
      coefficients_in[i] = - coefficients_in[i];
      coefficients_out[i] = - coefficients_out[i];
    }
  }

  template <typename DataType>
  Chebyshev1BandPassCoefficients<DataType>::Chebyshev1BandPassCoefficients(int nb_channels)
  :Parent(nb_channels, nb_channels), cut_frequencies(0, 0), ripple(0), in_order(1), out_order(1)
  {
  }

  template <typename DataType_>
  void Chebyshev1BandPassCoefficients<DataType_>::set_cut_frequencies(std::pair<DataType_, DataType_> cut_frequencies)
  {
    this->cut_frequencies = cut_frequencies;
    setup();
  }

  template <typename DataType_>
  void Chebyshev1BandPassCoefficients<DataType_>::set_cut_frequencies(DataType_ f0, DataType_ f1)
  {
    this->cut_frequencies = std::make_pair(f0, f1);
    setup();
  }

  template <typename DataType_>
  std::pair<DataType_, DataType_> Chebyshev1BandPassCoefficients<DataType_>::get_cut_frequencies() const
  {
    return cut_frequencies;
  }

  template <typename DataType_>
  void Chebyshev1BandPassCoefficients<DataType_>::set_ripple(DataType_ ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev1BandPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev1BandPassCoefficients<DataType>::set_order(int order)
  {
    in_order = out_order = 2 * order;
    setup();
  }

  template <typename DataType>
  void Chebyshev1BandPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    create_bp_chebyshev1_coeffs(in_order, ripple, 2 * cut_frequencies.first / input_sampling_rate, 2 * cut_frequencies.second / input_sampling_rate, coefficients_in, coefficients_out);
  }

  template <typename DataType>
  Chebyshev1BandStopCoefficients<DataType>::Chebyshev1BandStopCoefficients(int nb_channels)
  :Parent(nb_channels, nb_channels), cut_frequencies(0, 0), ripple(0), in_order(1), out_order(1)
  {
  }

  template <typename DataType_>
  void Chebyshev1BandStopCoefficients<DataType_>::set_cut_frequencies(std::pair<DataType_, DataType_> cut_frequencies)
  {
    this->cut_frequencies = cut_frequencies;
    setup();
  }

  template <typename DataType_>
  void Chebyshev1BandStopCoefficients<DataType_>::set_cut_frequencies(DataType_ f0, DataType_ f1)
  {
    this->cut_frequencies = std::make_pair(f0, f1);
    setup();
  }

  template <typename DataType_>
  std::pair<DataType_, DataType_> Chebyshev1BandStopCoefficients<DataType_>::get_cut_frequencies() const
  {
    return cut_frequencies;
  }

  template <typename DataType_>
  void Chebyshev1BandStopCoefficients<DataType_>::set_ripple(DataType_ ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  DataType_ Chebyshev1BandStopCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev1BandStopCoefficients<DataType>::set_order(int order)
  {
    in_order = out_order = 2 * order;
    setup();
  }

  template <typename DataType>
  void Chebyshev1BandStopCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    create_bs_chebyshev1_coeffs(in_order, ripple, 2 * cut_frequencies.first / input_sampling_rate, 2 * cut_frequencies.second / input_sampling_rate, coefficients_in, coefficients_out);
  }

  template class Chebyshev1LowPassCoefficients<float>;
  template class Chebyshev1LowPassCoefficients<double>;
  template class Chebyshev1HighPassCoefficients<float>;
  template class Chebyshev1HighPassCoefficients<double>;
  template class Chebyshev1BandPassCoefficients<float>;
  template class Chebyshev1BandPassCoefficients<double>;
  template class Chebyshev1BandStopCoefficients<float>;
  template class Chebyshev1BandStopCoefficients<double>;

  template class IIRFilter<Chebyshev1LowPassCoefficients<float> >;
  template class IIRFilter<Chebyshev1LowPassCoefficients<double> >;
  template class IIRFilter<Chebyshev1HighPassCoefficients<float> >;
  template class IIRFilter<Chebyshev1HighPassCoefficients<double> >;
  template class IIRFilter<Chebyshev1BandPassCoefficients<float> >;
  template class IIRFilter<Chebyshev1BandPassCoefficients<double> >;
  template class IIRFilter<Chebyshev1BandStopCoefficients<float> >;
  template class IIRFilter<Chebyshev1BandStopCoefficients<double> >;

}


#include "ematrix.h"

#include "defines.h"
#include "fmatrix.h"
#include "triangulation.h"

#include <Eigen/SVD>
#include <Eigen/Dense>
#include <iostream>

namespace {

    // essential matrix must have exactly two equal non zero singular values
    void ensureSpectralProperty(matrix3d &Ecv)
    {
        Eigen::MatrixXd E;
        copy(Ecv, E);

        Eigen::JacobiSVD<Eigen::MatrixXd> svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV);

        Eigen::MatrixXd U = svd.matrixU();
        Eigen::VectorXd s = svd.singularValues();
        Eigen::MatrixXd V = svd.matrixV();

        Eigen::MatrixXd S = Eigen::MatrixXd(3, 3);
        S.setZero();

        S(0, 0) = 1.0;
        S(1, 1) = 1.0;
        S(2, 2) = 0.0;

        E = U * S * V.transpose();

        copy(E, Ecv);
    }

}

cv::Matx33d phg::fmatrix2ematrix(const cv::Matx33d &F, const phg::Calibration &calib0, const phg::Calibration &calib1)
{
    // TODO check that correct conversion (transpose ok)

    // TODO add separate test for fmatrix (check 1) reprojection errors 2) f matrix singular value property)
    // TODO add separate test for ematrix (check 1) reprojection errors of normalized coordinates 2) e matrix singular value property)
    // TODO add separate test for ematrix decomposition: 1) check angle between cameras 2) check translation direction between cameras

    matrix3d E = calib1.K().t() * F * calib0.K();

    ensureSpectralProperty(E);

    return E;
}

namespace {

    matrix34d composeP(const Eigen::MatrixXd &R, const Eigen::VectorXd &t)
    {
        matrix34d result;

        result(0, 0) = R(0, 0);
        result(0, 1) = R(0, 1);
        result(0, 2) = R(0, 2);
        result(1, 0) = R(1, 0);
        result(1, 1) = R(1, 1);
        result(1, 2) = R(1, 2);
        result(2, 0) = R(2, 0);
        result(2, 1) = R(2, 1);
        result(2, 2) = R(2, 2);

        result(0, 3) = t[0];
        result(1, 3) = t[1];
        result(2, 3) = t[2];

        return result;
    }

    double getDepth(const vector2d &m0, const vector2d &m1, const phg::Calibration &calib0, const phg::Calibration &calib1, const matrix34d &P0, const matrix34d &P1)
    {
        vector3d p0 = calib0.unproject(m0);
        vector3d p1 = calib1.unproject(m1);

        vector3d ps[2] = {p0, p1};
        matrix34d Ps[2] = {P0, P1};

        vector4d X = phg::triangulatePoint(Ps, ps, 2);
        if (X[3] != 0) {
            X /= X[3];
        }

        return p0.dot(P0 * X) > 0 && p1.dot(P1 * X) > 0;
    }
}

// Матрицы камер для фундаментальной матрицы определены с точностью до проективного преобразования
// То есть, можно исказить трехмерный мир (применив 4-мерную однородную матрицу), и одновременно поменять матрицы P0, P1 так, что проекции в пикселях не изменятся
// Если мы знаем калибровки камер (матрицы K0, K1 в структуре матриц P0, P1), то можем наложить дополнительные ограничения, в частности, известно, что
// существенная матрица (Essential matrix = K1t * F * K0) имеет ровно два совпадающих ненулевых сингулярных значения, тогда как для фундаментальной матрицы они могут различаться
// Это дополнительное ограничение позволяет разложить существенную матрицу с точностью до 4 решений, вместо произвольного проективного преобразования (см. Hartley & Zisserman p.258)
// Обычно мы можем использовать одну общую калибровку, более менее верную для большого количества реальных камер и с ее помощью выполнить
// первичное разложение существенной матрицы (а из него, взаимное расположение камер) для последующего уточнения методом нелинейной оптимизации
void phg::decomposeEMatrix(cv::Matx34d &P0, cv::Matx34d &P1, const cv::Matx33d &Ecv, const std::vector<cv::Vec2d> &m0, const std::vector<cv::Vec2d> &m1, const Calibration &calib0, const Calibration &calib1, bool verbose)
{
    if (m0.size() != m1.size()) {
        throw std::runtime_error("decomposeEMatrix : m0.size() != m1.size()");
    }

    using mat = Eigen::MatrixXd;
    using vec = Eigen::VectorXd;

    mat E;
    copy(Ecv, E);

    // (см. Hartley & Zisserman p.258)

    Eigen::JacobiSVD<mat> svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV);

    mat U = svd.matrixU();
    vec s = svd.singularValues();
    mat V = svd.matrixV();

    // U, V must be rotation matrices, not just orthogonal
    if (U.determinant() < 0) U = -U;
    if (V.determinant() < 0) V = -V;

    if (verbose) {
        std::cout << "U:\n" << U << std::endl;
        std::cout << "s:\n" << s << std::endl;
        std::cout << "V:\n" << V << std::endl;
    }

    mat W(3, 3);
    W << 0, -1, 0, 1, 0, 0, 0, 0, 1;

    mat Z(3, 3);
    Z << 0, 1, 0, -1, 0, 0, 0, 0, 0;

    if (verbose) {
        std::cout << "W:\n" << W << std::endl;
        std::cout << "Z:\n" << Z << std::endl;
    }

    mat R0 = U * W * V.transpose();
    mat R1 = U * W.transpose() * V.transpose();

    if (verbose) {
        std::cout << "R0:\n" << R0 << std::endl;
        std::cout << "R1:\n" << R1 << std::endl;
    }

    vec t0 = U.col(2);
    vec t1 = -t0;

    if (verbose) {
        std::cout << "t0:\n" << t0 << std::endl;
    }

    P0 = matrix34d::eye();

    // 4 possible solutions
    matrix34d P10 = composeP(R0, t0);
    matrix34d P11 = composeP(R0, t1);
    matrix34d P12 = composeP(R1, t0);
    matrix34d P13 = composeP(R1, t1);
    matrix34d P1s[4] = {P10, P11, P12, P13};

    // need to select best of 4 solutions: 3d points should be in front of cameras (positive depths)
    int best_count = 0;
    int best_idx = -1;
    for (int i = 0; i < 4; ++i) {
        int count = 0;
        for (int j = 0; j < (int) m0.size(); ++j) {
            if (getDepth(m0[j], m1[j], calib0, calib1, P0, P1s[i]) > 0) {
                ++count;
            }
        }
        if (verbose) {
            std::cout << "decomposeEMatrix: count: " << count << std::endl;
        }
        if (count > best_count) {
            best_count = count;
            best_idx = i;
        }
    }

    if (best_count == 0) {
        throw std::runtime_error("decomposeEMatrix : can't decompose ematrix");
    }

    P1 = P1s[best_idx];

    if (verbose) {
        std::cout << "best idx: " << best_idx << std::endl;
        std::cout << "P0: \n" << P0 << std::endl;
        std::cout << "P1: \n" << P1 << std::endl;
    }
}

void phg::decomposeUndistortedPMatrix(cv::Matx33d &R, cv::Vec3d &O, const cv::Matx34d &P)
{
    R = P.get_minor<3, 3>(0, 0);

    cv::Matx31d O_mat = -R.t() * P.get_minor<3, 1>(0, 3);
    O(0) = O_mat(0);
    O(1) = O_mat(1);
    O(2) = O_mat(2);

    if (cv::determinant(R) < 0) {
        R *= -1;
    }
}

cv::Matx33d phg::composeEMatrixRT(const cv::Matx33d &R, const cv::Vec3d &T)
{
    return skew(T) * R;
}

cv::Matx34d phg::composeCameraMatrixRO(const cv::Matx33d &R, const cv::Vec3d &O)
{
    vector3d T = -R * O;
    return make34(R, T);
}


#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

int main()
{
  ArrayXXf m(2,2);
  ArrayXXf n(2,2);

  ArrayXXf result(2,2);

  //initialize arrays
  m << 1,2,
       3,4;

  n << 5,6,
       7,8;

  // --> array multiplication
  result = m * n;

  cout << "-- Array m*n: --" << endl
    << result << endl << endl;

  // --> Matrix multiplication
  result = m.matrix() * n.matrix();

  cout << "-- Matrix m*n: --" << endl
    << result << endl << endl;
}


#include <iostream>
#include <chrono>
#include <Eigen/Dense>
#include "elements.h"

using Eigen::Vector3d;

namespace elements
{
    VectorXd elements(Vector3d r, Vector3d v, double mu)
    {
        auto r_mag = r.norm();
        auto v_mag = v.norm();
        auto v_mag2 = v_mag * v_mag;
        auto h = r.cross(v);
        auto h_mag = h.norm();
        Vector3d k(0,0,1);
        auto n = k.cross(h);
        auto n_mag = n.norm();
        auto xi = v_mag2/2 - mu/r_mag;
        auto e = ((v_mag2-mu/r_mag)*r - v*r.dot(v))/mu;
        auto ecc = e.norm();
        double sma;
        if (ecc != 1) {
            sma = -mu/(2*xi);
        } else {
            sma = pow(h_mag,2)/mu;
        }
        auto inc = acos(h.z()/h_mag);
        auto node = acos(n.x()/n_mag);
        auto peri = acos(n.dot(e)/(ecc*n_mag));
        auto ano = acos(e.dot(r)/(ecc*r_mag));
        if (n.y() < 0) {
            node = M_PI*2 - node;
        }
        if (e.z() < 0) {
            peri = M_PI*2 - peri;
        }
        if (r.dot(v) < 0) {
            ano = M_PI*2 - ano;
        }
        VectorXd out(6); out << sma, ecc, inc, node, peri, ano;
        return out;
    }

    void benchmark(int times) {
        auto mu = 3.986004418e5;
        Vector3d r(8.59072560e+02, -4.13720368e+03, 5.29556871e+03);
        Vector3d v(7.37289205e+00, 2.08223573e+00, 4.39999794e-01);

        auto best = std::numeric_limits<double>::infinity();
        auto worst = -std::numeric_limits<double>::infinity();
        double all = 0;
        for (auto i=0; i < times; i++) {
            auto begin = std::chrono::high_resolution_clock::now();

            elements(r, v, mu);

            auto end = std::chrono::high_resolution_clock::now();
            auto current = std::chrono::duration_cast<std::chrono::nanoseconds>(end-begin).count()/1e9;
            all += current;
            if (current < best) {
                best = current;
            }
            if (current > worst) {
                worst = current;
            }
        }
        std::cout << "[" << all/times << "," << best << "," << worst << "]" << std::endl;
    }
}


/**
 * Includes
 */
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <iomanip>
#include <complex>
#include <typeinfo>
#include <boost/math/tools/polynomial.hpp>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/variables_map.hpp>
#include <boost/lexical_cast.hpp>
#include "string.h"
/**
 * Compiler const
 */
#define deflationMethod 1
#define deflationMethod2 2
#define mullerMethod 3
#define laguerreMethod 4
#define debug

namespace po = boost::program_options;
namespace anpi {
	/**
	 * Deflation class
	 */
	template<typename T> class deflation {
		public :
			/**
			 * Deflation class operator () overload
			 * @param poly: the polynomial array it can be almost any numerical type.
			 * @param root: a known root of the polynomial, has to be the same type as poly.
			 * @param order: the size of the polynomial -1, has to be a int.
			 * @return
			 */
			inline std::vector<T> operator () ( std::vector<T>& poly, T& root, T& res, int order) {
				res =poly[order];
				poly[order]=0;
				#ifdef debug
					std::cout << "Deflation method started." << std::endl;
				#endif
				for (int i=order-1;i>=0;i--){
					T s = poly[i];
					poly[i]=res;
					res=s+res*root;
					std::cout << poly[i] << "x^" << i;
					if(i!=0) std::cout << " + ";
				}
				std::cout << "\n";
				#ifdef debug
					std::cout << "Deflation method ended." << std::endl;
				#endif
				return poly;
			}
	};
	/**
	 * Deflation2 class, for the case that both complex conjugate roots have to be deleted
	 */
	template<typename T> class deflation2 {
		public :
			/**
			 * Deflation2 class operator () overload
			 * @param poly: the polynomial array, has to be a complex<T> type.
			 * @param root: a known root of the polynomial, has to be  the same type as poly.
			 * @param order: the size of the polynomial -1, has to be a int.
			 * @return
			 */
			inline std::vector<T> operator () (std::vector<T>& poly, T& root, T& res, int order) {
				res =poly[order];
				poly[order]=0;
				#ifdef debug
					std::cout << "Deflation method 2 started." << std::endl;
				#endif
				for (int i=order-1;i>=0;i--){
					T s = poly[i];
					poly[i]=res;
					res=s+res*root;
				}
				res =poly[--order];
				root=std::conj(root);
				poly[order]=0;
				for (int i=order-1;i>=0;i--){
					T s = poly[i];
					poly[i]=res;
					res=s+res*root;
					std::cout << poly[i] << "x^" << i;
					if(i!=0) std::cout << " + ";
				}
				std::cout << "\n";
				#ifdef debug
					std::cout << "Deflation method 2 ended." << std::endl;
				#endif
				return poly;
			}
	};
	/**
	 * Muller class
	 */
	template <typename T>
	class muller{
		public:
			/**
			 * Muller class operator () overload
			 * @param x0:
			 * @param x1:
			 * @param x2:
			 * @param coefs:
			 * @param max:
			 * @param pTolerance:
			 * @return
			 */
			std::vector<std::complex<T> > operator()(T x0, T x1, T x2,const std::vector< std::complex<T> >& coefs,unsigned int max, double pTolerance) {
			std::vector<std::complex<T> > res;
			std::vector<std::complex<T> > q = coefs;
			std::complex<T> z;
			while (q.size() > 2) {
			    z = polinom_zero(x0, x1, x2, q, max, pTolerance);
			    //z = polinom_zero(p, z);
			    q = horner(q, z);
			    res.push_back(z);
			  }
			  res.push_back(-q[0] / q[1]);
			  return res;
			}

			std::complex<T> evaluatePol(std::vector<std::complex<T> >& coef, std::complex<T> * x){
				std::complex<T> result = 0;
				for (int ite = 0; ite<coef.size(); ite++){
					result+= coef[ite]*std::pow(*x, ite);
				}
				return result;
			}

			std::vector<std::complex<T> > horner(const std::vector<std::complex<T> > &a, std::complex<T> x0) {
			  int n = a.size();
			  std::vector<std::complex<T> >  b(std::max(1, n - 1),0);

			  for(int i = n - 1; i > 0; i--)
			    b[i - 1] = a[i] + (i < n - 1 ? b[i] * x0 : 0);
			  return b;
			}

			std::complex<T>  polinom_zero (T x0, T x1, T x2,std::vector< std::complex<T> >& coefs,unsigned int max, double pTolerance){
				std::complex<T> xi0, xi1, xi2,xi3, h0,h1,q0,q1,A, B, C,result, tmp1, tmp2;
				long double err;
				xi0=x0;
				xi1=x1;
				xi2=x2;
				err=2;
				tmp1=0;
				tmp2=0;
				int ite =0;
				while(ite<max && (long double)err >= pTolerance){

					h0=xi1-xi0;
					h1=xi2-xi1;
					q0=(evaluatePol(coefs, &xi1)-evaluatePol(coefs, &xi0))/h0;
					q1=(evaluatePol(coefs, &xi2)-evaluatePol(coefs, &xi1))/h1;

					A=(q1-q0)/(h1+h0);
					B=A*h1+q1;
					C=(evaluatePol(coefs, &xi2));

					tmp1=xi2+((std::complex<T> )-2*C)/(B+std::sqrt(B*B-(std::complex<T> )4*A*C));
					tmp2=xi2+((std::complex<T> )-2*C)/(B-std::sqrt(B*B-(std::complex<T> )4*A*C));

					if(B.real()>=0)
						xi3=tmp1;
					else
						xi3=tmp2;
					err=std::abs((((xi3-xi2)/xi3)).real())*100;

					xi0=xi1;
					xi1=xi2;
					xi2=xi3;
					ite++;

				}
				return xi3;
			};

	};
	typedef std::complex<double> cdouble;
	typedef std::vector<cdouble> polyn;
	/**
	 * Laguerre class
	 */
	std::pair<polyn, cdouble> horner(const polyn &a, cdouble x0) {
	  int n = a.size();
	  polyn b = polyn(std::max(1, n - 1));

	  for(int i = n - 1; i > 0; i--)
	    b[i - 1] = a[i] + (i < n - 1 ? b[i] * x0 : 0);
	  return make_pair(b, a[0] + b[0] * x0);
	}

	cdouble eval(const polyn &p, cdouble x) {
	  return horner(p, x).second;
	}

	polyn derivative(const polyn &p) {
	  int n = p.size();
	  polyn r = polyn(std::max(1, n - 1));
	  for(int i = 1; i < n; i++)
	    r[i - 1] = p[i] * cdouble(i);
	  return r;
	}

	const double EPS = 1e-9;

	int cmp(cdouble x, cdouble y) {
	  double diff = abs(x) - abs(y);
	  return diff < -EPS ? -1 : (diff > EPS ? 1 : 0);
	}

	cdouble find_one_root(const polyn &p0, cdouble x) {
	  int n = p0.size() - 1;
	  polyn p1 = derivative(p0);
	  polyn p2 = derivative(p1);
	  for (int step = 0; step < 10000; step++) {
	    cdouble y0 = eval(p0, x);
	    if (cmp(y0, 0) == 0) break;
	    cdouble G = eval(p1, x) / y0;
	    cdouble H = G * G - eval(p2, x) - y0;
	    cdouble R = sqrt(cdouble(n - 1) * (H * cdouble(n) - G * G));
	    cdouble D1 = G + R;
	    cdouble D2 = G - R;
	    cdouble a = cdouble(n) / (cmp(D1, D2) > 0 ? D1 : D2);
	    x -= a;
	    if (cmp(a, 0) == 0) break;
	  }
	  return x;
	}

	std::vector<cdouble> find_all_roots(const polyn &p) {
	  std::vector<cdouble> res;
	  polyn q = p;
	  while (q.size() > 2) {
	    cdouble z(rand() / double(RAND_MAX), rand() / double(RAND_MAX));
	    z = find_one_root(q, z);
	    z = find_one_root(p, z);
	    q = horner(q, z).first;
	    res.push_back(z);
	  }
	  res.push_back(-q[0] / q[1]);
	  return res;
	}
}

/**
 * Auxiliar method for the main
 * @param method          Which method will be run (number from 1 to 4 or none for default).
 * @param poly            The polynomial that the method will be run in.
 * @param order           The orther of the polynomial.
 * @param root            The root of the polinomial (use for deflation).
 * @param X               The positions where the method will be started.
 * @param maxIt           The max times the method will be run(muller).
 * @param tolerance       The tolerance the method will have(muller).
 * @param singlePrecision Single precision flag.
 * @param polish          Polish flag.
 */
void mainAux(unsigned short method, std::vector<double>& poly, short order, double root, std::vector<double> X, int maxIt=50, double tolerance=0.00001, bool singlePrecision=0,bool polish=0){
	if(poly.size()==0 and method!=0){
		std::cout << "To use a method you need to enter a polynomial with the option --polynomial and the coeficients."<< std::endl;
		return;
	}
	switch(method){
		case deflationMethod: //Deflation
		{
			if(singlePrecision){
				std::complex<float> res =*(new std::complex<float>(0,0));
				std::complex<float> roots=*(new std::complex<float>(root,0));
				std::vector<std::complex<float> > polys;
				anpi::deflation<std::complex<float> > defla;
				for(int i=0; i<poly.size(); i++){
					std::complex<float> tmp=*(new std::complex<float>(poly[i],0));
					polys.push_back(tmp);
				}
				defla(polys,roots, res, (int)order);
			}else{
				std::complex<double> res =*(new std::complex<double>(0,0));
				std::complex<double> rootd=*(new std::complex<double>(root,0));
				std::vector<std::complex<double> > polyd;
				anpi::deflation<std::complex<double> > defla;
				for(int i=0; i<poly.size(); i++){
					std::complex<float> tmp=*(new std::complex<float>(poly[i],0));
					polyd.push_back(tmp);
				}
				defla(polyd,rootd, res, (int)order);
			}
			break;
		}
		case deflationMethod2: //Deflation2
		{
			if(singlePrecision){
				std::complex<float> res =*(new std::complex<float>(0,0));
				std::complex<float> roots=*(new std::complex<float>(root,0));
				std::vector<std::complex<float> > polys;
				anpi::deflation2<std::complex<float> > defla;
				for(int i=0; i<poly.size(); i++){
					std::complex<float> tmp=*(new std::complex<float>(poly[i],0));
					polys.push_back(tmp);
				}
				defla(polys,roots, res, (int)order);
			}else{
				std::complex<double> res =*(new std::complex<double>(0,0));
				std::complex<double> rootd=*(new std::complex<double>(root,0));
				std::vector<std::complex<double> > polyd;
				anpi::deflation2<std::complex<double> > defla;
				for(int i=0; i<poly.size(); i++){
					std::complex<float> tmp=*(new std::complex<float>(poly[i],0));
					polyd.push_back(tmp);
				}
				defla(polyd,rootd, res, (int)order);
			}
			break;
		}
		case mullerMethod: //Muller
		{

			if(X.size()!=3){
				std::cout << "Three positions has to be set to use this method. Use the option: --position and the positions space separated." << std::endl;
				return ;
			}
			if(singlePrecision){
				anpi::muller<float> mull;
				std::vector<std::complex<float> > polyd;
				for(int i=0; i<poly.size(); i++){
					std::complex<float> tmp=*(new std::complex<float>(poly[i],0));
					polyd.push_back(tmp);
				}
				std::vector<std::complex<float> > res;
				res = mull((float)X[0],
								(float)X[1],
								(float)X[2],
								polyd,
								(unsigned int)maxIt,
								(float)tolerance);
				for(int i = 0; i < res.size(); i++){
					std::cout<< "x" << i <<": " << res[i] << std::endl;
				}
				break;
			}else{
				anpi::muller<double> mull;
				std::vector<std::complex<double> > polyd;
				for(int i=0; i<poly.size(); i++){
					std::complex<double> tmp=*(new std::complex<double>(poly[i],0));
					polyd.push_back(tmp);
				}
				std::vector<std::complex<double> > res;
				res = mull((double)X[0],
								(double)X[1],
								(double)X[2],
								polyd,
								(unsigned int)maxIt,
								(double)tolerance);
				for(int i = 0; i < res.size(); i++){
					std::cout<< "x" << i <<": " << res[i] << std::endl;
				}
				break;
			}
		}
		case laguerreMethod:
		{
			anpi::polyn p;
			// x^3 - 8x^2 - 13x + 140 = (x+4)(x-5)(x-7)
			//X**3 + 2x**2 -109x -110 = (x+11)(x+8)(x-4)
			for(int i=0; i<poly.size(); i++){
				p.push_back(poly[i]);
			}
			std::vector<anpi::cdouble> roots = anpi::find_all_roots(p);

			for(size_t i = 0; i < roots.size(); i++) {
				if (abs(roots[i].real()) < anpi::EPS) roots[i] -= anpi::cdouble(roots[i].real(), 0);
					if (abs(roots[i].imag()) < anpi::EPS) roots[i] -= anpi::cdouble(0, roots[i].imag());
						std::cout << std::setprecision(3) << roots[i] << std::endl;
			}
			break;
		}
		default:
		{
			//Deflation method
			std::cout << "Deflation of 1x^3+3x^2+4x+12, with 2i as root." <<std::endl;
			std::vector<std::complex<double> >  polyd;
			std::complex<double> tmp= *(new std::complex<double>(12,0));
			polyd.push_back(tmp);
			tmp= *(new std::complex<double>(4,0));
			polyd.push_back(tmp);
			tmp= *(new std::complex<double>(3,0));
			polyd.push_back(tmp);
			tmp= *(new std::complex<double>(1,0));
			polyd.push_back(tmp);
			std::complex<double> rootd =*(new std::complex<double>(0,2));
			std::complex<double> res =*(new std::complex<double>(0,0));
			anpi::deflation<std::complex<double> > defla;
			defla(polyd,rootd, res, (int)order);
			//Deflation method 2
			std::cout << "Deflation method 2 of 1x^3+3x^2+4x+12, with 2i as root." <<std::endl;
			rootd =*(new std::complex<double>(0,2));
			res =*(new std::complex<double>(0,0));
			anpi::deflation2<std::complex<double> > defla2;
			defla2(polyd,rootd, res, (int)order);
			//Muller method
			std::cout << "Muller method roots of 1x^3+2x^2+-5x+-6." <<std::endl;
			anpi::muller<double> mull;
			std::vector<std::complex<double> >coefs;
			tmp=-6;
			coefs.push_back(tmp);
			tmp=-5;
			coefs.push_back(tmp);
			tmp=2;
			coefs.push_back(tmp);
			tmp=1;
			coefs.push_back(tmp);
			std::vector<std::complex<double> > result;
			result = mull((double)-4,
							(double)-2,
							(double)0,
							coefs,
							(unsigned int)50,
							(long double)0.000001);
			for(int i = 0; i < result.size(); i++){
				std::cout<< "x" << i <<": " << result[i] << std::endl;
			}

			//Laguerre method
			std::cout << "Laguerre roots of 1x^3-2x^2+-109x+-110." <<std::endl;
			anpi::polyn p;
			// x^3 - 8x^2 - 13x + 140 = (x+4)(x-5)(x-7)
			//X**3 + 2x**2 -109x -110 = (x+11)(x+8)(x-4)
			p.push_back(-110);
			p.push_back(-109);
			p.push_back(-2);
			p.push_back(1);

			std::vector<anpi::cdouble> roots = anpi::find_all_roots(p);

			for(size_t i = 0; i < roots.size(); i++) {
				if (abs(roots[i].real()) < anpi::EPS) roots[i] -= anpi::cdouble(roots[i].real(), 0);
					if (abs(roots[i].imag()) < anpi::EPS) roots[i] -= anpi::cdouble(0, roots[i].imag());
						std::cout << std::setprecision(3) << roots[i] << std::endl;
			}
		}

	}
}

/**
 * Main method, runs all the methods with a default or you can enter options
 * @param Uses program options, call the program plus --help to see detailed description
 * @return the return is depending which method is called
 */
int main(int argc, char** argv){
	using boost::lexical_cast;
	using boost::bad_lexical_cast;
	//Default values for the methods:
	short order=3;
	unsigned short method=0;
	double tolerance=0.000001;
	int maxIt=50;
	std::vector<double> poly;
	double root;
	std::vector<double> position;
	bool polish=0;
	bool singlePrecision=0;
	//Program options definitions
	po::options_description description("Project1 Usage");
	description.add_options()
	("help", "Display this help message")
	("polynomial", po::value<std::vector<double> >(&poly)->multitoken(), "The polynomial that the program will use, just add the coeficients space separated.")
	("position", po::value<std::vector<double> >(&position)->multitoken(), "The positions to start the method space separated.")
	("root", po::value<double>(&root), "The root to find.")
	("order", po::value<short>(&order), "The order of the polynomial.")
	("muller", "Use this option to use the muller method.")
	("deflation", "Use this option to use deflation method.")
	("deflation2", "Use this option to use deflation2 method.")
	("laguerre", "Use this option to use laguerre method.")
	("polish", "Use this option to polish the roots.")
	("tolerance", po::value<double>(&tolerance), "The order of the polynomial.")
	("maxIterations", po::value<int>(&maxIt), "The order of the polynomial.")
	("singlePrecision", "Use this to set the methods to double singlePrecision, otherwise double precision will be used.");
	po::positional_options_description p;
	p.add("file", -1);
	po::variables_map vm;
	//Try to use the entered options
	try {
		po::store(po::parse_command_line(argc, argv, description, po::command_line_style::unix_style ^ po::command_line_style::allow_short), vm);		po::notify(vm);
		if(vm.count("help")){
			std::cout << description;
			return 0;
		}
		if(vm.count("order")){
			#ifdef debug
				std::cout << "Polynomial order changed to: " << order << std::endl;
			#endif
			if(order<0){
				std::cout << "The order has to be a positive number." << std::endl;
				return -1;
			}
		}
		if(vm.count("tolerance")){
			#ifdef debug
				std::cout << "The tolerance for the method will be: " << tolerance << std::endl;
			#endif
			if(tolerance<0){
				std::cout << "The tolerance has to be a positive number." << std::endl;
				return -1;
			}

		}
		if(vm.count("maxIterations")){
			#ifdef debug
				std::cout << "Max iterations will be changed to: " << maxIt << std::endl;
			#endif
			if(maxIt<0){
				std::cout << "The iterations has to be a positive number." << std::endl;
				return -1;
			}

		}
		if(vm.count("singlePrecision")){
			#ifdef debug
				std::cout << "Polynomial order changed to: " << singlePrecision << std::endl;
			#endif
			singlePrecision = 1;
		}
		if(vm.count("deflation")){
			#ifdef debug
				std::cout << "Deflation method will be used." << std::endl;
			#endif
			method = deflationMethod;
		}
		if(vm.count("deflation2")){
			#ifdef debug
				std::cout << "Deflation method 2 will be used." << std::endl;
			#endif
			method = deflationMethod2;
		}
		if(vm.count("muller")){
			#ifdef debug
				std::cout << "Muller method will be used." << std::endl;
			#endif
			method = mullerMethod;
		}
		if(vm.count("laguerre")){
			#ifdef debug
				std::cout << "Laguerre method will be used." << std::endl;
			#endif
			method = laguerreMethod;
		}
		if(vm.count("poly")){
			#ifdef debug
				std::cout << "A custom polynomial will be used. With size: " <<poly.size() << std::endl;
			#endif
			if(poly.size()<=0){
				std::cout << "The polynomial coeficients must be more than 0." << std::endl;
				return -1;
			}
		}
		if(vm.count("root")){
			#ifdef debug
				std::cout << "A custom root will be used: " << root << std::endl;
			#endif
		}
		if(vm.count("position")){
			#ifdef debug
				std::cout << "Custom positions will be used." << std::endl;
			#endif
			if(poly.size()<=0){
				std::cout << "The positions must be more than 0." << std::endl;
				return -1;
			}
		}
		mainAux(method, poly, order, root, position,maxIt, tolerance, singlePrecision,polish);
		return 0;
	}
	catch(const po::error &ex){
		std::cerr << ex.what() << '\n';
	}

}


//pca.cpp
//https://github.com/ihar/EigenPCA/blob/master/pca.cpp

#define NODEBUG
#include <iostream>
#ifdef DEBUG
  #include <iterator>
#endif
#include <Eigen/SVD>
#include "pca.h"

using namespace std;
using namespace Eigen;

int Pca::Calculate(vector<float> &x,
              const unsigned int &nrows,
              const unsigned int &ncols,
              const bool is_corr,
              const bool is_center,
              const bool is_scale) {
  _ncols = ncols;
  _nrows = nrows;
  _is_corr = is_corr;
  _is_center = is_center;
  _is_scale = is_scale;
  if (x.size()!= _nrows*_ncols) {
    return -1;
  }
  if ((1 == _ncols) || (1 == nrows)) {
    return -1;
  }
  // Convert vector to Eigen 2-dimensional matrix
  //Map<MatrixXf> _xXf(x.data(), _nrows, _ncols);
  _xXf.resize(_nrows, _ncols);
  for (unsigned int i = 0; i < _nrows; ++i) {
    for (unsigned int j = 0; j < _ncols; ++j) {
      _xXf(i, j) = x[j + i*_ncols];
    }
  }
  // Mean and standard deviation for each column
  VectorXf mean_vector(_ncols);
  mean_vector = _xXf.colwise().mean();
  VectorXf sd_vector(_ncols);
  unsigned int zero_sd_num = 0;
  float denom = static_cast<float>((_nrows > 1)? _nrows - 1: 1);
  for (unsigned int i = 0; i < _ncols; ++i) {
     VectorXf curr_col  = VectorXf::Constant(_nrows, mean_vector(i)); // mean(x) for column x
     curr_col = _xXf.col(i) - curr_col; // x - mean(x)
     curr_col = curr_col.array().square(); // (x-mean(x))^2
     sd_vector(i) = sqrt((curr_col.sum())/denom);
     if (0 == sd_vector(i)) {
      zero_sd_num++;
     }
  }
  // If colums with sd == 0 are too many,
  // don't continue calculations
  if (1 > _ncols-zero_sd_num) {
    return -1;
  }
  // Delete columns where sd == 0
  MatrixXf tmp(_nrows, _ncols-zero_sd_num);
  VectorXf tmp_mean_vector(_ncols-zero_sd_num);
  unsigned int curr_col_num = 0;
  for (unsigned int i = 0; i < _ncols; ++i) {
    if (0 != sd_vector(i)) {
      tmp.col(curr_col_num) = _xXf.col(i);
      tmp_mean_vector(curr_col_num) = mean_vector(i);
      curr_col_num++;
    } else {
      _eliminated_columns.push_back(i);
    }
  }
  _ncols -= zero_sd_num;
  _xXf = tmp;
  mean_vector = tmp_mean_vector;
  tmp.resize(0, 0); tmp_mean_vector.resize(0);
  // Shift to zero
  if (true == _is_center) {
    for (unsigned int i = 0; i < _ncols; ++i) {
      _xXf.col(i) -= VectorXf::Constant(_nrows, mean_vector(i));
    }
  }
  // Scale to unit variance
  if ( (false == _is_corr) || (true == _is_scale)) {
    for (unsigned int i = 0; i < _ncols; ++i) {
     _xXf.col(i) /= sqrt(_xXf.col(i).array().square().sum()/denom);
    }
  }
  #ifdef DEBUG
    cout << "\nScaled matrix:\n";
    cout << _xXf << endl;
    cout << "\nMean before scaling:\n" << mean_vector.transpose();
    cout << "\nStandard deviation before scaling:\n" << sd_vector.transpose();
  #endif
  // When _nrows < _ncols then svd will be used.
  // If corr is true and _nrows > _ncols then will be used correlation matrix
  // (TODO): What about covariance?
  if ( (_nrows < _ncols) || (false == _is_corr)) { // Singular Value Decomposition is on
    _method = "svd";
    JacobiSVD<MatrixXf> svd(_xXf, ComputeThinV);
    VectorXf eigen_singular_values = svd.singularValues();
    VectorXf tmp_vec = eigen_singular_values.array().square();
    float tmp_sum = tmp_vec.sum();
    tmp_vec /= tmp_sum;
    // PC's standard deviation and
    // PC's proportion of variance
    _kaiser = 0;
    unsigned int lim = (_nrows < _ncols)? _nrows : _ncols;
    for (unsigned int i = 0; i < lim; ++i) {
      _sd.push_back(eigen_singular_values(i)/sqrt(denom));
      if (_sd[i] >= 1) {
        _kaiser = i + 1;
      }
      _prop_of_var.push_back(tmp_vec(i));
    }
    #ifdef DEBUG
      cout << "\n\nStandard deviations for PCs:\n";
      copy(_sd.begin(), _sd.end(),std::ostream_iterator<float>(std::cout," "));
      cout << "\n\nKaiser criterion: PC #" << _kaiser << endl;
    #endif
    tmp_vec.resize(0);
    // PC's cumulative proportion
    _thresh95 = 1;
    _cum_prop.push_back(_prop_of_var[0]);
    for (unsigned int i = 1; i < _prop_of_var.size(); ++i) {
      _cum_prop.push_back(_cum_prop[i-1]+_prop_of_var[i]);
      if (_cum_prop[i] < 0.95) {
        _thresh95 = i+1;
      }
    }
    #ifdef DEBUG
      cout << "\nCumulative proportion:\n";
      copy(_cum_prop.begin(), _cum_prop.end(),std::ostream_iterator<float>(std::cout," "));
      cout << "\n\nThresh95 criterion: PC #" << _thresh95 << endl;
    #endif
    // Scores
    MatrixXf eigen_scores = _xXf * svd.matrixV();
    #ifdef DEBUG
      cout << "\n\nRotated values (scores):\n" << eigen_scores;
    #endif
    _scores.reserve(lim*lim);
    for (unsigned int i = 0; i < lim; ++i) {
      for (unsigned int j = 0; j < lim; ++j) {
        _scores.push_back(eigen_scores(i, j));
      }
    }
    eigen_scores.resize(0, 0);
    #ifdef DEBUG
      cout << "\n\nScores in vector:\n";
      copy(_scores.begin(), _scores.end(),std::ostream_iterator<float>(std::cout," "));
      cout << "\n";
    #endif
  } else { // COR OR COV MATRICES ARE HERE
    _method = "cor";
    // Calculate covariance matrix
    MatrixXf eigen_cov; // = MatrixXf::Zero(_ncols, _ncols);
    VectorXf sds;
    // (TODO) Should be weighted cov matrix, even if is_center == false
    eigen_cov = (1.0 /(_nrows/*-1*/)) * _xXf.transpose() * _xXf;
    sds = eigen_cov.diagonal().array().sqrt();
    MatrixXf outer_sds = sds * sds.transpose();
    eigen_cov = eigen_cov.array() / outer_sds.array();
    outer_sds.resize(0, 0);
    // ?If data matrix is scaled, covariance matrix is equal to correlation matrix
    #ifdef DEBUG
      cout << eigen_cov << endl;
    #endif
    EigenSolver<MatrixXf> edc(eigen_cov);
    VectorXf eigen_eigenvalues = edc.eigenvalues().real();
    #ifdef DEBUG
      cout << endl << eigen_eigenvalues.transpose() << endl;
    #endif
    MatrixXf eigen_eigenvectors = edc.eigenvectors().real();
    #ifdef DEBUG
      cout << endl << eigen_eigenvectors << endl;
    #endif
    // The eigenvalues and eigenvectors are not sorted in any particular order.
    // So, we should sort them
    typedef pair<float, int> eigen_pair;
    vector<eigen_pair> ep;
    for (unsigned int i = 0 ; i < _ncols; ++i) {
	    ep.push_back(make_pair(eigen_eigenvalues(i), i));
    }
    sort(ep.begin(), ep.end()); // Ascending order by default
    // Sort them all in descending order
    MatrixXf eigen_eigenvectors_sorted = MatrixXf::Zero(eigen_eigenvectors.rows(), eigen_eigenvectors.cols());
    VectorXf eigen_eigenvalues_sorted = VectorXf::Zero(_ncols);
    int colnum = 0;
    int i = ep.size()-1;
    for (; i > -1; i--) {
      eigen_eigenvalues_sorted(colnum) = ep[i].first;
      eigen_eigenvectors_sorted.col(colnum++) += eigen_eigenvectors.col(ep[i].second);
    }
    #ifdef DEBUG
      cout << endl << eigen_eigenvalues_sorted.transpose() << endl;
      cout << endl << eigen_eigenvectors_sorted << endl;
    #endif
    // We don't need not sorted arrays anymore
    eigen_eigenvalues.resize(0);
    eigen_eigenvectors.resize(0, 0);

    _sd.clear(); _prop_of_var.clear(); _kaiser = 0;
    float tmp_sum = eigen_eigenvalues_sorted.sum();
    for (unsigned int i = 0; i < _ncols; ++i) {
      _sd.push_back(sqrt(eigen_eigenvalues_sorted(i)));
      if (_sd[i] >= 1) {
        _kaiser = i + 1;
      }
      _prop_of_var.push_back(eigen_eigenvalues_sorted(i)/tmp_sum);
    }
    #ifdef DEBUG
      cout << "\nStandard deviations for PCs:\n";
      copy(_sd.begin(), _sd.end(), std::ostream_iterator<float>(std::cout," "));
      cout << "\nProportion of variance:\n";
      copy(_prop_of_var.begin(), _prop_of_var.end(), std::ostream_iterator<float>(std::cout," "));
      cout << "\nKaiser criterion: PC #" << _kaiser << endl;
    #endif
    // PC's cumulative proportion
    _cum_prop.clear(); _thresh95 = 1;
    _cum_prop.push_back(_prop_of_var[0]);
    for (unsigned int i = 1; i < _prop_of_var.size(); ++i) {
      _cum_prop.push_back(_cum_prop[i-1]+_prop_of_var[i]);
      if (_cum_prop[i] < 0.95) {
        _thresh95 = i+1;
      }
    }
    #ifdef DEBUG
      cout << "\n\nCumulative proportions:\n";
      copy(_cum_prop.begin(), _cum_prop.end(), std::ostream_iterator<float>(std::cout," "));
      cout << "\n\n95% threshold: PC #" << _thresh95 << endl;
    #endif
    // Scores for PCA with correlation matrix
    // Scale before calculating new values
    for (unsigned int i = 0; i < _ncols; ++i) {
     _xXf.col(i) /= sds(i);
    }
    sds.resize(0);
    MatrixXf eigen_scores = _xXf * eigen_eigenvectors_sorted;
    #ifdef DEBUG
      cout << "\n\nRotated values (scores):\n" << eigen_scores;
    #endif
    _scores.clear();
    _scores.reserve(_ncols*_nrows);
    for (unsigned int i = 0; i < _nrows; ++i) {
      for (unsigned int j = 0; j < _ncols; ++j) {
        _scores.push_back(eigen_scores(i, j));
      }
    }
    eigen_scores.resize(0, 0);
    #ifdef DEBUG
      cout << "\n\nScores in vector:\n";
      copy(_scores.begin(), _scores.end(), std::ostream_iterator<float>(std::cout," "));
      cout << "\n";
    #endif
  }

  return 0;
}
std::vector<float> Pca::sd(void) { return _sd; };
std::vector<float> Pca::prop_of_var(void) {return _prop_of_var; };
std::vector<float> Pca::cum_prop(void) { return _cum_prop; };
std::vector<float> Pca::scores(void) { return _scores; };
std::vector<unsigned int> Pca::eliminated_columns(void) { return _eliminated_columns; }
string Pca::method(void) { return _method; }
unsigned int Pca::kaiser(void) { return _kaiser; };
unsigned int Pca::thresh95(void) { return _thresh95; };
unsigned int Pca::ncols(void) { return _ncols; }
unsigned int Pca::nrows(void) { return _nrows; }
bool Pca::is_scale(void) {  return _is_scale; }
bool Pca::is_center(void) { return _is_center; }
Pca::Pca(void) {
  _nrows      = 0;
  _ncols      = 0;
  // Variables will be scaled by default
  _is_center  = true;
  _is_scale   = true;
  // By default will be used singular value decomposition
  _method   = "svd";
  _is_corr  = false;

  _kaiser   = 0;
  _thresh95 = 1;
}
Pca::~Pca(void) {
  _xXf.resize(0, 0);
  _x.clear();
}


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

std::atomic_size_t Ncalls(0);

using CEGO::EArray;

auto linfit_coeffs(const EArray<double>& x, const EArray<double>& y, const std::size_t Nnum, const std::size_t Nden) {
    Eigen::MatrixXd A(x.size(), Nnum + Nden);
    Eigen::MatrixXd b = y;
    for (auto i = 0; i < Nnum; ++i) {
        A.col(i) = x.pow(i);
    }
    for (auto i = 0; i < Nden; ++i) {
        A.col(i + Nnum) = -y*x.pow(i+1);
    }
    auto c = A.colPivHouseholderQr().solve(b).eval();
    auto ycheck = (A * c).eval();
    return c;
}

class RatPoly {
public:
    EArray<double> m_T, B2, m_LHS, m_x;
    std::size_t m_Nnum, m_Nden;

    RatPoly(const std::size_t Nnum, const std::size_t Nden) : m_Nnum(Nnum), m_Nden(Nden)
    {
        // Data from second virial coefficient from Garberoglio for water, DOI: 10.1039/C8FD00092A
        m_T.resize(21);
        B2.resize(21);
        m_T << 200, 225, 250, 273.15, 300, 325, 350, 375, 400, 450, 500, 550, 600, 700, 800, 900, 1000, 1250, 1500, 1750, 2000;
        B2 << -2.3825E-02, -8.6200E-03, -3.9850E-03, -2.2590E-03, -1.3301E-03, -8.9350E-04, -6.3650E-04, -4.7840E-04, -3.7240E-04, -2.4480E-04, -1.7240E-04, -1.2830E-04, -9.8700E-05, -6.2460E-05, -4.1280E-05, -2.7580E-05, -1.8550E-05, -5.0400E-06, 2.0800E-06, 6.2000E-06, 8.8800E-06;
        m_LHS = B2;
        m_x = 200/m_T;
        //solve_linear();
    }
    void solve_linear() {
        std::cout << "----------- LINEAR --------- " << std::endl;
        Eigen::ArrayXi indices = Eigen::ArrayXi::LinSpaced(m_Nnum + m_Nden, 0L, static_cast<int>(m_x.size()-1));
        EArray<double> xx = m_x(indices), yy = m_LHS(indices);
        EArray<double> c = linfit_coeffs(xx, yy, m_Nnum, m_Nden);
        std::cout << c << std::endl;
        double o = objective(c);
        std::cout << "objective: " << o << std::endl;
        EArray<double> devs = abs_rel_deviations(c).eval() * 100;
        std::cout << "relative(%) devs: " << devs << std::endl;
    }
    template <typename TYPE> EArray<TYPE> eval_RHS(const EArray<double>& x, const EArray<TYPE> &c) {
        EArray<TYPE> num = EArray<TYPE>::Zero(x.size()),
                     den = EArray<TYPE>::Zero(x.size());
        assert(m_Nnum + m_Nden == c.size());
        for (auto i = 0; i < m_Nnum; ++i) {
            num += c[i]*x.pow(i);
        }
        for (auto i = 0; i < m_Nden; ++i) {
            den += c[m_Nnum+i]*x.pow(i+1);
        }
        return num/(1+den);
    }
    template <typename TYPE> TYPE objective(const EArray<TYPE>& c) {
        return ((eval_RHS(m_x, c) - m_LHS)/m_LHS).square().sum();
    }
    template <typename TYPE> EArray<TYPE> abs_rel_deviations(const EArray<TYPE>& c) {
        return ((eval_RHS(m_x, c) - m_LHS)/m_LHS).eval();
    }
    double objective(const CEGO::AbstractIndividual *pind) {
        const auto &c = dynamic_cast<const CEGO::NumericalIndividual<double>*>(pind)->get_coeff_array<double>();
        return objective(c);
    }
};

int do_one()
{
    //std::srand((unsigned int)time(0));
    std::size_t Nnum = 3, Nden = 5;

    // Construct the bounds
    std::vector<CEGO::Bound> bounds;
    for (auto i = 0; i < Nnum; ++i) {
        bounds.push_back(CEGO::Bound(std::pair<double, double>(-1000, 1000)));
    }
    for (auto i = 0; i < Nden; ++i){
        bounds.push_back(CEGO::Bound(std::pair<double, double>(-1000, 1000)));
    }
    RatPoly rp(Nnum, Nden);

    auto Ncalls = 0;
    CEGO::CostFunction<double> cost_wrapper = [&rp](const CEGO::AbstractIndividual*pind) {return rp.objective(pind); };
    auto Ntotal_individuals = 1000;
    auto Nlayers = 7;
    auto layers = CEGO::Layers<double>(cost_wrapper, bounds.size(), Ntotal_individuals/Nlayers, Nlayers, 3);
    layers.parallel = true;
    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)*100 << 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;
}


/*
 * elliptic_functions.cpp
 *
 * Copyright 2009-2012 Karsten Ahnert
 * Copyright 2009-2012 Mario Mulansky
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <iostream>
#include <fstream>
#include <cmath>

#include <boost/array.hpp>

#include <boost/numeric/odeint/config.hpp>

#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/stepper/bulirsch_stoer.hpp>
#include <boost/numeric/odeint/stepper/bulirsch_stoer_dense_out.hpp>

using namespace std;
using namespace boost::numeric::odeint;

typedef boost::array< double , 3 > state_type;

/*
 * x1' = x2*x3
 * x2' = -x1*x3
 * x3' = -m*x1*x2
 */

void rhs( const state_type &x , state_type &dxdt , const double t )
{
    static const double m = 0.51;

    dxdt[0] = x[1]*x[2];
    dxdt[1] = -x[0]*x[2];
    dxdt[2] = -m*x[0]*x[1];
}

ofstream out;

void write_out( const state_type &x , const double t )
{
    out << t << '\t' << x[0] << '\t' << x[1] << '\t' << x[2] << endl;
}

int main()
{
    bulirsch_stoer_dense_out< state_type > stepper( 1E-9 , 1E-9 , 1.0 , 0.0 );

    state_type x1 = {{ 0.0 , 1.0 , 1.0 }};

    double t = 0.0;
    double dt = 0.01;

    out.open( "elliptic1.dat" );
    out.precision(16);
    integrate_const( stepper , rhs , x1 , t , 100.0 , dt , write_out );
    out.close();

    state_type x2 = {{ 0.0 , 1.0 , 1.0 }};

    out.open( "elliptic2.dat" );
    out.precision(16);
    integrate_adaptive( stepper , rhs , x2 , t , 100.0 , dt , write_out );
    out.close();

    typedef runge_kutta_dopri5< state_type > dopri5_type;
    typedef controlled_runge_kutta< dopri5_type > controlled_dopri5_type;
    typedef dense_output_runge_kutta< controlled_dopri5_type > dense_output_dopri5_type;

    dense_output_dopri5_type dopri5 = make_dense_output( 1E-9 , 1E-9 , dopri5_type() );
    //dense_output_dopri5_type dopri5( controlled_dopri5_type( default_error_checker< double >( 1E-9 , 0.0 , 0.0 , 0.0 )  ) );

    state_type x3 = {{ 0.0 , 1.0 , 1.0 }};

    out.open( "elliptic3.dat" );
    out.precision(16);
    integrate_adaptive( dopri5 , rhs , x3 , t , 100.0 , dt , write_out );
    out.close();
}