#pragma once

// system includes --------------------------------------------------
#include <Eigen/Dense>

// own includes -----------------------------------------------------
#include <ridgelet/ridgelet_cell_array.hpp>
#include <ridgelet/rt.hpp>

template <typename FRAME>
std::vector<double>
make_inv_diagonal_preconditioner(const FRAME &frame, double vx, double vy)
{
  auto &lambdas = frame.lambdas();
  unsigned int N = lambdas.size();
  std::vector<double> d(N);

  Eigen::Vector2d sjk;
  Eigen::Vector2d v;
  v(0) = vx;
  v(1) = vy;

  for (unsigned int i = 0; i < N; ++i) {
    int j = lambdas[i].j;
    double p2mn = std::pow(2., j - 1);
    int k = lambdas[i].k;
    if (lambdas[i].t == rt_type::S) {
      d[i] = 1.0;
    } else if (lambdas[i].t == rt_type::X) {
      sjk(0) = 1;
      sjk(1) = double(k) / p2mn;
      sjk.normalize();
      d[i] = 1 + std::pow(2., j) * std::abs(sjk.dot(v));
    } else if (lambdas[i].t == rt_type::Y) {
      sjk(0) = double(k) / p2mn;
      sjk(1) = 1;
      sjk.normalize();
      d[i] = 1 + std::pow(2., j) * std::abs(sjk.dot(v));
    } else if (lambdas[i].t == rt_type::D) {
      k = (k < 0) ? -1 : 1;
      sjk(0) = 1;
      sjk(1) = k;
      sjk.normalize();
      d[i] = 1 + std::pow(2., j) * std::abs(sjk.dot(v));
    } else {
      assert(false);
    }
  }

  return d;
}

template <typename ARRAY_T>
class DiagonalOperator
{
 private:
  typedef RidgeletCellArray<ARRAY_T> rca_t;

 public:
  DiagonalOperator(const std::vector<double> &d)
      : d_(d)
  { /* empty  */
  }

  DiagonalOperator() {}

  DiagonalOperator &operator=(const DiagonalOperator &other) { d_ = other.d_; }

  DiagonalOperator(std::vector<double> &&d)
      : d_(std::forward<std::vector<double>>(d))
  { /* empty  */
  }

  void apply(rca_t &dst) const;
  void apply(rca_t &dst, const rca_t &src) const;

  void invert();

 private:
  std::vector<double> d_;
};

template <typename ARRAY_T>
void
DiagonalOperator<ARRAY_T>::apply(rca_t &dst) const
{
  assert(d_.size() == dst.coeffs().size());

  for (unsigned int i = 0; i < d_.size(); ++i) {
    dst[i] *= d_[i];
  }
}

template <typename ARRAY_T>
void
DiagonalOperator<ARRAY_T>::apply(rca_t &dst, const rca_t &src) const
{
  assert(d_.size() == dst.coeffs().size());
  assert(d_.size() == src.coeffs().size());

  for (unsigned int i = 0; i < d_.size(); ++i) {
    dst[i] = d_[i] * src[i];
  }
}

template <typename ARRAY_T>
void
DiagonalOperator<ARRAY_T>::invert()
{
  for (unsigned int i = 0; i < d_.size(); ++i) {
    d_[i] = 1. / d_[i];
  }
}

// ================================================================================
// ================================================================================
// ================================================================================
class TransportOperator
{
 protected:
  typedef Eigen::ArrayXXcd complex_array_t;
  typedef Eigen::ArrayXd col_t;  // (column vector)

 public:
  TransportOperator(double vx, double vy, double Lx, double Ly, int Nx, int Ny, double dt = 1.0)
      : vx_(vx)
      , vy_(vy)
      , Lx_(Lx)
      , Ly_(Ly)
      , Nx_(Nx)
      , Ny_(Ny)
      , dt_(dt)
  {
    Eigen::Vector2d v;
    v(0) = vx;
    v(1) = vy;

    // init xi_x
    if (Nx % 2 == 0)
      xi_x_ = col_t::LinSpaced(Nx, -Nx / 2, Nx / 2 - 1) / Lx;
    else
      xi_x_ = col_t::LinSpaced(Nx, -Nx / 2, Nx / 2) / Lx;

    // init xi_y
    if (Ny % 2 == 0)
      xi_y_ = col_t::LinSpaced(Ny, -Ny / 2, Ny / 2 - 1) / Ly;
    else
      xi_y_ = col_t::LinSpaced(Ny, -Ny / 2, Ny / 2) / Ly;
  }

  template <typename COMPLEX_ARRAY>
  void apply(COMPLEX_ARRAY &dst, const COMPLEX_ARRAY &src, bool conj = false) const;

  template <typename COMPLEX_ARRAY>
  void apply_bckwrd_euler(COMPLEX_ARRAY &dst) const;

 protected:
  double vx_;
  double vy_;
  double Lx_;
  double Ly_;
  unsigned int Nx_;
  unsigned int Ny_;

  col_t xi_x_;
  col_t xi_y_;
  const double twoPI = 2 * 3.141592653589793238462643;
  const std::complex<double> I = std::complex<double>(0, 1);
  double dt_;
};

template <typename COMPLEX_ARRAY>
void
TransportOperator::apply(COMPLEX_ARRAY &dst, const COMPLEX_ARRAY &src, bool conj) const
{
  typedef typename COMPLEX_ARRAY::Scalar numeric_t;

  const double f = conj ? -1 : 1;
  dst = src +
        f * (twoPI * dt_ * I) *
            (vx_ * xi_x_.transpose().replicate(Ny_, 1) + vy_ * xi_y_.replicate(1, Nx_))
                .cast<numeric_t>()
                .array() *
            src.array();
}

template <typename COMPLEX_ARRAY>
void
TransportOperator::apply_bckwrd_euler(COMPLEX_ARRAY &dst) const
{
  typedef typename COMPLEX_ARRAY::Scalar numeric_t;
  dst = dst.cwiseQuotient(
      COMPLEX_ARRAY::Ones(Ny_, Nx_) +
      twoPI * dt_ * I *
          (vx_ * xi_x_.transpose().replicate(Ny_, 1) + vy_ * xi_y_.replicate(1, Nx_))
              .cast<numeric_t>()
              .array());
}

// ================================================================================
/**
 *   @brief \f$ T' T  \f$
 */
class AhAOp : public TransportOperator
{
 private:
  typedef Eigen::ArrayXXcd complex_array_t;
  typedef Eigen::ArrayXd col_t;

 public:
  AhAOp(double vx, double vy, double Lx, double Ly, int Nx, int Ny, double dt = 1.0)
      : TransportOperator(vx, vy, Lx, Ly, Nx, Ny, dt)
  {
    dt2_ = dt_ * dt_;
  }

  template <typename DERIVED1, typename DERIVED2>
  void apply(Eigen::ArrayBase<DERIVED1> &dst, const Eigen::ArrayBase<DERIVED2> &src) const;

 private:
  using TransportOperator::xi_x_;
  using TransportOperator::xi_y_;
  double dt2_;
};

template <typename DERIVED1, typename DERIVED2>
void
AhAOp::apply(Eigen::ArrayBase<DERIVED1> &dst, const Eigen::ArrayBase<DERIVED2> &src) const
{
  static_assert(std::is_same<typename DERIVED1::Scalar, typename DERIVED2::Scalar>::value,
                "type mismatch");
  typedef typename DERIVED1::Scalar numeric_t;

  dst = src +
        twoPI * twoPI * dt2_ *
            (vx_ * xi_x_.transpose().replicate(Ny_, 1) + vy_ * xi_y_.replicate(1, Nx_))
                .cwiseAbs2()
                .cast<numeric_t>()
                .array() *
            src;
}

// ================================================================================
// ================================================================================
// ================================================================================
// transport operator with boundary conditions
class TransportOperatorBC : public TransportOperator
{
 public:
  template <typename DERIVED>
  TransportOperatorBC(const Eigen::DenseBase<DERIVED> &sigma,
                      double vx,
                      double vy,
                      double Lx,
                      double Ly,
                      int Nx,
                      int Ny,
                      double dt = 1.0)
      : TransportOperator(vx, vy, Lx, Ly, Nx, Ny, dt)
  {
    sigma_ = sigma;
  }

  template <typename COMPLEX_ARRAY>
  void apply(COMPLEX_ARRAY &dst, const COMPLEX_ARRAY &src, bool conj = false) const;

 private:
#ifdef USE_PLANNED_FFT
  typedef FFTr2c<PlannerR2C> fft_t;
#else
  typedef FFTr2c<PlannerR2COD> fft_t;
#endif

  typedef Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> array_t;

 private:
  Eigen::ArrayXXd sigma_;
};

template <typename COMPLEX_ARRAY>
void
TransportOperatorBC::apply(COMPLEX_ARRAY &dst, const COMPLEX_ARRAY &src, bool conj) const
{
  typedef COMPLEX_ARRAY complex_array_t;
  typedef typename complex_array_t::Scalar numeric_t;
  const double f = conj ? -1 : 1;

  // TODO: (inefficient) memory allocation
  array_t fn(src.rows(), src.cols());
  double r = std::sqrt(vx_ * vx_ + vy_ * vy_);
  fft_t fft;
  fft.ift(fn, src);
  fn *= sigma_ * r;
  fft.ft(dst, fn, false);
  dst *= dt_;
  dst += src +
         f * (twoPI * dt_ * I) *
             (vx_ * xi_x_.transpose().replicate(Ny_, 1) + vy_ * xi_y_.replicate(1, Nx_))
                 .cast<numeric_t>()
                 .array() *
             src.array();
}

/**
 * @brief \f$ T'T \f$  * where T is of type \a TransportOperatorBC an absorption
 * term (otherwise
 * identical to \a AhAOp)
 *
 *
 */
class AhAOpsigma : public TransportOperatorBC
{
 private:
  typedef TransportOperatorBC transport_op_t;

 public:
  template <typename DERIVED>
  AhAOpsigma(const Eigen::DenseBase<DERIVED> &sigma,
             double vx,
             double vy,
             double Lx,
             double Ly,
             int Nx,
             int Ny,
             double dt = 1.0)
      : TransportOperatorBC(sigma, vx, vy, Lx, Ly, Nx, Ny, dt)
  { /* empty  */
  }

  template <typename COMPLEX_ARRAY>
  void apply(COMPLEX_ARRAY &dst, const COMPLEX_ARRAY &src) const
  {
    typedef COMPLEX_ARRAY complex_array_t;
    typedef typename complex_array_t::Scalar numeric_t;

    complex_array_t tmp(dst.rows(), dst.cols());

    transport_op_t::apply(tmp, src);
    transport_op_t::apply(dst, tmp, true);
  }
};

// ================================================================================
// ================================================================================
// ================================================================================
/// Preconditioned transport operator \f$ D^{-1} T D\f$
template <typename RT_TYPE, typename OP_T>
class PTransportOp_Base
{
 private:
  typedef RT_TYPE rt_t;
  typedef typename rt_t::complex_array_t complex_array_t;
  typedef OP_T op_t;
  typedef typename rt_t::rt_coeff_t rt_coeff_t;

 public:
  /**
   *
   * @param rt    ridgelet transform object
   * @param aha   A^T A
   * @param vx    velocity in x direction
   * @param vy    velocity in y direction
   */
  PTransportOp_Base(const rt_t &rt, const op_t &aha, double vx, double vy)
      : rt_(rt)
      , aha_(aha)
      , D_(make_inv_diagonal_preconditioner(rt.frame(), vx, vy))
      , rt_coeffs_(rt.frame())
  {
    fi.resize(rt.frame().Ny(), rt.frame().Nx());
    fo.resize(rt.frame().Ny(), rt.frame().Nx());

    // prepare preconditioner
    D_.invert();
  }

  void apply(RidgeletCellArray<rt_coeff_t> &dst, const RidgeletCellArray<rt_coeff_t> &src) const
  {
    // apply D_
    D_.apply(rt_coeffs_, src);
    rt_.irt(fi, rt_coeffs_.coeffs());
    assert(!fi.hasNaN());
    aha_.apply(fo, fi);
    assert(!fo.hasNaN());
    rt_.rt(dst.coeffs(), fo);
    D_.apply(dst);
  }

 private:
  const rt_t &rt_;
  const op_t &aha_;
  DiagonalOperator<rt_coeff_t> D_;

  mutable RidgeletCellArray<rt_coeff_t> rt_coeffs_;
  mutable complex_array_t fi;
  mutable complex_array_t fo;
};

template <typename RT_TYPE = RT<>>
using PTransportOp = PTransportOp_Base<RT_TYPE, AhAOp>;

template <typename RT_TYPE = RT<>>
using PTransportOpBC = PTransportOp_Base<RT_TYPE, AhAOpsigma>;

// ================================================================================
/// Preconditioned transport operator \f$ D^{-1} T D\f$
template <typename RT_TYPE, typename OP_T>
class PTransportOp_BaseId
{
 private:
  typedef RT_TYPE rt_t;
  typedef typename rt_t::complex_array_t complex_array_t;
  typedef OP_T op_t;
  typedef typename rt_t::rt_coeff_t rt_coeff_t;

 public:
  /**
   *
   * @param rt    ridgelet transform object
   * @param aha   A^T A
   * @param vx    velocity in x direction
   * @param vy    velocity in y direction
   */
  PTransportOp_BaseId(const rt_t &rt, const op_t &aha, double vx, double vy)
      : rt_(rt)
      , aha_(aha)
  {
    fi.resize(rt.frame().Ny(), rt.frame().Nx());
    fo.resize(rt.frame().Ny(), rt.frame().Nx());
  }

  void apply(RidgeletCellArray<rt_coeff_t> &dst, const RidgeletCellArray<rt_coeff_t> &src) const
  {
    rt_.irt(fi, src.coeffs());
    assert(!fi.hasNaN());
    aha_.apply(fo, fi);
    assert(!fo.hasNaN());
    rt_.rt(dst.coeffs(), fo);
  }

 private:
  const rt_t &rt_;
  const op_t &aha_;
  mutable complex_array_t fi;
  mutable complex_array_t fo;
};

template <typename RT_TYPE = RT<>>
using PTransportOpId = PTransportOp_BaseId<RT_TYPE, AhAOp>;


/*
 * NormalVectorsFilter.cpp
 *
 *  Created on: May 05, 2015
 *      Author: Peter Fankhauser, Martin Wermelinger
 *   Institute: ETH Zurich, ANYbotics
 */

#include <grid_map_filters/NormalVectorsFilter.hpp>

#include <grid_map_core/grid_map_core.hpp>
#include <pluginlib/class_list_macros.h>

#include <Eigen/Dense>
#include <stdexcept>

using namespace filters;

namespace grid_map {

template<typename T>
NormalVectorsFilter<T>::NormalVectorsFilter()
    : method_(Method::Raster),
      estimationRadius_(0.0)
{
}

template<typename T>
NormalVectorsFilter<T>::~NormalVectorsFilter()
{
}

template<typename T>
bool NormalVectorsFilter<T>::configure()
{
  if (!FilterBase<T>::getParam(std::string("radius"), estimationRadius_)) {
    ROS_DEBUG("Normal vectors filter did not find parameter `radius`.");
    method_ = Method::Raster;
  } else {
    method_ = Method::Area;
    if (estimationRadius_ < 0.0) {
      ROS_ERROR("Normal vectors filter estimation radius must be greater than zero.");
      return false;
    }
    ROS_DEBUG("Normal vectors estimation radius = %f", estimationRadius_);
  }

  std::string normalVectorPositiveAxis;
  if (!FilterBase<T>::getParam(std::string("normal_vector_positive_axis"), normalVectorPositiveAxis)) {
    ROS_ERROR("Normal vectors filter did not find parameter `normal_vector_positive_axis`.");
    return false;
  }
  if (normalVectorPositiveAxis == "z") {
    normalVectorPositiveAxis_ = Vector3::UnitZ();
  } else if (normalVectorPositiveAxis == "y") {
    normalVectorPositiveAxis_ = Vector3::UnitY();
  } else if (normalVectorPositiveAxis == "x") {
    normalVectorPositiveAxis_ = Vector3::UnitX();
  } else {
    ROS_ERROR("The normal vector positive axis '%s' is not valid.", normalVectorPositiveAxis.c_str());
    return false;
  }

  if (!FilterBase < T > ::getParam(std::string("input_layer"), inputLayer_)) {
    ROS_ERROR("Normal vectors filter did not find parameter `input_layer`.");
    return false;
  }
  ROS_DEBUG("Normal vectors filter input layer is = %s.", inputLayer_.c_str());

  if (!FilterBase < T > ::getParam(std::string("output_layers_prefix"), outputLayersPrefix_)) {
    ROS_ERROR("Normal vectors filter did not find parameter `output_layers_prefix`.");
    return false;
  }
  ROS_DEBUG("Normal vectors filter output_layer = %s.", outputLayersPrefix_.c_str());

  return true;
}

template<typename T>
bool NormalVectorsFilter<T>::update(const T& mapIn, T& mapOut)
{
  std::vector<std::string> normalVectorsLayers;
  normalVectorsLayers.push_back(outputLayersPrefix_ + "x");
  normalVectorsLayers.push_back(outputLayersPrefix_ + "y");
  normalVectorsLayers.push_back(outputLayersPrefix_ + "z");

  mapOut = mapIn;
  for (const auto& layer : normalVectorsLayers) mapOut.add(layer);
  switch (method_) {
    case Method::Area:
      computeWithArea(mapOut, inputLayer_, outputLayersPrefix_);
      break;
    case Method::Raster:
      computeWithRaster(mapOut, inputLayer_, outputLayersPrefix_);
      break;
  }

  return true;
}

template<typename T>
void NormalVectorsFilter<T>::computeWithArea(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix)
{
  // For each cell in requested area.
  for (GridMapIterator iterator(map);
      !iterator.isPastEnd(); ++iterator) {
    // Check if this is an empty cell (hole in the map).
    if (!map.isValid(*iterator, inputLayer_)) continue;

    // Requested position (center) of circle in map.
    Position center;
    map.getPosition(*iterator, center);

    // Prepare data computation.
    const int maxNumberOfCells = pow(ceil(2 * estimationRadius_ / map.getResolution()), 2);
    Eigen::MatrixXd points(3, maxNumberOfCells);

    // Gather surrounding data.
    size_t nPoints = 0;
    for (CircleIterator iterator(map, center, estimationRadius_); !iterator.isPastEnd(); ++iterator) {
      if (!map.isValid(*iterator, inputLayer_)) continue;
      Position3 point;
      map.getPosition3(inputLayer_, *iterator, point);
      points.col(nPoints) = point;
      nPoints++;
    }
    points.conservativeResize(3, nPoints); // TODO Eigen version?

    // Compute Eigenvectors.
    const Position3 mean = points.leftCols(nPoints).rowwise().sum() / nPoints;
    const Eigen::MatrixXd NN = points.leftCols(nPoints).colwise() - mean;

    const Eigen::Matrix3d covarianceMatrix(NN * NN.transpose());
    Vector3 eigenvalues = Vector3::Ones();
    Eigen::Matrix3d eigenvectors = Eigen::Matrix3d::Identity();
    // Ensure that the matrix is suited for eigenvalues calculation.
    if (covarianceMatrix.fullPivHouseholderQr().rank() >= 3) {
      const Eigen::EigenSolver<Eigen::MatrixXd> solver(covarianceMatrix);
      eigenvalues = solver.eigenvalues().real();
      eigenvectors = solver.eigenvectors().real();
    } else {
      ROS_DEBUG("Covariance matrix needed for eigen decomposition is degenerated. Expected cause: no noise in data (nPoints = %i)", (int) nPoints);
      // Use z-axis as default surface normal. // TODO Make dependend on surfaceNormalPositiveAxis_;
      eigenvalues.z() = 0.0;
    }
    // Keep the smallest Eigenvector as normal vector.
    int smallestId(0);
    double smallestValue(std::numeric_limits<double>::max());
    for (int j = 0; j < eigenvectors.cols(); j++) {
      if (eigenvalues(j) < smallestValue) {
        smallestId = j;
        smallestValue = eigenvalues(j);
      }
    }
    Vector3 eigenvector = eigenvectors.col(smallestId);
    if (eigenvector.dot(normalVectorPositiveAxis_) < 0.0) eigenvector = -eigenvector;
    map.at(outputLayersPrefix_ + "x", *iterator) = eigenvector.x();
    map.at(outputLayersPrefix_ + "y", *iterator) = eigenvector.y();
    map.at(outputLayersPrefix_ + "z", *iterator) = eigenvector.z();
  }
}

template<typename T>
void NormalVectorsFilter<T>::computeWithRaster(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix)
{
  throw std::runtime_error("NormalVectorsFilter::computeWithRaster() is not yet implemented!");
  // TODO: http://www.flipcode.com/archives/Calculating_Vertex_Normals_for_Height_Maps.shtml
}

} /* namespace */

PLUGINLIB_EXPORT_CLASS(grid_map::NormalVectorsFilter<grid_map::GridMap>, filters::FilterBase<grid_map::GridMap>)


// ========================================================================================
//  ApproxMVBB
//  Copyright (C) 2014 by Gabriel Nützi <nuetzig (at) imes (d0t) mavt (d0t) ethz
//  (døt) ch>
//
//  This Source Code Form is subject to the terms of the Mozilla Public
//  License, v. 2.0. If a copy of the MPL was not distributed with this
//  file, You can obtain one at http://mozilla.org/MPL/2.0/.
// ========================================================================================

#ifndef ApproxMVBB_Common_MyMatrixTypeDefs_hpp
#define ApproxMVBB_Common_MyMatrixTypeDefs_hpp

#include "ApproxMVBB/Common/Platform.hpp"

//#define EIGEN_DONT_VECTORIZE
//#define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT

#include <Eigen/Dense>

namespace ApproxMVBB
{
    // ================================================================================================
    /*! @brief This
     	These are some small matrix definitions.
    */

    namespace MyMatrix
    {
        template<typename Scalar>
        using Matrix44 = Eigen::Matrix<Scalar, 4, 4>;
        template<typename Scalar>
        using Matrix43 = Eigen::Matrix<Scalar, 4, 3>;
        template<typename Scalar>
        using Matrix34 = Eigen::Matrix<Scalar, 3, 4>;
        template<typename Scalar>
        using Matrix33 = Eigen::Matrix<Scalar, 3, 3>;
        template<typename Scalar>
        using Matrix32 = Eigen::Matrix<Scalar, 3, 2>;
        template<typename Scalar>
        using Matrix23 = Eigen::Matrix<Scalar, 2, 3>;
        template<typename Scalar>
        using Matrix22 = Eigen::Matrix<Scalar, 2, 2>;
        template<typename Scalar>
        using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
        template<typename Scalar>
        using Vector2 = Eigen::Matrix<Scalar, 2, 1>;

        template<typename Scalar>
        using Quaternion = Eigen::Quaternion<Scalar>;
        template<typename Scalar>
        using AngleAxis = Eigen::AngleAxis<Scalar>;

        template<typename Scalar>
        using Vector4 = Eigen::Matrix<Scalar, 4, 1>;
        template<typename Scalar>
        using Vector6 = Eigen::Matrix<Scalar, 6, 1>;
        template<typename Scalar>
        using VectorDyn = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;

        template<typename Scalar>
        using MatrixDynDyn = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
        template<typename Scalar>
        using MatrixDiagDyn = Eigen::DiagonalMatrix<Scalar, Eigen::Dynamic>;
        template<typename Scalar>
        using MatrixDynDynRow = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

        template<typename Scalar, int M>
        using MatrixStatDyn = Eigen::Matrix<Scalar, M, Eigen::Dynamic>;
        template<typename Scalar, int N>
        using MatrixDynStat = Eigen::Matrix<Scalar, Eigen::Dynamic, N>;
        template<typename Scalar, int M, int N>
        using MatrixStatStat = Eigen::Matrix<Scalar, M, N>;
        template<typename Scalar, int M>
        using VectorStat = Eigen::Matrix<Scalar, M, 1>;

        template<typename Scalar>
        using AffineTrafo = Eigen::Transform<Scalar, 3, Eigen::TransformTraits::Affine>;
        template<typename Scalar>
        using AffineTrafo2d = Eigen::Transform<Scalar, 2, Eigen::TransformTraits::Affine>;

        template<typename Scalar, int M>
        using ArrayStatDyn = Eigen::Array<Scalar, M, Eigen::Dynamic>;
        template<typename Scalar, int N>
        using ArrayDynStat = Eigen::Array<Scalar, Eigen::Dynamic, N>;
        template<typename Scalar, int M, int N>
        using ArrayStatStat = Eigen::Array<Scalar, M, N>;
        template<typename Scalar, int M>
        using ArrayStat = Eigen::Array<Scalar, M, 1>;

        template<typename Scalar>
        using Array3 = Eigen::Array<Scalar, 3, 1>;
        template<typename Scalar>
        using Array2 = Eigen::Array<Scalar, 2, 1>;
    }  // namespace MyMatrix

    namespace MyMatrix
    {
        template<typename Derived>
        using MatrixBase = Eigen::MatrixBase<Derived>;
        template<typename Derived>
        using ArrayBase = Eigen::ArrayBase<Derived>;

        template<typename Derived>
        using VectorBDyn = Eigen::VectorBlock<Derived, Eigen::Dynamic>;
        template<typename Derived, int M>
        using VectorBStat = Eigen::VectorBlock<Derived, M>;

        template<typename Derived>
        using MatrixBDynDyn = Eigen::Block<Derived>;
        template<typename Derived, int M>
        using MatrixBStatDyn = Eigen::Block<Derived, M, Eigen::Dynamic>;
        template<typename Derived, int N>
        using MatrixBDynStat = Eigen::Block<Derived, Eigen::Dynamic, N>;

        template<typename EigenType>
        using MatrixRef = Eigen::Ref<EigenType>;

        template<typename EigenType>
        using MatrixMap = Eigen::Map<EigenType>;
    }  // namespace MyMatrix

    struct APPROXMVBB_EXPORT MyMatrixIOFormat
    {
        static const Eigen::IOFormat Matlab;
        static const Eigen::IOFormat CommaSep;
        static const Eigen::IOFormat SpaceSep;
    };
}  // namespace ApproxMVBB

#    define ApproxMVBB_DEFINE_MATRIX_SPECIALTYPES                                \
        template<typename Derived>                                               \
        using MatrixBase = ApproxMVBB::MyMatrix::MatrixBase<Derived>;            \
        template<typename Derived>                                               \
        using ArrayBase = ApproxMVBB::MyMatrix::ArrayBase<Derived>;              \
                                                                                \
        template<typename Derived>                                               \
        using VectorBDyn = ApproxMVBB::MyMatrix::VectorBDyn<Derived>;            \
        template<typename Derived, int M>                                        \
        using VectorBStat = ApproxMVBB::MyMatrix::VectorBStat<Derived, M>;       \
                                                                                \
        template<typename Derived>                                               \
        using MatrixBDynDyn = ApproxMVBB::MyMatrix::MatrixBDynDyn<Derived>;      \
        template<typename Derived, int N>                                        \
        using MatrixBDynStat = ApproxMVBB::MyMatrix::MatrixBDynStat<Derived, N>; \
        template<typename Derived, int M>                                        \
        using MatrixBStatDyn = ApproxMVBB::MyMatrix::MatrixBStatDyn<Derived, M>; \
                                                                                \
        template<typename EigenType>                                             \
        using MatrixRef = ApproxMVBB::MyMatrix::MatrixRef<EigenType>;            \
        template<typename EigenType>                                             \
        using MatrixMap = ApproxMVBB::MyMatrix::MatrixMap<EigenType>

/**
     * @brief This macro is used to typedef all custom matrix types which have
     * nothing to do with the system.
     */
#    define ApproxMVBB_DEFINE_MATRIX_TYPES_OF(_PREC_)                              \
        using Matrix44        = ApproxMVBB::MyMatrix::Matrix44<_PREC_>;            \
        using Matrix33        = ApproxMVBB::MyMatrix::Matrix33<_PREC_>;            \
        using Matrix22        = ApproxMVBB::MyMatrix::Matrix22<_PREC_>;            \
        using Matrix32        = ApproxMVBB::MyMatrix::Matrix32<_PREC_>;            \
        using Matrix23        = ApproxMVBB::MyMatrix::Matrix23<_PREC_>;            \
        using Matrix43        = ApproxMVBB::MyMatrix::Matrix43<_PREC_>;            \
        using Matrix34        = ApproxMVBB::MyMatrix::Matrix34<_PREC_>;            \
        using Vector3         = ApproxMVBB::MyMatrix::Vector3<_PREC_>;             \
        using Vector2         = ApproxMVBB::MyMatrix::Vector2<_PREC_>;             \
        using Vector4         = ApproxMVBB::MyMatrix::Vector4<_PREC_>;             \
        using Vector6         = ApproxMVBB::MyMatrix::Vector6<_PREC_>;             \
        using Quaternion      = ApproxMVBB::MyMatrix::Quaternion<_PREC_>;          \
        using AngleAxis       = ApproxMVBB::MyMatrix::AngleAxis<_PREC_>;           \
        using VectorDyn       = ApproxMVBB::MyMatrix::VectorDyn<_PREC_>;           \
        using MatrixDynDyn    = ApproxMVBB::MyMatrix::MatrixDynDyn<_PREC_>;        \
        using MatrixDiagDyn   = ApproxMVBB::MyMatrix::MatrixDiagDyn<_PREC_>;       \
        using MatrixDynDynRow = ApproxMVBB::MyMatrix::MatrixDynDynRow<_PREC_>;     \
                                                                                \
        template<int M>                                                            \
        using MatrixStatDyn = ApproxMVBB::MyMatrix::MatrixStatDyn<_PREC_, M>;      \
        template<int N>                                                            \
        using MatrixDynStat = ApproxMVBB::MyMatrix::MatrixDynStat<_PREC_, N>;      \
        template<int M, int N>                                                     \
        using MatrixStatStat = ApproxMVBB::MyMatrix::MatrixStatStat<_PREC_, M, N>; \
        template<int M>                                                            \
        using VectorStat = ApproxMVBB::MyMatrix::VectorStat<_PREC_, M>;            \
                                                                                \
        using AffineTrafo   = ApproxMVBB::MyMatrix::AffineTrafo<_PREC_>;           \
        using AffineTrafo2d = ApproxMVBB::MyMatrix::AffineTrafo2d<_PREC_>;         \
                                                                                \
        template<int M>                                                            \
        using ArrayStatDyn = ApproxMVBB::MyMatrix::ArrayStatDyn<_PREC_, M>;        \
        template<int N>                                                            \
        using ArrayDynStat = ApproxMVBB::MyMatrix::ArrayDynStat<_PREC_, N>;        \
        template<int M, int N>                                                     \
        using ArrayStatStat = ApproxMVBB::MyMatrix::ArrayStatStat<_PREC_, M, N>;   \
        template<int M>                                                            \
        using ArrayStat = ApproxMVBB::MyMatrix::ArrayStat<_PREC_, M>;              \
        using Array3    = ApproxMVBB::MyMatrix::Array3<_PREC_>;                    \
        using Array2    = ApproxMVBB::MyMatrix::Array2<_PREC_>;                    \
                                                                                \
        ApproxMVBB_DEFINE_MATRIX_SPECIALTYPES

#endif


#pragma once
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <cmath>
#include <iostream>
#include <limits>

#define DBG_MACRO_NO_WARNING
#include <dbg.h>

namespace addn {
struct DN {
  double v;
  double a;

  DN(double _v, double _a) : v(_v), a(_a) {}
  DN(double _v) : DN(_v, 0.0) {}
  DN(const DN& x) : DN(x.v, x.a) {}
  DN() : DN(0.0, 0.0) {}
  inline DN& operator=(const DN& x) {
    v = x.v;
    a = x.a;
    return *this;
  }

  inline DN& operator=(const double x) {
    v = x;
    a = 0;
    return *this;
  }

  inline DN& operator+=(const DN& x) {
    v += x.v;
    a += x.a;
    return *this;
  }

  inline DN& operator/=(const DN& x) {
    a = (a * x.v - x.a * v) / std::pow(x.v, 2.0);
    v /= x.v;
    return *this;
  }

  operator double() const { return v; }
};

inline std::ostream& operator<<(std::ostream& out, const DN& n) {
  out << "(" << n.v << ", " << n.a << ")";
  return out;
}

inline DN operator-(const DN& x) { return DN{-x.v, -x.a}; }
inline DN operator+(const DN& x, const DN& y) { return DN{x.v + y.v, x.a + y.a}; }
inline DN operator-(const DN& x, const DN& y) { return DN{x.v - y.v, x.a - y.a}; }
inline DN operator*(const DN& x, const DN& y) { return DN{x.v * y.v, x.a * y.v + y.a * x.v}; }
inline DN operator/(const DN& x, const DN& y) {
  return DN{x.v / y.v, (x.a * y.v - y.a * x.v) / std::pow(y.v, 2.0)};
}

inline bool operator==(const DN& x, const DN& y) { return x.v == y.v && x.a == y.a; }
inline bool operator==(const DN& x, const double y) { return x.v == y && x.a == 0.0; }
inline bool operator==(const double x, const DN& y) { return x == y.v && 0.0 == y.a; }
inline bool operator!=(const DN& x, const DN& y) { return x.v != y.v || x.a != y.a; }
inline bool operator!=(const DN& x, const double y) { return x.v != y || x.a != 0.0; }
inline bool operator!=(const double x, const DN& y) { return x != y.v || 0.0 != y.a; }
inline bool operator<(const DN& x, const DN& y) { return x.v < y.v; }
inline bool operator<(const DN& x, const double y) { return x.v < y; }
inline bool operator<(const double x, const DN& y) { return x < y; }
inline bool operator>(const DN& x, const DN& y) { return x.v > y.v; }
inline bool operator>(const DN& x, const double y) { return x.v > y; }
inline bool operator>(const double x, const DN& y) { return x > y.v; }
inline bool operator<=(const DN& x, const DN& y) { return x.v <= y.v; }
inline bool operator<=(const DN& x, const double y) { return x.v <= y; }
inline bool operator<=(const double x, const DN& y) { return x <= y.v; }
inline bool operator>=(const DN& x, const DN& y) { return x.v >= y.v; }
inline bool operator>=(const DN& x, const double y) { return x.v >= y; }
inline bool operator>=(const double x, const DN& y) { return x >= y.v; }

inline DN pow(const DN& x, const DN& y) {
  const auto powv = std::pow(x.v, y.v);
  return DN{powv, powv * (std::log(x.v) * y.a + y.v / x.v * x.a)};
}

inline DN pow(const DN& x, const double y) {
  return DN{std::pow(x.v, y), y * std::pow(x.v, y - 1.0) * x.a};
}

inline DN pow(const double x, const DN& y) {
  const auto powv = std::pow(x, y.v);
  return DN{powv, powv * std::log(x) * y.a};
}

inline DN sin(const DN& x) { return DN{std::sin(x.v), x.a * std::cos(x.v)}; }
inline DN cos(const DN& x) { return DN{std::cos(x.v), -x.a * std::sin(x.v)}; }
inline DN tan(const DN& x) {
  const auto tanv = std::tan(x.v);
  return DN{tanv, x.a * (1 + std::pow(tanv, 2.0))};
}

inline DN asin(const DN& x) { return DN{std::asin(x.v), x.a / std::sqrt(1 - std::pow(x.v, 2.0))}; }

inline DN acos(const DN& x) {
  return DN{std::acos(x.v), -x.a / std::sqrt(1 - std::pow(x.v, 2.0))};
}

inline DN atan(const DN& x) {
  return DN{std::atan(x.v), x.a * (1 - std::pow(std::tanh(x.v), 2.0))};
}

inline DN atan2(const DN& y, const DN& x) {
  const DN q = y / x;
  return DN{std::atan2(y.v, x.v), q.a * (1 - std::pow(std::tanh(q.v), 2.0))};
}

inline DN atan2(const DN& y, const double x) {
  return DN{std::atan2(y.v, x), y.a * (1 - std::pow(std::tanh(y.v / x), 2.0))};
}

inline DN atan2(const double x, const DN& y) { return atan2(y, x); }

inline DN exp(const DN& x) { return DN{std::exp(x.v), x.a * std::exp(x.v)}; }
inline DN log(const DN& x) { return DN{std::log(x.v), x.a / x.v}; }
inline DN sqrt(const DN& x) {
  const double v_sqrt = std::sqrt(x.v);
  const double denom  = 2 * v_sqrt;
  return DN{v_sqrt, denom == 0 ? 0.0 : x.a / denom};
}

inline DN abs(const DN& x) { return DN{std::fabs(x.v), std::copysign(x.a, x.v)}; }
inline DN abs2(const DN& x) { return x * x; }
inline DN min(const DN& x, const DN& y) {
  if (std::max(std::fabs(x.v), std::fabs(y.v)) == std::numeric_limits<double>::infinity()) {
    return x <= y ? x : y;
  }

  const auto z = x >= y ? 1.0 : 0.0;
  return DN{z * y.v + (1.0 - z) * x.v, z * y.a + (1.0 - z) * x.a};
}

inline DN max(const DN& x, const DN& y) {
  if (std::max(std::fabs(x.v), std::fabs(y.v)) == std::numeric_limits<double>::infinity()) {
    return x >= y ? x : y;
  }

  const auto z = y >= x ? 1.0 : 0.0;
  return DN{z * y.v + (1.0 - z) * x.v, z * y.a + (1.0 - z) * x.a};
}

inline DN operator+(const DN& x, const double y) { return DN{x.v + y, x.a}; }
inline DN operator+(const double x, const DN& y) { return DN{x + y.v, y.a}; }
inline DN operator-(const DN& x, const double y) { return DN{x.v - y, x.a}; }
inline DN operator-(const double x, const DN& y) { return DN{x - y.v, -y.a}; }
inline DN operator*(const DN& x, const double y) { return DN{x.v * y, x.a * y}; }
inline DN operator*(const double x, const DN& y) { return DN{x * y.v, y.a * x}; }
inline DN operator/(const DN& x, const double y) {
  return DN{x.v / y, (x.a * y) / std::pow(y, 2.0)};
}

inline DN operator/(const double x, const DN& y) {
  return DN{x / y.v, (-y.a * x) / std::pow(y.v, 2.0)};
}

inline DN ceil(const DN& x) { return DN{std::ceil(x.v), 0.0}; }
inline DN floor(const DN& x) { return DN{std::floor(x.v), 0.0}; }

inline DN cosh(const DN& x) { return DN{std::cosh(x.v), x.a * std::sinh(x.v)}; }
inline DN sinh(const DN& x) { return DN{std::sinh(x.v), x.a * std::cosh(x.v)}; }
inline DN tanh(const DN& x) {
  return DN{std::tanh(x.v), x.a * (1.0 - std::pow(std::tanh(x.v), 2))};
}
}  // namespace addn

namespace Eigen {
template <> struct NumTraits<addn::DN> : NumTraits<double> {
  using Real       = addn::DN;
  using NonInteger = addn::DN;
  using Literal    = addn::DN;
  using Nested     = addn::DN;

  enum {
    IsComplex             = 0,
    IsInteger             = 0,
    IsSigned              = 1,
    RequireInitialization = 1,
    ReadCost              = 1,
    AddCost               = 2,
    MulCost               = 4
  };
};
}  // namespace Eigen


/**
 * @file solver_builder.hpp
 * @author François Hamonic (francois.hamonic@gmail.com)
 * @brief OSI_Builder class declaration
 * @version 0.1
 * @date 2020-10-27
 *
 * @copyright Copyright (c) 2020
 */
#ifndef SOLVER_BUILDER_HPP
#define SOLVER_BUILDER_HPP

#include <cmath>
#include <functional>
#include <memory>
#include <numeric>
#include <string>
#include <vector>

#include <boost/iterator/zip_iterator.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/range/combine.hpp>

#include <range/v3/all.hpp>

namespace SolverBuilder_Utils {
enum InequalitySense { LESS = -1, EQUAL = 0, GREATER = 1 };
enum OptimizationSense { MIN = -1, MAX = 1 };
constexpr double INFTY = std::numeric_limits<double>::max();

class LinearExpression {
private:
    double _constant;
    std::vector<int> _indices;
    std::vector<double> _coefficients;

public:
    LinearExpression() : _constant{0} {}

    void add(double c) { _constant += c; }
    void add(int id, double coef = 1) {
        _indices.push_back(id);
        _coefficients.push_back(coef);
    }

    double getConstant() const { return _constant; }
    int getNbTerms() const { return _indices.size(); }
    int * getIndicesData() { return _indices.data(); }
    double * getCoefficientsData() { return _coefficients.data(); }
    const std::vector<int> & getIndices() const { return _indices; }
    const std::vector<double> & getCoefficients() const {
        return _coefficients;
    }

    LinearExpression & simplify() {
        auto zip_view = ranges::view::zip(_indices, _coefficients);
        ranges::sort(zip_view,
                     [](auto p1, auto p2) { return p1.first < p2.first; });

        const auto begin = zip_view.begin();
        auto first = begin;
        const auto end = zip_view.end();
        for(auto next = first + 1; next != end; ++next) {
            if((*first).first != (*next).first) {
                if((*first).second != 0.0) ++first;
                *first = *next;
                continue;
            }
            (*first).second += (*next).second;
        }
        const std::size_t new_length = std::distance(begin, first + 1);
        _indices.resize(new_length);
        _coefficients.resize(new_length);
        return *this;
    }
};

struct linear_ineq_constraint {
    InequalitySense sense;
    LinearExpression linear_expression;
    linear_ineq_constraint() = default;
};

class linear_ineq_constraint_rhs_easy_init {
private:
    linear_ineq_constraint constraint_data;

public:
    linear_ineq_constraint_rhs_easy_init(linear_ineq_constraint && data)
        : constraint_data(std::move(data)) {}
    linear_ineq_constraint_rhs_easy_init & operator()(double c) {
        constraint_data.linear_expression.add(-c);
        return *this;
    }
    linear_ineq_constraint_rhs_easy_init & operator()(int id, double coef = 1) {
        constraint_data.linear_expression.add(id, -coef);
        return *this;
    }
};

class linear_ineq_constraint_lhs_easy_init {
private:
    linear_ineq_constraint constraint_data;

public:
    linear_ineq_constraint_lhs_easy_init() {}
    linear_ineq_constraint_lhs_easy_init & operator()(double c) {
        constraint_data.linear_expression.add(c);
        return *this;
    }
    linear_ineq_constraint_lhs_easy_init & operator()(int id, double coef = 1) {
        constraint_data.linear_expression.add(id, coef);
        return *this;
    }

    linear_ineq_constraint_rhs_easy_init less() {
        constraint_data.sense = LESS;
        return linear_ineq_constraint_rhs_easy_init(std::move(constraint_data));
    }
    linear_ineq_constraint_rhs_easy_init equal() {
        constraint_data.sense = EQUAL;
        return linear_ineq_constraint_rhs_easy_init(std::move(constraint_data));
    }
    linear_ineq_constraint_rhs_easy_init greater() {
        constraint_data.sense = GREATER;
        return linear_ineq_constraint_rhs_easy_init(std::move(constraint_data));
    }
};

struct linear_range_constraint {
    double lower_bound, upper_bound;
    LinearExpression linear_expression;
    linear_range_constraint()
        : lower_bound{std::numeric_limits<double>::min()}
        , upper_bound{std::numeric_limits<double>::max()} {}
};

class linear_range_constraint_lhs_easy_init {
private:
    linear_range_constraint constraint_data;

public:
    linear_range_constraint_lhs_easy_init() {}
    linear_range_constraint_lhs_easy_init & lower(double c) {
        constraint_data.lower_bound = c;
        return *this;
    }
    linear_range_constraint_lhs_easy_init & upper(double c) {
        constraint_data.upper_bound = c;
        return *this;
    }
    linear_range_constraint_lhs_easy_init & operator()(int id,
                                                       double coef = 1) {
        constraint_data.linear_expression.add(id, coef);
        return *this;
    }

    linear_range_constraint take_data() { return std::move(constraint_data); }
};

class QuadraticExpression {
private:
    LinearExpression _linear_expression;
    std::vector<int> _quad_indices_1;
    std::vector<int> _quad_indices_2;
    std::vector<double> quad_coefficients;

public:
    void add(double c) { _linear_expression.add(c); }
    void add(int id, double coef = 1) { _linear_expression.add(id, coef); }
    void add(int id_1, int id_2, double coef = 1) {
        _quad_indices_1.push_back(id_1);
        _quad_indices_2.push_back(id_2);
        quad_coefficients.push_back(coef);
    }

    const LinearExpression & getLineraExpression() const {
        return _linear_expression;
    }

    double getConstant() const { return _linear_expression.getConstant(); }

    int getNbLinearTerms() const { return _linear_expression.getNbTerms(); }
    int * getLinearIndicesData() { return _linear_expression.getIndicesData(); }
    double * getLinearCoefficientsData() {
        return _linear_expression.getCoefficientsData();
    }
    const std::vector<int> & getLinearIndices() const {
        return _linear_expression.getIndices();
    }
    const std::vector<double> & getLinearCoefficients() const {
        return _linear_expression.getCoefficients();
    }

    int getNbQuadTerms() const { return _quad_indices_1.size(); }
    int * getQuadIndices1Data() { return _quad_indices_1.data(); }
    int * getQuadIndices2Data() { return _quad_indices_2.data(); }
    double * getQuadCoefficientsData() { return quad_coefficients.data(); }
    const std::vector<int> & getQuadIndices1() const { return _quad_indices_1; }
    const std::vector<int> & getQuadIndices2() const { return _quad_indices_2; }
    const std::vector<double> & getQuadCoefficients() const {
        return quad_coefficients;
    }

    bool isLinear() const { return _quad_indices_1.empty(); }

    QuadraticExpression & simplify() {
        _linear_expression.simplify();
        return *this;
    }
};

struct quadratic_ineq_constraint {
    InequalitySense sense;
    QuadraticExpression quadratic_expression;
    quadratic_ineq_constraint() = default;
};

class quadratic_ineq_constraint_rhs_easy_init {
private:
    quadratic_ineq_constraint constraint_data;

public:
    quadratic_ineq_constraint_rhs_easy_init(quadratic_ineq_constraint && data)
        : constraint_data(std::move(data)) {}
    quadratic_ineq_constraint_rhs_easy_init & operator()(double c) {
        constraint_data.quadratic_expression.add(-c);
        return *this;
    }
    quadratic_ineq_constraint_rhs_easy_init & operator()(int id,
                                                         double coef = 1) {
        constraint_data.quadratic_expression.add(id, -coef);
        return *this;
    }
    quadratic_ineq_constraint_rhs_easy_init & operator()(int id_1, int id_2,
                                                         double coef = 1) {
        constraint_data.quadratic_expression.add(id_1, id_2, -coef);
        return *this;
    }

    quadratic_ineq_constraint take_data() { return std::move(constraint_data); }
};

class quadratic_ineq_constraint_lhs_easy_init {
private:
    quadratic_ineq_constraint constraint_data;

public:
    quadratic_ineq_constraint_lhs_easy_init() {}
    quadratic_ineq_constraint_lhs_easy_init & operator()(double c) {
        constraint_data.quadratic_expression.add(c);
        return *this;
    }
    quadratic_ineq_constraint_lhs_easy_init & operator()(int id,
                                                         double coef = 1) {
        constraint_data.quadratic_expression.add(id, coef);
        return *this;
    }
    quadratic_ineq_constraint_lhs_easy_init & operator()(int id_1, int id_2,
                                                         double coef = 1) {
        constraint_data.quadratic_expression.add(id_1, id_2, coef);
        return *this;
    }

    quadratic_ineq_constraint_rhs_easy_init less() {
        constraint_data.sense = LESS;
        return quadratic_ineq_constraint_rhs_easy_init(
            std::move(constraint_data));
    }
    quadratic_ineq_constraint_rhs_easy_init equal() {
        constraint_data.sense = EQUAL;
        return quadratic_ineq_constraint_rhs_easy_init(
            std::move(constraint_data));
    }
    quadratic_ineq_constraint_rhs_easy_init greater() {
        constraint_data.sense = GREATER;
        return quadratic_ineq_constraint_rhs_easy_init(
            std::move(constraint_data));
    }
};

}  // namespace SolverBuilder_Utils

// using namespace SolverBuilder_Utils;

// /**
//  * @brief A practical class for building OsiSolver instances
//  */
// class SolverBuilder {
//     public:
//         class VarType {
//             protected:
//                 int begin_id;
//                 int end_id;
//                 double _default_lb;
//                 double _default_ub;
//                 bool _integer;
//                 VarType(int number, double lb=0, double ub=INFTY, bool
//                 integer=false) : begin_id(0), end_id(number),
//                 _default_lb(lb), _default_ub(ub), _integer(integer) {}
//                 VarType() : VarType(0) {}
//             public:
//                 void offsetIds(int offset) { begin_id += offset; end_id+=
//                 offset; } int getNumber() const { return end_id - begin_id; }
//                 double getDefaultLB() { return _default_lb; }
//                 double getDefaultUB() { return _default_ub; }
//                 bool isInteger() { return _integer; }
//         };
//     private:
//         int nb_vars;
//         std::vector<VarType*> varTypes;

//         std::unique_ptr<double[]> objective;
//         std::unique_ptr<double[]> col_lb;
//         std::unique_ptr<double[]> col_ub;
//         std::unique_ptr<std::string[]> col_names;

//         std::vector<int> starts;
//         std::vector<int> indices;
//         std::vector<double> coefficients;

//         std::vector<double> row_lb;
//         std::vector<double> row_ub;

//         std::vector<int> integers_variables;
//     public:
//         // OSI_Builder();
//         // ~OSI_Builder();

//         // OSI_Builder & addVarType(VarType * var_type);
//         // void init();
//         // OSI_Builder & setObjective(int  var_id, double coef);
//         // OSI_Builder & setBounds(int  var_id, double lb, double ub);
//         // OSI_Builder & buffEntry(int  var_id, double coef);
//         // OSI_Builder & popEntryBuffer();
//         // OSI_Builder & clearEntryBuffer();
//         // OSI_Builder & pushRowWithoutClearing(double lb, double ub);
//         // OSI_Builder & pushRow(double lb, double ub);
//         // OSI_Builder & setColName(int var_id, std::string name);

//         // OSI_Builder & setContinuous(int var_id);
//         // OSI_Builder & setInteger(int var_id);

//         template <class OsiSolver>
//         OsiSolver * buildSolver(int sense, bool relaxed=false) {
//             OsiSolver * solver = new OsiSolver();
//             solver->loadProblem(*matrix, col_lb, col_ub, objective,
//             row_lb.data(), row_ub.data()); solver->setObjSense(sense);
//             if(relaxed)
//                 return solver;
//             for(int i : integers_variables)
//                 solver->setInteger(i);
//             solver->setColNames(colNames, 0, nb_vars, 0);
//             return solver;
//         }

//         static int nb_pairs(int n) {
//             return n*(n-1)/2;
//         };
//         static int nb_couples(int n) {
//             return 2*nb_pairs(n);
//         };

//         static int compose_pair(int i, int j) {
//             assert(i != j);
//             if(i > j) std::swap(i,j);
//             return nb_pairs(j)+i;
//         };
//         static std::pair<int,int> retrieve_pair(int id) {
//             const int j = std::round(std::sqrt(2*id+1));
//             const int i = id - nb_pairs(j);
//             return std::make_pair(i,j);
//         };
//         static int compose_couple(int i, int j) {
//             return 2*compose_pair(i, j) + (i < j ? 0 : 1);
//         };
//         static std::pair<int,int> retrieve_couple(int id) {
//             std::pair<int,int> p = retrieve_pair(id / 2);
//             if(id % 2 == 1) std::swap(p.first, p.second);
//             return p;
//         };

//         const std::vector<VarType*> & getVarTypes() { return varTypes; }

//         int getNbVars() const { return nb_vars; };

//         int getNbNonZeroVars() const {
//             std::vector<int> non_zero(nb_vars, 0);
//             const int * indices = matrix->getIndices();
//             for(int i=0; i<matrix->getNumElements(); ++i)
//                 non_zero[indices[i]] = 1;
//             return std::accumulate(non_zero.begin(), non_zero.end(), 0);
//         };
//         int getNbConstraints() const { return matrix->getNumRows(); };

//         int getNbElems() const { return nb_entries; };

//         double * getObjective() { return objective; }
//         CoinPackedMatrix * getMatrix() { return matrix; }

//         double * getColLB() { return col_lb; }
//         double * getColUB() { return col_ub; }

//         double * getRowLB() { return row_lb.data(); }
//         double * getRowUB() { return row_ub.data(); }
// };

#endif  // OSICLPSOLVER_BUILDER_HPP


/*
 * Copyright Nick Thompson, 2018
 * Use, modification and distribution are subject to the
 * Boost Software License, Version 1.0. (See accompanying file
 * LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
 */
#ifndef BOOST_MATH_QUADRATURE_NAIVE_MONTE_CARLO_HPP
#define BOOST_MATH_QUADRATURE_NAIVE_MONTE_CARLO_HPP
#include <sstream>
#include <algorithm>
#include <vector>
#include <atomic>
#include <memory>
#include <functional>
#include <future>
#include <thread>
#include <initializer_list>
#include <utility>
#include <random>
#include <chrono>
#include <map>
#include <type_traits>
#include <boost/math/policies/error_handling.hpp>

namespace boost { namespace math { namespace quadrature {

namespace detail {
  enum class limit_classification {FINITE,
                                   LOWER_BOUND_INFINITE,
                                   UPPER_BOUND_INFINITE,
                                   DOUBLE_INFINITE};
}

template<class Real, class F, class RandomNumberGenerator = std::mt19937_64, class Policy = boost::math::policies::policy<>,
         typename std::enable_if<std::is_trivially_copyable<Real>::value, bool>::type = true>
class naive_monte_carlo
{
public:
    naive_monte_carlo(const F& integrand,
                      std::vector<std::pair<Real, Real>> const & bounds,
                      Real error_goal,
                      bool singular = true,
                      uint64_t threads = std::thread::hardware_concurrency(),
                      uint64_t seed = 0) noexcept : m_num_threads{threads}, m_seed{seed}
    {
        using std::numeric_limits;
        using std::sqrt;
        uint64_t n = bounds.size();
        m_lbs.resize(n);
        m_dxs.resize(n);
        m_limit_types.resize(n);
        m_volume = 1;
        static const char* function = "boost::math::quadrature::naive_monte_carlo<%1%>";
        for (uint64_t i = 0; i < n; ++i)
        {
            if (bounds[i].second <= bounds[i].first)
            {
                boost::math::policies::raise_domain_error(function, "The upper bound is <= the lower bound.\n", bounds[i].second, Policy());
                return;
            }
            if (bounds[i].first == -numeric_limits<Real>::infinity())
            {
                if (bounds[i].second == numeric_limits<Real>::infinity())
                {
                    m_limit_types[i] = detail::limit_classification::DOUBLE_INFINITE;
                }
                else
                {
                    m_limit_types[i] = detail::limit_classification::LOWER_BOUND_INFINITE;
                    // Ok ok this is bad to use the second bound as the lower limit and then reflect.
                    m_lbs[i] = bounds[i].second;
                    m_dxs[i] = numeric_limits<Real>::quiet_NaN();
                }
            }
            else if (bounds[i].second == numeric_limits<Real>::infinity())
            {
                m_limit_types[i] = detail::limit_classification::UPPER_BOUND_INFINITE;
                if (singular)
                {
                    // I've found that it's easier to sample on a closed set and perturb the boundary
                    // than to try to sample very close to the boundary.
                    m_lbs[i] = std::nextafter(bounds[i].first, (std::numeric_limits<Real>::max)());
                }
                else
                {
                    m_lbs[i] = bounds[i].first;
                }
                m_dxs[i] = numeric_limits<Real>::quiet_NaN();
            }
            else
            {
                m_limit_types[i] = detail::limit_classification::FINITE;
                if (singular)
                {
                    if (bounds[i].first == 0)
                    {
                        m_lbs[i] = std::numeric_limits<Real>::epsilon();
                    }
                    else
                    {
                        m_lbs[i] = std::nextafter(bounds[i].first, (std::numeric_limits<Real>::max)());
                    }

                    m_dxs[i] = std::nextafter(bounds[i].second, std::numeric_limits<Real>::lowest()) - m_lbs[i];
                }
                else
                {
                    m_lbs[i] = bounds[i].first;
                    m_dxs[i] = bounds[i].second - bounds[i].first;
                }
                m_volume *= m_dxs[i];
            }
        }

        m_integrand = [this, &integrand](std::vector<Real> & x)->Real
        {
            Real coeff = m_volume;
            for (uint64_t i = 0; i < x.size(); ++i)
            {
                // Variable transformation are listed at:
                // https://en.wikipedia.org/wiki/Numerical_integration
                // However, we've made some changes to these so that we can evaluate on a compact domain.
                if (m_limit_types[i] == detail::limit_classification::FINITE)
                {
                    x[i] = m_lbs[i] + x[i]*m_dxs[i];
                }
                else if (m_limit_types[i] == detail::limit_classification::UPPER_BOUND_INFINITE)
                {
                    Real t = x[i];
                    Real z = 1/(1 + numeric_limits<Real>::epsilon() - t);
                    coeff *= (z*z)*(1 + numeric_limits<Real>::epsilon());
                    x[i] = m_lbs[i] + t*z;
                }
                else if (m_limit_types[i] == detail::limit_classification::LOWER_BOUND_INFINITE)
                {
                    Real t = x[i];
                    Real z = 1/(t+sqrt((numeric_limits<Real>::min)()));
                    coeff *= (z*z);
                    x[i] = m_lbs[i] + (t-1)*z;
                }
                else
                {
                    Real t1 = 1/(1+numeric_limits<Real>::epsilon() - x[i]);
                    Real t2 = 1/(x[i]+numeric_limits<Real>::epsilon());
                    x[i] = (2*x[i]-1)*t1*t2/4;
                    coeff *= (t1*t1+t2*t2)/4;
                }
            }
            return coeff*integrand(x);
        };

        // If we don't do a single function call in the constructor,
        // we can't do a restart.
        std::vector<Real> x(m_lbs.size());

        // If the seed is zero, that tells us to choose a random seed for the user:
        if (seed == 0)
        {
            std::random_device rd;
            seed = rd();
        }

        RandomNumberGenerator gen(seed);
        Real inv_denom = 1/static_cast<Real>(((gen.max)()-(gen.min)()));

        m_num_threads = (std::max)(m_num_threads, (uint64_t) 1);
        m_thread_calls.reset(new std::atomic<uint64_t>[threads]);
        m_thread_Ss.reset(new std::atomic<Real>[threads]);
        m_thread_averages.reset(new std::atomic<Real>[threads]);

        Real avg = 0;
        for (uint64_t i = 0; i < m_num_threads; ++i)
        {
            for (uint64_t j = 0; j < m_lbs.size(); ++j)
            {
                x[j] = (gen()-(gen.min)())*inv_denom;
            }
            Real y = m_integrand(x);
            m_thread_averages[i] = y; // relaxed store
            m_thread_calls[i] = 1;
            m_thread_Ss[i] = 0;
            avg += y;
        }
        avg /= m_num_threads;
        m_avg = avg; // relaxed store

        m_error_goal = error_goal; // relaxed store
        m_start = std::chrono::system_clock::now();
        m_done = false; // relaxed store
        m_total_calls = m_num_threads;  // relaxed store
        m_variance = (numeric_limits<Real>::max)();
    }

    std::future<Real> integrate()
    {
        // Set done to false in case we wish to restart:
        m_done.store(false); // relaxed store, no worker threads yet
        m_start = std::chrono::system_clock::now();
        return std::async(std::launch::async,
                          &naive_monte_carlo::m_integrate, this);
    }

    void cancel()
    {
        // If seed = 0 (meaning have the routine pick the seed), this leaves the seed the same.
        // If seed != 0, then the seed is changed, so a restart doesn't do the exact same thing.
        m_seed = m_seed*m_seed;
        m_done = true; // relaxed store, worker threads will get the message eventually
        // Make sure the error goal is infinite, because otherwise we'll loop when we do the final error goal check:
        m_error_goal = (std::numeric_limits<Real>::max)();
    }

    Real variance() const
    {
        return m_variance.load();
    }

    Real current_error_estimate() const
    {
        using std::sqrt;
        //
        // There is a bug here: m_variance and m_total_calls get updated asynchronously
        // and may be out of synch when we compute the error estimate, not sure if it matters though...
        //
        return sqrt(m_variance.load()/m_total_calls.load());
    }

    std::chrono::duration<Real> estimated_time_to_completion() const
    {
        auto now = std::chrono::system_clock::now();
        std::chrono::duration<Real> elapsed_seconds = now - m_start;
        Real r = this->current_error_estimate()/m_error_goal.load(); // relaxed load
        if (r*r <= 1) {
            return 0*elapsed_seconds;
        }
        return (r*r - 1)*elapsed_seconds;
    }

    void update_target_error(Real new_target_error)
    {
        m_error_goal = new_target_error;  // relaxed store
    }

    Real progress() const
    {
        Real r = m_error_goal.load()/this->current_error_estimate();  // relaxed load
        if (r*r >= 1)
        {
            return 1;
        }
        return r*r;
    }

    Real current_estimate() const
    {
        return m_avg.load();
    }

    uint64_t calls() const
    {
        return m_total_calls.load();  // relaxed load
    }

private:

   Real m_integrate()
   {
      uint64_t seed;
      // If the user tells us to pick a seed, pick a seed:
      if (m_seed == 0)
      {
         std::random_device rd;
         seed = rd();
      }
      else // use the seed we are given:
      {
         seed = m_seed;
      }
      RandomNumberGenerator gen(seed);
      int max_repeat_tries = 5;
      do{

         if (max_repeat_tries < 5)
         {
            m_done = false;

#ifdef BOOST_NAIVE_MONTE_CARLO_DEBUG_FAILURES
            std::cout << "Failed to achieve required tolerance first time through..\n";
            std::cout << "  variance =    " << m_variance << std::endl;
            std::cout << "  average =     " << m_avg << std::endl;
            std::cout << "  total calls = " << m_total_calls << std::endl;

            for (std::size_t i = 0; i < m_num_threads; ++i)
               std::cout << "  thread_calls[" << i << "] = " << m_thread_calls[i] << std::endl;
            for (std::size_t i = 0; i < m_num_threads; ++i)
               std::cout << "  thread_averages[" << i << "] = " << m_thread_averages[i] << std::endl;
            for (std::size_t i = 0; i < m_num_threads; ++i)
               std::cout << "  thread_Ss[" << i << "] = " << m_thread_Ss[i] << std::endl;
#endif
         }

         std::vector<std::thread> threads(m_num_threads);
         for (uint64_t i = 0; i < threads.size(); ++i)
         {
            threads[i] = std::thread(&naive_monte_carlo::m_thread_monte, this, i, gen());
         }
         do {
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            uint64_t total_calls = 0;
            for (uint64_t i = 0; i < m_num_threads; ++i)
            {
               uint64_t t_calls = m_thread_calls[i].load(std::memory_order_consume);
               total_calls += t_calls;
            }
            Real variance = 0;
            Real avg = 0;
            for (uint64_t i = 0; i < m_num_threads; ++i)
            {
               uint64_t t_calls = m_thread_calls[i].load(std::memory_order_consume);
               // Will this overflow? Not hard to remove . . .
               avg += m_thread_averages[i].load(std::memory_order_relaxed)*((Real)t_calls / (Real)total_calls);
               variance += m_thread_Ss[i].load(std::memory_order_relaxed);
            }
            m_avg.store(avg, std::memory_order_release);
            m_variance.store(variance / (total_calls - 1), std::memory_order_release);
            m_total_calls = total_calls; // relaxed store, it's just for user feedback
            // Allow cancellation:
            if (m_done) // relaxed load
            {
               break;
            }
         } while (m_total_calls < 2048 || this->current_error_estimate() > m_error_goal.load(std::memory_order_consume));
         // Error bound met; signal the threads:
         m_done = true; // relaxed store, threads will get the message in the end
         std::for_each(threads.begin(), threads.end(),
            std::mem_fn(&std::thread::join));
         if (m_exception)
         {
            std::rethrow_exception(m_exception);
         }
         // Incorporate their work into the final estimate:
         uint64_t total_calls = 0;
         for (uint64_t i = 0; i < m_num_threads; ++i)
         {
            uint64_t t_calls = m_thread_calls[i].load(std::memory_order_consume);
            total_calls += t_calls;
         }
         Real variance = 0;
         Real avg = 0;

         for (uint64_t i = 0; i < m_num_threads; ++i)
         {
            uint64_t t_calls = m_thread_calls[i].load(std::memory_order_consume);
            // Averages weighted by the number of calls the thread made:
            avg += m_thread_averages[i].load(std::memory_order_relaxed)*((Real)t_calls / (Real)total_calls);
            variance += m_thread_Ss[i].load(std::memory_order_relaxed);
         }
         m_avg.store(avg, std::memory_order_release);
         m_variance.store(variance / (total_calls - 1), std::memory_order_release);
         m_total_calls = total_calls; // relaxed store, this is just user feedback

         // Sometimes, the master will observe the variance at a very "good" (or bad?) moment,
         // Then the threads proceed to find the variance is much greater by the time they hear the message to stop.
         // This *WOULD* make sure that the final error estimate is within the error bounds.
      }
      while ((--max_repeat_tries >= 0) && (this->current_error_estimate() > m_error_goal));

      return m_avg.load(std::memory_order_consume);
    }

    void m_thread_monte(uint64_t thread_index, uint64_t seed)
    {
        using std::numeric_limits;
        try
        {
            std::vector<Real> x(m_lbs.size());
            RandomNumberGenerator gen(seed);
            Real inv_denom = (Real) 1/(Real)( (gen.max)() - (gen.min)()  );
            Real M1 = m_thread_averages[thread_index].load(std::memory_order_consume);
            Real S = m_thread_Ss[thread_index].load(std::memory_order_consume);
            // Kahan summation is required or the value of the integrand will go on a random walk during long computations.
            // See the implementation discussion.
            // The idea is that the unstabilized additions have error sigma(f)/sqrt(N) + epsilon*N, which diverges faster than it converges!
            // Kahan summation turns this to sigma(f)/sqrt(N) + epsilon^2*N, and the random walk occurs on a timescale of 10^14 years (on current hardware)
            Real compensator = 0;
            uint64_t k = m_thread_calls[thread_index].load(std::memory_order_consume);
            while (!m_done) // relaxed load
            {
                int j = 0;
                // If we don't have a certain number of calls before an update, we can easily terminate prematurely
                // because the variance estimate is way too low. This magic number is a reasonable compromise, as 1/sqrt(2048) = 0.02,
                // so it should recover 2 digits if the integrand isn't poorly behaved, and if it is, it should discover that before premature termination.
                // Of course if the user has 64 threads, then this number is probably excessive.
                int magic_calls_before_update = 2048;
                while (j++ < magic_calls_before_update)
                {
                    for (uint64_t i = 0; i < m_lbs.size(); ++i)
                    {
                        x[i] = (gen() - (gen.min)())*inv_denom;
                    }
                    Real f = m_integrand(x);
                    using std::isfinite;
                    if (!isfinite(f))
                    {
                        // The call to m_integrand transform x, so this error message states the correct node.
                        std::stringstream os;
                        os << "Your integrand was evaluated at {";
                        for (uint64_t i = 0; i < x.size() -1; ++i)
                        {
                             os << x[i] << ", ";
                        }
                        os << x[x.size() -1] << "}, and returned " << f << std::endl;
                        static const char* function = "boost::math::quadrature::naive_monte_carlo<%1%>";
                        boost::math::policies::raise_domain_error(function, os.str().c_str(), /*this is a dummy arg to make it compile*/ 7.2, Policy());
                    }
                    ++k;
                    Real term = (f - M1)/k;
                    Real y1 = term - compensator;
                    Real M2 = M1 + y1;
                    compensator = (M2 - M1) - y1;
                    S += (f - M1)*(f - M2);
                    M1 = M2;
                }
                m_thread_averages[thread_index].store(M1, std::memory_order_release);
                m_thread_Ss[thread_index].store(S, std::memory_order_release);
                m_thread_calls[thread_index].store(k, std::memory_order_release);
            }
        }
        catch (...)
        {
            // Signal the other threads that the computation is ruined:
            m_done = true; // relaxed store
            std::lock_guard<std::mutex> lock(m_exception_mutex); // Scoped lock to prevent race writing to m_exception
            m_exception = std::current_exception();
        }
    }

    std::function<Real(std::vector<Real> &)> m_integrand;
    uint64_t m_num_threads;
    std::atomic<uint64_t> m_seed;
    std::atomic<Real> m_error_goal;
    std::atomic<bool> m_done;
    std::vector<Real> m_lbs;
    std::vector<Real> m_dxs;
    std::vector<detail::limit_classification> m_limit_types;
    Real m_volume;
    std::atomic<uint64_t> m_total_calls;
    // I wanted these to be vectors rather than maps,
    // but you can't resize a vector of atomics.
    std::unique_ptr<std::atomic<uint64_t>[]> m_thread_calls;
    std::atomic<Real> m_variance;
    std::unique_ptr<std::atomic<Real>[]> m_thread_Ss;
    std::atomic<Real> m_avg;
    std::unique_ptr<std::atomic<Real>[]> m_thread_averages;
    std::chrono::time_point<std::chrono::system_clock> m_start;
    std::exception_ptr m_exception;
    std::mutex m_exception_mutex;
};

}}}
#endif


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

  @copyright 2015 NumScale SAS
  @copyright 2015 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_CONSTANT_SQRT_2PI_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_SQRT_2PI_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    Constant \f$\sqrt{2\pi}\f$.

    @par Semantic:

    For type T:

    @code
    T r = Sqrt_2pi<T>();
    @endcode

    is similar to:

    @code
    T r = sqrt(Pix2<T>();
    @endcode

    @return a value of type T

**/
  template<typename T> T Sqrt_2pi();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      Constant \f$\sqrt{2\pi}\f$.

      Generate the  constant sqrt_2pi.

      @return The Sqrt_2pi constant for the proper type
    **/
    const boost::dispatch::functor<tag::sqrt_2pi_> sqrt_2pi = {};
  }
} }
#endif

#include <boost/simd/constant/definition/sqrt_2pi.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


///////////////////////////////////////////////////////////////////////////////
// numeric::algorithm::min_element.hpp                                       //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NUMERIC_ALGORITHM_MIN_ELEMENT_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_NUMERIC_ALGORITHM_MIN_ELEMENT_HPP_ER_2009
#include <limits>
#include <boost/iterator/iterator_traits.hpp>
#include <boost/numeric/conversion/bounds.hpp>
#include <boost/limits.hpp>
#include <boost/concept_check.hpp>
#include <boost/concept/assert.hpp>
#include <iostream>

namespace boost{
namespace statistics{
namespace detail{
namespace numeric{
namespace algorithm{

// This is a rewrite of std::min_element with one additional argument, m, which
// is assigned the min value of *i over [first,last).
//
// Rationale: saves one dereferencing operation,
// which may be worthwhile if dereferencing is expensive.
template <typename It,typename V>
It min_element(It first, It last,V& min_value) {
    typedef typename iterator_value<It>::type value_type;

    // TODO see about enable_if/disable_if
    static bool has_inf = std::numeric_limits<V>::has_infinity;
    static V inf_if = std::numeric_limits<V>::infinity();
    static V highest = boost::numeric::bounds<V>::highest();
    static V inf_ = has_inf? inf_if : highest;

    BOOST_CONCEPT_ASSERT((
        boost::IncrementableIterator<It>
    ));

    min_value = inf_;
    value_type value;
    It result = first;
    while (first != last){
        value = (*first);
        if (value < min_value){
            min_value = value;
            result = first;
        }

        ++first;
    }
    return result;
}

}// algorithm
}// numeric
}// detail
}// statistics
}// boost

#endif


/* Copyright (C) 2020 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */

#include <NTL/ZZ.h>
#include <algorithm>

#include <helib/helib.h>
#include <helib/debugging.h>

#include "gtest/gtest.h"
#include "test_common.h"

namespace {
struct Parameters
{
  Parameters(unsigned m,
             unsigned p,
             unsigned r,
             unsigned bits,
             const std::vector<long>& gens = {},
             const std::vector<long>& ords = {}) :
      m(m), p(p), r(r), bits(bits), gens(gens), ords(ords){};

  const unsigned m;
  const unsigned p;
  const unsigned r;
  const unsigned bits;
  const std::vector<long> gens;
  const std::vector<long> ords;

  friend std::ostream& operator<<(std::ostream& os, const Parameters& params)
  {
    return os << "{"
              << "m = " << params.m << ", "
              << "p = " << params.p << ", "
              << "r = " << params.r << ", "
              << "gens = " << helib::vecToStr(params.gens) << ", "
              << "ords = " << helib::vecToStr(params.ords) << ", "
              << "bits = " << params.bits << "}";
  }
};

class TestBGV : public ::testing::TestWithParam<Parameters>
{
protected:
  const unsigned long m;
  const unsigned long p;
  const unsigned long r;
  const unsigned long bits;
  helib::Context context;
  helib::SecKey secretKey;
  const helib::PubKey publicKey;
  const helib::EncryptedArray& ea;

  TestBGV() :
      m(GetParam().m),
      p(GetParam().p),
      r(GetParam().r),
      bits(GetParam().bits),
      context(helib::ContextBuilder<helib::BGV>()
                  .m(m)
                  .p(p)
                  .r(r)
                  .bits(bits)
                  .build()),
      secretKey(context),
      publicKey((secretKey.GenSecKey(),
                 helib::addSome1DMatrices(secretKey),
                 secretKey)),
      ea(context.getEA())
  {}

  virtual void SetUp() override
  {
    if (helib_test::verbose) {
      ea.getPAlgebra().printout();
      std::cout << "r = " << context.getAlMod().getR() << std::endl;
      std::cout << "ctxtPrimes=" << context.getCtxtPrimes()
                << ", specialPrimes=" << context.getSpecialPrimes() << "\n"
                << std::endl;
    }

    helib::setupDebugGlobals(&secretKey, context.shareEA());
  }

  virtual void TearDown() override { helib::cleanupDebugGlobals(); }
};

TEST_P(TestBGV, negatingCiphertextWorks)
{
  helib::PtxtArray p1(ea), p2(ea);
  p1.random();

  helib::Ctxt c1(publicKey);
  p1.encrypt(c1);

  c1.negate();
  p1.negate();

  p2.decrypt(c1, secretKey);

  EXPECT_EQ(p1, p2);
}

TEST_P(TestBGV, addingPolyConstantToCiphertextWorks)
{
  helib::PtxtArray p1(ea), p2(ea), const1(ea);
  p1.random();
  const1.random();

  helib::Ctxt c1(publicKey);
  p1.encrypt(c1);

  c1.addConstant(const1);
  p1 += const1;

  p2.decrypt(c1, secretKey);

  EXPECT_EQ(p1, p2);
}

TEST_P(TestBGV, addingNegatedPolyConstantToCiphertextWorks)
{
  helib::PtxtArray p1(ea), p2(ea), const1(ea);
  p1.random();
  const1.random();

  helib::Ctxt c1(publicKey);
  p1.encrypt(c1);

  p1 -= const1;
  const1.negate();
  c1.addConstant(const1);

  p2.decrypt(c1, secretKey);

  EXPECT_EQ(p1, p2);
}

TEST_P(TestBGV, multiplyingPolyConstantToCiphertextWorks)
{
  helib::PtxtArray p1(ea), p2(ea), const1(ea);
  p1.random();
  const1.random();

  helib::Ctxt c1(publicKey);
  p1.encrypt(c1);

  c1.multByConstant(const1);
  p1 *= const1;

  p2.decrypt(c1, secretKey);

  EXPECT_EQ(p1, p2);
}

TEST_P(TestBGV, addingLongToCiphertextWorks)
{
  long const1 = 1;
  helib::PtxtArray p1(ea), p2(ea);
  p1.random();

  helib::Ctxt c1(publicKey);
  p1.encrypt(c1);

  c1.addConstant(const1);
  p1 += const1;

  p2.decrypt(c1, secretKey);

  EXPECT_EQ(p1, p2);
}

TEST_P(TestBGV, multiplyingLongToCiphertextWorks)
{
  long const1 = 2;
  helib::PtxtArray p1(ea), p2(ea);
  p1.random();

  helib::Ctxt c1(publicKey);
  p1.encrypt(c1);

  c1.multByConstant(const1);
  p1 *= const1;

  p2.decrypt(c1, secretKey);

  EXPECT_EQ(p1, p2);
}

TEST_P(TestBGV, rotatingCiphertextWorks)
{
  helib::PtxtArray p1(ea), p2(ea);
  p1.random();

  helib::Ctxt c1(publicKey);
  p1.encrypt(c1);

  ea.rotate(c1, 3);
  rotate(p1, 3);

  p2.decrypt(c1, secretKey);

  EXPECT_EQ(p1, p2);
}

TEST_P(TestBGV, addingCiphertextsWorks)
{
  helib::PtxtArray p1(ea), p2(ea), p3(ea);
  p1.random();
  p2.random();

  helib::Ctxt c1(publicKey), c2(publicKey);
  p1.encrypt(c1);
  p2.encrypt(c2);

  c1 += c2;
  p1 += p2;

  p3.decrypt(c1, secretKey);

  EXPECT_EQ(p1, p3);
}

TEST_P(TestBGV, subtractingCiphertextsWorks)
{
  helib::PtxtArray p1(ea), p2(ea), p3(ea);
  p1.random();
  p2.random();

  helib::Ctxt c1(publicKey), c2(publicKey);
  p1.encrypt(c1);
  p2.encrypt(c2);

  c1 -= c2;
  p1 -= p2;

  p3.decrypt(c1, secretKey);

  EXPECT_EQ(p1, p3);
}

TEST_P(TestBGV, timesEqualsOfCiphertextsWorks)
{
  helib::PtxtArray p1(ea), p2(ea), p3(ea);
  p1.random();
  p2.random();

  helib::Ctxt c1(publicKey), c2(publicKey);
  p1.encrypt(c1);
  p2.encrypt(c2);

  c1 *= c2;
  p1 *= p2;

  p3.decrypt(c1, secretKey);

  EXPECT_EQ(p1, p3);
  // Check that relinearization has occurred
  EXPECT_TRUE(c1.inCanonicalForm());
}

TEST_P(TestBGV, rawMultiplicationOfCiphertextsWorks)
{
  helib::PtxtArray p1(ea), p2(ea), p3(ea);
  p1.random();
  p2.random();

  helib::Ctxt c1(publicKey), c2(publicKey);
  p1.encrypt(c1);
  p2.encrypt(c2);

  c1.multLowLvl(c2);
  p1 *= p2;

  p3.decrypt(c1, secretKey);

  EXPECT_EQ(p1, p3);
  // Check that relinearization has not occurred
  EXPECT_FALSE(c1.inCanonicalForm());
}

TEST_P(TestBGV, highLevelMultiplicationOfCiphertextsWorks)
{
  helib::PtxtArray p1(ea), p2(ea), p3(ea);
  p1.random();
  p2.random();

  helib::Ctxt c1(publicKey), c2(publicKey);
  p1.encrypt(c1);
  p2.encrypt(c2);

  c1.multiplyBy(c2);
  p1 *= p2;

  p3.decrypt(c1, secretKey);

  EXPECT_EQ(p1, p3);
  // Check that relinearization has not occurred
  EXPECT_TRUE(c1.inCanonicalForm());
}

TEST_P(TestBGV, squaringCiphertextWorks)
{
  helib::PtxtArray p1(ea), p2(ea);
  p1.random();

  helib::Ctxt c1(publicKey);
  p1.encrypt(c1);

  c1.square();
  p1 *= p1;

  p2.decrypt(c1, secretKey);

  EXPECT_EQ(p1, p2);
  // Check that relinearization has not occurred
  EXPECT_TRUE(c1.inCanonicalForm());
}

TEST_P(
    TestBGV,
    multiplyingCiphertextByNegativeConstantAndThenAddingToOtherCiphertextWorks)
{
  helib::PtxtArray p1(ea), p2(ea), p3(ea), const1(ea, -1);
  p1.random();
  p2.random();

  helib::Ctxt c1(publicKey), c2(publicKey);
  p1.encrypt(c1);
  p2.encrypt(c2);

  c1.multByConstant(const1);
  c1 += c2;
  p1 *= const1;
  p1 += p2;

  p3.decrypt(c1, secretKey);

  EXPECT_EQ(p1, p3);
}

INSTANTIATE_TEST_SUITE_P(typicalParameters,
                         TestBGV,
                         ::testing::Values(
                             // FAST
                             Parameters(257, 2, 1, 150)
                             // SLOW
                             // Parameters(2049, 2, 1, 150)
                             ));

} // namespace