/**
 * \ file TanhShaperFilter.cpp
 */

#include <array>
#include <fstream>

#include <ATK/config.h>

#include <ATK/Distortion/TanhShaperFilter.h>
#include <ATK/Mock/SimpleSinusGeneratorFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

constexpr gsl::index PROCESSSIZE = 1000;

BOOST_AUTO_TEST_CASE( TanhShaperFilter_coeff_test )
{
  ATK::TanhShaperFilter<double> shaper;
  shaper.set_coefficient(10);
  BOOST_CHECK_EQUAL(shaper.get_coefficient(), 10);
}

BOOST_AUTO_TEST_CASE( TanhShaperFilter_coeff_range_test )
{
  ATK::TanhShaperFilter<double> shaper;
  BOOST_CHECK_THROW(shaper.set_coefficient(0), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( TanhShaperFilter_const_sin1k )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::TanhShaperFilter<double> shaper;
  shaper.set_input_sampling_rate(1024*64);
  shaper.set_output_sampling_rate(1024*64);

  shaper.set_input_port(0, generator, 0);
  shaper.process(PROCESSSIZE);

  auto sin = generator.get_output_array(0);
  auto array = shaper.get_output_array(0);

  for(size_t i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_CHECK_CLOSE(array[i], std::tanh(sin[i]), 0.00001);
  }
}

BOOST_AUTO_TEST_CASE( TanhShaperFilter_const_2_sin1k )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::TanhShaperFilter<double> shaper;
  shaper.set_input_sampling_rate(1024*64);
  shaper.set_output_sampling_rate(1024*64);
  shaper.set_coefficient(2);

  shaper.set_input_port(0, generator, 0);
  shaper.process(PROCESSSIZE);

  auto sin = generator.get_output_array(0);
  auto array = shaper.get_output_array(0);

  for(size_t i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_CHECK_CLOSE(array[i], std::tanh(2 * sin[i]) / 2, 0.00001);
  }
}


#ifndef _MLES_ACTIVATION_HPP_
#define _MLES_ACTIVATION_HPP_

#include <Eigen/Dense>
#include <string>
#include <ostream>
#include <istream>
#include <memory>

namespace mles
{
    class Activation;
    typedef std::shared_ptr<Activation> ActivationPtr;

    class Activation
    {
        private:
            std::string name;
            int parametersSize;
        public:
            Activation(const std::string& name, int parametersSize = 0)
            {
                this->name = name;
                this->parametersSize = parametersSize;
            }

            virtual ~Activation()
            {

            }

            std::string getName()
            {
                return name;
            }

            int getParametersSize()
            {
                return parametersSize;
            }

            bool is(const std::string& name)
            {
                return name == this->name;
            }

            virtual void getActivation(Eigen::MatrixXd& x, Eigen::MatrixXd& y)
            {
                y.resize(x.rows(), x.cols());
                for(int i = 0; i < x.rows(); i++)
                {
                    for(int j = 0; j < x.cols(); j++)
                        y(i,j) = this->getActivation(x(i,j));
                }
            }

            virtual void getDerivative(Eigen::MatrixXd& x, Eigen::MatrixXd& y)
            {
                y.resize(x.rows(), x.cols());
                for(int i = 0; i < x.rows(); i++)
                {
                    for(int j = 0; j < x.cols(); j++)
                        y(i,j) = this->getDerivative(x(i,j));
                }
            }

            virtual double getActivation(double x)
            {
                return x;
            }

            virtual double getDerivative(double x)
            {
                return 0;
            }

            virtual Activation* clone()
            {
                return new Activation(name);
            }

            virtual void load(std::istream& f)
            {

            }

            virtual void write(std::ostream& f)
            {

            }
    };
}

#endif


//#define BOOST_TEST_MAIN
#include <boost/test/included/unit_test.hpp>
#include <iostream>
#include <sstream>

#include "auxiliary_functions.h"
#include "combination_sum_3.h"

BOOST_AUTO_TEST_SUITE(combination_sum_3)

BOOST_AUTO_TEST_CASE(combination_sum_1) {
	int k(3);
	int n(9);
	std::vector<std::vector<int>> expected = { {1,2,6},{1,3,5},{2,3,4} };
	Solution sol;

	std::vector<std::vector<int>> actual = sol.combinationSum3(k, n);

	BOOST_CHECK(expected == actual);
}

BOOST_AUTO_TEST_CASE(combination_sum_2) {
	int k(3);
	int n(7);
	std::vector<std::vector<int>> expected = {{1,2,4}};
	Solution sol;

	std::vector<std::vector<int>> actual = sol.combinationSum3(k, n);

	BOOST_CHECK(expected == actual);
}

BOOST_AUTO_TEST_SUITE_END()


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestCopyIf
#include <boost/test/unit_test.hpp>

#include <boost/compute/lambda.hpp>
#include <boost/compute/algorithm/copy_if.hpp>
#include <boost/compute/container/vector.hpp>

#include "check_macros.hpp"
#include "context_setup.hpp"

namespace bc = boost::compute;
namespace compute = boost::compute;

BOOST_AUTO_TEST_CASE(copy_if_int)
{
    int data[] = { 1, 6, 3, 5, 8, 2, 4 };
    bc::vector<int> input(data, data + 7);

    bc::vector<int> output(input.size());
    bc::fill(output.begin(), output.end(), -1);

    using ::boost::compute::_1;

    bc::vector<int>::iterator iter =
        bc::copy_if(input.begin(), input.end(), output.begin(), _1 < 5);
    BOOST_VERIFY(iter == output.begin() + 4);
    CHECK_RANGE_EQUAL(int, 7, output, (1, 3, 2, 4, -1, -1, -1));

    bc::fill(output.begin(), output.end(), 42);
    iter =
        bc::copy_if(input.begin(), input.end(), output.begin(), _1 * 2 >= 10);
    BOOST_VERIFY(iter == output.begin() + 3);
    CHECK_RANGE_EQUAL(int, 7, output, (6, 5, 8, 42, 42, 42, 42));
}

BOOST_AUTO_TEST_CASE(copy_if_odd)
{
    int data[] = { 1, 2, 3, 4, 5, 1, 2, 3, 4, 5 };
    bc::vector<int> input(data, data + 10);

    using ::boost::compute::_1;

    bc::vector<int> odds(input.size());
    bc::vector<int>::iterator odds_end =
        bc::copy_if(input.begin(), input.end(), odds.begin(), _1 % 2 == 1);
    BOOST_CHECK(odds_end == odds.begin() + 6);
    CHECK_RANGE_EQUAL(int, 6, odds, (1, 3, 5, 1, 3, 5));

    bc::vector<int> evens(input.size());
    bc::vector<int>::iterator evens_end =
        bc::copy_if(input.begin(), input.end(), evens.begin(), _1 % 2 == 0);
    BOOST_CHECK(evens_end == evens.begin() + 4);
    CHECK_RANGE_EQUAL(int, 4, evens, (2, 4, 2, 4));
}

BOOST_AUTO_TEST_CASE(clip_points_below_plane)
{
    float data[] = { 1.0f, 2.0f, 3.0f, 0.0f,
                     -1.0f, 2.0f, 3.0f, 0.0f,
                     -2.0f, -3.0f, 4.0f, 0.0f,
                     4.0f, -3.0f, 2.0f, 0.0f };
    bc::vector<bc::float4_> points(reinterpret_cast<bc::float4_ *>(data),
                                   reinterpret_cast<bc::float4_ *>(data) + 4);

    // create output vector filled with (0, 0, 0, 0)
    bc::vector<bc::float4_> output(points.size());
    bc::fill(output.begin(), output.end(), bc::float4_(0.0f, 0.0f, 0.0f, 0.0f));

    // define the plane (at origin, +X normal)
    bc::float4_ plane_origin(0.0f, 0.0f, 0.0f, 0.0f);
    bc::float4_ plane_normal(1.0f, 0.0f, 0.0f, 0.0f);

    using ::boost::compute::_1;
    using ::boost::compute::lambda::dot;

    bc::vector<bc::float4_>::const_iterator iter =
        bc::copy_if(points.begin(),
                    points.end(),
                    output.begin(),
                    dot(_1 - plane_origin, plane_normal) > 0.0f);
    BOOST_CHECK(iter == output.begin() + 2);
}

BOOST_AUTO_TEST_CASE(copy_index_if_int)
{
    int data[] = { 1, 6, 3, 5, 8, 2, 4 };
    compute::vector<int> input(data, data + 7);

    compute::vector<int> output(input.size());
    compute::fill(output.begin(), output.end(), -1);

    using ::boost::compute::_1;
    using ::boost::compute::detail::copy_index_if;

    compute::vector<int>::iterator iter =
        copy_index_if(input.begin(), input.end(), output.begin(), _1 < 5);
    BOOST_VERIFY(iter == output.begin() + 4);
    CHECK_RANGE_EQUAL(int, 7, output, (0, 2, 5, 6, -1, -1, -1));
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright (c) 2007–2018 The scikit-learn developers. All rights reserved.
// Copyright 2018 Martin Krasser. All Rights Reserved.
// Modifications copyright (C) 2018 Uber Technologies, Inc.
//
// 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 "gaussian_process.h"

#include <cmath>
#include <iostream>

#include <Eigen/LU>

#include "LBFGS.h"

using Eigen::VectorXd;
using Eigen::MatrixXd;

namespace horovod {
namespace common {

// Returns true if any of the elements in the vectors is not a number.
bool isnan(const VectorXd& x) {
  for (int i = 0; i < x.size(); ++i) {
    if (std::isnan(x[i])) {
      return true;
    }
  }
  return false;
}

GaussianProcessRegressor::GaussianProcessRegressor(double alpha) : alpha_(alpha) {}

void GaussianProcessRegressor::Fit(MatrixXd* x_train, MatrixXd* y_train) {
  // Cache the last used training inputs and outputs for later prediction
  x_train_ = x_train;
  y_train_ = y_train;

  // This function will apply the natural logarithm element-wise to a matrix
  auto ln = [](double x) {
    return std::log(x);
  };

  // f(x): the objective function to be minimized by our optimizer.
  // Computes the negative log-likelihood for training data x_train and y_train and given noise level.
  double a2 = alpha_ * alpha_;
  double d3 = 0.5 * x_train_->rows() * std::log(2 * M_PI);
  auto f = [&, a2, d3](const VectorXd& x) {
    int64_t m = x_train_->rows();
    MatrixXd k = Kernel(*x_train_, *x_train_, x[0], x[1]) + (a2 * MatrixXd::Identity(m, m));
    MatrixXd k_inv = k.inverse();

    // Compute determinant via Cholesky decomposition
    MatrixXd l = k.llt().matrixL().toDenseMatrix();
    double d1 = l.diagonal().unaryExpr(ln).sum();
    MatrixXd d2 = 0.5 * (y_train_->transpose() * (k_inv * (*y_train_)));
    MatrixXd cov = d2.array() + (d1 + d3);

    return cov(0, 0);
  };

  // We wish to minimize the negative log-likelihood of f(x) above by evaluating it at a given point, then
  // empirically approximating the derivative to follow the gradient downwards.
  double f_min = std::numeric_limits<double>::max();
  VectorXd x_min;
  auto nll_fn = [&](const VectorXd& x, VectorXd& grad) {
    // f(x) computed at the current point x
    double fx = f(x);

    // Update the best value observed so far, if x is a valid point
    if (!isnan(x) && fx < f_min) {
      f_min = fx;
      x_min = x;
    }

    // Update the gradient vector in place and return the f(x) value computed
    ApproxFPrime(x, f, fx, grad);
    return fx;
  };

  // Use the L-BFGS method for minimizing the objective, limit our search to a set number of iterations
  // if convergence has not be reached within the threshold (epsilon).
  LBFGSpp::LBFGSParam<double> param;
  param.epsilon = 1e-5;
  param.max_iterations = 100;
  LBFGSpp::LBFGSSolver<double> solver(param);

  // Find the kernel parameter values for length_ and sigma_f_ that maximize the likelihood of the training data.
  VectorXd x = VectorXd::Ones(2);
  double fx;
  solver.minimize(nll_fn, x, fx);

  // If the returned value is NaN, then we short-circuit the optimizer by returning the cached best values found
  if (!isnan(x)) {
    length_ = x[0];
    sigma_f_ = x[1];
  } else {
    length_ = x_min[0];
    sigma_f_ = x_min[1];
  }
}

void GaussianProcessRegressor::Predict(const MatrixXd& x, VectorXd& mu, VectorXd* sigma) const {
  MatrixXd cov;
  PosteriorPrediction(x, *x_train_, *y_train_, mu, cov, length_, sigma_f_, alpha_);

  // Only compute standard deviation if it was requested
  if (sigma != nullptr) {
    // Extract the standard deviation from the covariance matrix
    auto sqrt = [](double x) {
      return std::sqrt(x);
    };
    *sigma = cov.diagonal().unaryExpr(sqrt);
  }
}

void GaussianProcessRegressor::PosteriorPrediction(
    const MatrixXd& x_s, const MatrixXd& x_train, const MatrixXd& y_train, VectorXd& mu_s, MatrixXd& cov_s,
    double l, double sigma_f, double sigma_y) const {
  // With m training data and n new input data. sy2 is the noise term in the diagonal of k. It is set to 0 if
  // observations are noisy.
  int64_t n = x_s.rows();
  int64_t m = x_train.rows();
  double sy2 = sigma_y * sigma_y;

  // The posterior predictive distribution is Gaussian with mean mu_s and covariance cov_s. By definition of the
  // Gaussian Process, the joint distribution of observed data x_train and predictions y_train is distributed
  // normally with mean 0 and standard deviation [[k, k_s], [k_s^T, k_ss]].
  MatrixXd k = Kernel(x_train, x_train, l, sigma_f) + (sy2 * MatrixXd::Identity(m, m));
  MatrixXd k_s = Kernel(x_train, x_s, l, sigma_f);
  MatrixXd k_ss = Kernel(x_s, x_s, l, sigma_f) + (1e-8 * MatrixXd::Identity(n, n));
  MatrixXd k_inv = k.inverse();

  // Compute sufficient statistics of the posterior predictive distribution: mean and covariance.
  mu_s = (k_s.transpose() * k_inv) * y_train;
  cov_s = k_ss - (k_s.transpose() * k_inv) * k_s;
}

void GaussianProcessRegressor::ApproxFPrime(const VectorXd& x, const std::function<double(const VectorXd&)>& f,
                                            double f0, VectorXd& grad, double epsilon) {
  VectorXd ei = VectorXd::Zero(x.size());
  for (int k = 0; k < x.size(); ++k) {
    ei[k] = 1.0;
    VectorXd d = epsilon * ei;
    grad[k] = (f(x + d) - f0) / d[k];
    ei[k] = 0.0;
  }
}

MatrixXd GaussianProcessRegressor::Kernel(const MatrixXd& x1, const MatrixXd& x2,
                                          double l, double sigma_f) const {
  // Squared Exponential Kernel, also known as the Gaussian or RBF Kernel.
  auto x1_vec = x1.cwiseProduct(x1).rowwise().sum();
  auto x2_vec = x2.cwiseProduct(x2).rowwise().sum();
  auto x1_x2 = x1_vec.replicate(1, x2_vec.size()).rowwise() + x2_vec.transpose();

  auto& dot = x1 * x2.transpose();
  auto sqdist = x1_x2 - (dot.array() * 2).matrix();

  // The length parameter l controls the smoothness of the function and sigma_f the vertical variation. We use
  // the same l for all input dimensions (isotropic kernel).
  double sigma_f2 = sigma_f * sigma_f;
  double l2 = l * l;
  auto op = [sigma_f2, l2](double x) {
    return sigma_f2 * std::exp(-0.5 / l2 * x);
  };

  return sqdist.unaryExpr(op);
}

} // namespace common
} // namespace horovod


// *****************************************************************************
//
// Copyright (c) 2015, Southwest Research Institute® (SwRI®)
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of Southwest Research Institute® (SwRI®) nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// *****************************************************************************

#include <swri_geometry_util/cubic_spline.h>

#include <Eigen/Dense>

namespace swri_geometry_util
{

  bool CubicSplineInterpolation(
    const std::vector<cv::Vec2d>& points,
    double delta,
    std::vector<std::vector<cv::Vec2d> >& splines)
  {
    if (delta <= 0)
    {
      return false;
    }

    size_t num_points = points.size();

    splines.clear();
    splines.resize(num_points - 1);

    // Accumulated distance along linear path.
    std::vector<double> s = std::vector<double>(num_points);

    // Distance between consecutive points on path.
    std::vector<double> ds = std::vector<double>(num_points);

    s[0] = 0;
    for (size_t i = 1; i < num_points; i++)
    {
      double dx = points[i][0] - points[i - 1][0];
      double dy = points[i][1] - points[i - 1][1];

      ds[i - 1] = std::sqrt(std::pow(dx, 2) + std::pow(dy, 2));
      s[i] = s[i - 1] + ds[i - 1];
    }

    // Normalize s to be [0:1]
    double totalDistance = s[num_points - 1];
    for (size_t i = 0; i < num_points; i++)
    {
      s[i] /= totalDistance;
    }

    Eigen::MatrixX2d u(num_points, 2);
    Eigen::MatrixXd A(num_points, num_points);
    Eigen::MatrixX2d z(num_points, 2);

    // Set up the z0 equation.
    u(0, 0) = 0;
    u(0, 1) = 0;
    A(0, 0) = 1;
    for (size_t i = 1; i < num_points; i++)
    {
      A(0, i) = 0;
    }

    // Set up the z1 to zn-1 equations.
    for (size_t i = 1; i < num_points - 1; i++)
    {
      double hi = s[i + 1] - s[i];
      double himl = s[i] - s[i - 1];

      u(i, 0) = 6.0 * ((points[i + 1][0] - points[i][0]) / hi - (points[i][0]
          - points[i - 1][0]) / himl);
      u(i, 1) = 6.0 * ((points[i + 1][1] - points[i][1]) / hi - (points[i][1]
          - points[i - 1][1]) / himl);

      for (size_t j = 0; j < i - 1; j++)
      {
        A(i, j) = 0;
      }
      A(i, i - 1) = himl;
      A(i, i) = 2 * (himl + hi);
      A(i, i + 1) = hi;
      for (size_t j = i + 2; j < num_points; j++)
      {
        A(i, j) = 0;
      }
    }

    // Set up the zn equation
    u(num_points - 1, 0) = 0;
    u(num_points - 1, 1) = 0;
    for (size_t j = 0; j < num_points - 1; j++)
    {
      A(num_points - 1, j) = 0;
    }
    A(num_points - 1, num_points - 1) = 1;

    // Solve Az = u
    z = A.colPivHouseholderQr().solve(u);

    for (size_t i = 0; i < num_points - 1; i++)
    {
      std::vector<cv::Vec2d>& spline = splines[i];

      spline.push_back(points[i]);

      double hi = s[i + 1] - s[i];
      double act_dist = ds[i];
      int32_t num_to_add = static_cast<int32_t> (act_dist / delta);
      double normalizedDelta = hi / (num_to_add + 1);

      // Add the additional points between waypoints
      for (int32_t j = 1; j <= num_to_add; j++)
      {
        // Calculate the (x,y) value of the interpolated point
        double si = s[i] + j * normalizedDelta;
        double xx = (z(i + 1, 0) * pow(si - s[i], 3) + z(i, 0) * pow(s[i + 1]
            - si, 3)) / (6.0 * hi) + (points[i + 1][0] / hi - hi * z(i + 1, 0)
            / 6.0) * (si - s[i]) + (points[i][0] / hi - hi * z(i, 0) / 6.0)
            * (s[i + 1] - si);
        double yy = (z(i + 1, 1) * pow(si - s[i], 3) + z(i, 1) * pow(s[i + 1]
            - si, 3)) / (6.0 * hi) + (points[i + 1][1] / hi - hi * z(i + 1, 1)
            / 6.0) * (si - s[i]) + (points[i][1] / hi - hi * z(i, 1) / 6.0)
            * (s[i + 1] - si);

        // Add the point to the interpolated array
        spline.push_back(cv::Vec2d(xx, yy));
      }

      spline.push_back(points[i + 1]);
    }

    return true;
  }

  bool CubicSplineInterpolation(
    const std::vector<tf::Vector3>& points,
    double delta,
    std::vector<std::vector<tf::Vector3> >& splines)
  {
    std::vector<cv::Vec2d> cv_points(points.size());
    for (size_t i = 0; i < points.size(); i++)
    {
      cv_points[i] = cv::Vec2d(points[i].x(), points[i].y());
    }

    std::vector<std::vector<cv::Vec2d> > cv_splines;
    bool result = CubicSplineInterpolation(cv_points, delta, cv_splines);

    splines.resize(cv_splines.size());
    for (size_t i = 0; i < cv_splines.size(); i++)
    {
      splines[i].resize(cv_splines[i].size());
      for (size_t j = 0; j < cv_splines[i].size(); j++)
      {
        splines[i][j] = tf::Vector3(cv_splines[i][j][0], cv_splines[i][j][1], 0);
      }
    }

    return result;
  }
}


#ifndef STAN_MATH_PRIM_FUN_MODIFIED_BESSEL_FIRST_KIND_HPP
#define STAN_MATH_PRIM_FUN_MODIFIED_BESSEL_FIRST_KIND_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <stan/math/prim/functor/apply_scalar_binary.hpp>
#include <boost/math/special_functions/bessel.hpp>

namespace stan {
namespace math {

/**
 *
   \f[
   \mbox{modified\_bessel\_first\_kind}(v, z) =
   \begin{cases}
     I_v(z) & \mbox{if } -\infty\leq z \leq \infty \\[6pt]
     \textrm{error} & \mbox{if } z = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{modified\_bessel\_first\_kind}(v, z)}{\partial z} =
   \begin{cases}
     \frac{\partial\, I_v(z)}{\partial z} & \mbox{if } -\infty\leq z\leq \infty
 \\[6pt] \textrm{error} & \mbox{if } z = \textrm{NaN} \end{cases} \f]

   \f[
     {I_v}(z) = \left(\frac{1}{2}z\right)^v\sum_{k=0}^\infty
 \frac{\left(\frac{1}{4}z^2\right)^k}{k!\Gamma(v+k+1)} \f]

   \f[
   \frac{\partial \, I_v(z)}{\partial z} = I_{v-1}(z)-\frac{v}{z}I_v(z)
   \f]
 *
 */
template <typename T2, require_arithmetic_t<T2>* = nullptr>
inline T2 modified_bessel_first_kind(int v, const T2 z) {
  check_not_nan("modified_bessel_first_kind", "z", z);

  return boost::math::cyl_bessel_i(v, z);
}

/**
 * This function exists because when z is of type integer,
 * cyl_bessel_i(v, z) returns an integer. This
 * results in overflow when the function value is large.
 */
inline double modified_bessel_first_kind(int v, int z) {
  check_not_nan("modified_bessel_first_kind", "z", z);

  return boost::math::cyl_bessel_i(v, static_cast<double>(z));
}

/**
 * Enables the vectorised application of the modified_bessel_first_kind
 * function, when the first and/or second arguments are containers.
 *
 * @tparam T1 type of first input
 * @tparam T2 type of second input
 * @param a First input
 * @param b Second input
 * @return modified_bessel_first_kind function applied to the two inputs.
 */
template <typename T1, typename T2, require_any_container_t<T1, T2>* = nullptr>
inline auto modified_bessel_first_kind(const T1& a, const T2& b) {
  return apply_scalar_binary(a, b, [&](const auto& c, const auto& d) {
    return modified_bessel_first_kind(c, d);
  });
}

}  // namespace math
}  // namespace stan

#endif


#include <Eigen/Dense>
#include <mutex>

#ifndef UTILS_EXPONENTIAL_FILTER_H
#define UTILS_EXPONENTIAL_FILTER_H
//https://gregstanleyandassociates.com/whitepapers/FaultDiagnosis/Filtering/Exponential-Filter/exponential-filter.htm

namespace utils
{
    class ExponentialFilter
    {
    private:
        Eigen::VectorXd y_last_;
        const double a_;

    public:
        /**
         * Constructor for filter
         * @param a: The filter value (usually between 0.8 and 0.99)
         * @param y_0: The init value of the filter (usually 0)
         */
        ExponentialFilter(double a, const Eigen::VectorXd &y_0);

        /**
         * Predict the next value of the signal
         * @param x: The to be filtered vector; return value is stored in same vector
         */
        void predict(Eigen::VectorXd &x);
    };
} // namespace utils

#endif


/**
 * \Description:
 * this node is used to estimate the absoulte location(x,y) of the robot using Trilteration.
 * It subscribe to:
 * - /beacon_distances
 * It publishes to:
 * - /pose
 *
 * ALI&SERRANO_ECN_M1_2017
 */

//Cpp
#include <sstream>
#include <stdio.h>
#include <vector>
#include <iostream>
#include <stdlib.h>
#include <math.h>
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;
using Eigen::MatrixXd;

//ROS
#include "ros/ros.h"

// Include here the ".h" files corresponding to the topic type you use.
#include <nav_msgs/Odometry.h>
#include "tf/transform_datatypes.h"

#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"
#include <std_msgs/Float64MultiArray.h>

#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose2D.h>

//#include <geometry_msgs/PoseWithCovariance.h>
//#include <std_msgs/Float32.h>

// global variables...
std_msgs::Float64MultiArray dist;
bool  is_dist_available=false;

void msurmnt_Callback ( std_msgs::Float64MultiArray  dist_msg) {
    is_dist_available= true;
    dist = dist_msg;
}

int circle_circle_intersection(double x0, double y0, double r0,
                               double x1, double y1, double r1,
                               double &xi, double &yi,
                               double &xi_prime, double &yi_prime)
{
    double a, dx, dy, d, h, rx, ry;
    double x2, y2;

    // dx and dy are the vertical and horizontal distances between the circle centers.
    dx = x1 - x0;
    dy = y1 - y0;

    // Determine the straight-line distance between the centers.
    //d = sqrt((dy*dy) + (dx*dx));
    d = hypot(dx,dy); // Suggested by Keith Briggs

    // Check for solvability.
    if (d > (r0 + r1)) /* no solution. circles do not intersect. */
        return 0;
    if (d < fabs(r0 - r1)) /* no solution. one circle is contained in the other */
        return 0;

    /* 'point 2' is the point where the line through the circle
   * intersection points crosses the line between the circle centers.
   */

    /* Determine the distance from point 0 to point 2. */
    a = ((r0*r0) - (r1*r1) + (d*d)) / (2.0 * d) ;

    /* Determine the coordinates of point 2. */
    x2 = x0 + (dx * a/d);
    y2 = y0 + (dy * a/d);

    // Determine the distance from point 2 to either of the intersection points.
    h = sqrt((r0*r0) - (a*a));

    // Now determine the offsets of the intersection points from point 2.
    rx = -dy * (h/d);
    ry = dx * (h/d);

    /* Determine the absolute intersection points. */
    xi = x2 + rx;
    xi_prime = x2 - rx;
    yi = y2 + ry;
    yi_prime = y2 - ry;

    return 1;
}

void select_point(double p1x, double p1y, double p2x, double p2y, double bx, double by, double &px, double &py, double d){
    double t1= fabs (sqrt( (bx- p1x)*(bx-p1x) + (by-p1y)*(by-p1y)) );
    double t2= fabs (sqrt( (bx-p2x)*(bx-p2x) + (by-p2y)*(by-p2y)) );
    if ( fabs(d-t1) < fabs(d-t2) )
       { px= p1x;  py=p1y; }
    else
       { px= p2x;  py=p2y; }
}

void incircle(double p1x, double p1y, double p2x, double p2y, double p3x, double p3y, double &xc, double &yc){
    double a1 =fabs (sqrt( (p3x-p2x)*(p3x-p2x) + (p3y-p2y)*(p3y-p2y)) );
    double a2 =fabs (sqrt( (p1x-p3x)*(p1x-p3x) + (p1y-p3y)*(p1y-p3y)) );
    double a3 =fabs (sqrt( (p1x-p2x)*(p1x-p2x) + (p1y-p2y)*(p1y-p2y)) );
    xc= ( a1*p1x + a2*p2x + a3*p3x )/(a1+a2+a3);
    yc= ( a1*p1y + a2*p2y + a3*p3y )/(a1+a2+a3);
}

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

    //ROS Initialization
    ros::init(argc, argv, "ekf_node");

    // Define your node handles
    ros::NodeHandle nh_loc("~"), nh_glob;

    // Read the node parameters if any

    // Declare your node's subscriptions and service clients
    ros::Subscriber msurmnt_sub = nh_glob.subscribe<std_msgs::Float64MultiArray>("/beacon_distances",1, msurmnt_Callback) ;

    // Declare you publishers and service servers
    ros::Publisher  pose_pub = nh_glob.advertise<geometry_msgs::Pose2D>("/pose",1) ;

    // node initilization

    //geometry_msgs::Point B1, B2, B3;
    //B1.x=0;  B1.y=10; B2.x=10;  B2.y=0; B3.x=20; B3.y=20;

    //double x1=0, x2=0, x3=10, y1=5, y2=10, y3=10, x, y, d1, d2, d3;
    double d1, d2, d3, xc, yc;
    ros::Rate rate(10);

    while (ros::ok()){

        ros::spinOnce();
                if( ! is_dist_available ){
                    ROS_INFO("Waiting for odom/dist.") ;
                    rate.sleep() ;
                    continue ;
                }

        // measurement part:

        // Actual measurement  Y
        d1= dist.data[0];
        d2= dist.data[1];
        d3= dist.data[2];

        //double d1=5, d2=5, d3=1;
        double b1x=0, b1y=0, b2x=0, b2y=190, b3x=350, b3y=0;
        double ip12x1, ip12y1, ip12x2, ip12y2,   ip13x1, ip13y1, ip13x2, ip13y2,   ip23x1, ip23y1, ip23x2, ip23y2;

        int t12= circle_circle_intersection(b1x, b1y, d1, b2x, b2y, d2, ip12x1, ip12y1, ip12x2, ip12y2); //12
        int t13= circle_circle_intersection(b1x, b1y, d1, b3x, b3y, d3, ip13x1, ip13y1, ip13x2, ip13y2); //13
        int t23= circle_circle_intersection(b2x, b2y, d2, b3x, b3y, d3, ip23x1, ip23y1, ip23x2, ip23y2); //23

        if ( t12 && t13 && t23){
            //std::cout<< "point1: (" << ip1x << ", " << ip1y << ")" << ")"<< std::endl;
            //std::cout<< "point2: (" << ip2x << ", " << ip2y << ")" << ")"<< std::endl;
            double ip1x, ip1y, ip2x, ip2y, ip3x, ip3y;

            select_point(ip12x1, ip12y1, ip12x2, ip12y2, b3x, b3y, ip1x, ip1y, d3 );
            select_point(ip13x1, ip13y1, ip13x2, ip13y2, b2x, b2y, ip2x, ip2y, d2 );
            select_point(ip23x1, ip23y1, ip23x2, ip23y2, b1x, b1y, ip3x, ip3y, d1 );

            std::cout<< "point1: (" << ip1x << ", " << ip1y << ")" << ")"<< std::endl;
            std::cout<< "point2: (" << ip2x << ", " << ip2y << ")" << ")"<< std::endl;
            std::cout<< "point3: (" << ip3x << ", " << ip3y << ")" << ")"<< std::endl;

            incircle(ip1x, ip1y, ip2x, ip2y, ip3x, ip3y, xc, yc);
            std::cout<< "center: (" << xc << ", " << yc << ")" << ")"<< std::endl;

        }
        else
            std::cout<<"no common intersection"<<std::endl;

//        float c1= (d1*d1) - (x1*x1) - (y1*y1);
//        float c2= (d2*d2) - (x2*x2) - (y2*y2);
//        float c3= (d3*d3) - (x3*x3) - (y3*y3);

//        x= ( ((c1-c2)*(y3-y1)) - ((c1-c3)*(y2-y1)) ) / ( 2*( ( (x2-x1)*(y3-y1) ) - ((x3-x1)*(y2-y1)) ));
//        y= ( c1 - c2 - (2*x*(x2-x1)) ) / (2 * (y2-y1) );
        // measurement covariance Q_gamma
        //Q_gamma(0,0)= .1;  Q_gamma(1,1)= .1;  Q_gamma(2,2)= .1;

        // Estimated_covariance_est: Pk+1/k+1 = ( I - Kk*Ck ) * Pk+1/k
        //P= ( I - K*C ) * P;

        // publish the final pose ........................................................................................................
        geometry_msgs::Pose2D pose;
        pose.x = xc;
        pose.y = yc;
        pose.theta =  0;
        pose_pub.publish(pose);

        rate.sleep();
    }

}


#include "DiffWristKinematics.h"

#include <Eigen/Dense>

using Eigen::Matrix2f;
using Eigen::Vector2f;

gearpos_t::gearpos_t(const Vector2f& vec) : left(vec(0)), right(vec(1)) {}
gearpos_t::gearpos_t(float left, float right) : left(left), right(right) {}
Vector2f gearpos_t::vec() const {
	return Vector2f(left, right);
}

jointpos_t::jointpos_t(const Vector2f& vec) : pitch(vec(0)), roll(vec(1)) {}
jointpos_t::jointpos_t(float pitch, float roll) : pitch(pitch), roll(roll) {}
Vector2f jointpos_t::vec() const {
	return Vector2f(pitch, roll);
}

static Matrix2f createTransform() {
	Matrix2f ret;
	ret <<
		0.5, 0.5,
		-0.5, 0.5;
	return ret;
}

static const Matrix2f gearToJointPosTransform = createTransform();
static const Matrix2f jointToGearPosTransform = gearToJointPosTransform.inverse();

jointpos_t DiffWristKinematics::gearPosToJointPos(const gearpos_t& gearPos) const {
	Vector2f res = gearToJointPosTransform * gearPos.vec();
	return res;
}

gearpos_t DiffWristKinematics::jointPosToGearPos(const jointpos_t &jointPos) const {
	Vector2f res = jointToGearPosTransform * jointPos.vec();
	return res;
}

jointpos_t DiffWristKinematics::gearPowerToJointPower(const gearpos_t &gearPwr) const {
	Vector2f jointPwr = gearToJointPosTransform * gearPwr.vec();
	// compute infinity norm; i.e. component with max absolute value
	float maxVal = jointPwr.lpNorm<Eigen::Infinity>();
	// if any component is absolutely greater than 1, divide by maximum component to scale
	// everything to the interval [-1,1]
	if(maxVal > 1){
		jointPwr /= maxVal;
	}
	return jointPwr;
}

gearpos_t DiffWristKinematics::jointPowerToGearPower(const jointpos_t &jointPwr) const {
    Vector2f gearPwr = jointToGearPosTransform * jointPwr.vec();
	// compute infinity norm; i.e. component with max absolute value
	float maxVal = gearPwr.lpNorm<Eigen::Infinity>();
	// if any component is absolutely greater than 1, divide by maximum component to scale
	// everything to the interval [-1,1]
	if(maxVal > 1){
		gearPwr /= maxVal;
	}
	return gearPwr;
}


#define EIGEN_NO_DEBUG

#include "ccpca.hpp"

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <cmath>
#include <iostream>
#include <limits>
#include <thread>

CCPCA::CCPCA(Eigen::Index const nComponents, bool const standardize)
    : cpca_(CPCA(nComponents, standardize)) {
  featContribs_.resize(0);
  concatMat_.resize(0, 0);
}

Eigen::MatrixXf
CCPCA::fitTransform(Eigen::MatrixXf const &K, Eigen::MatrixXf const &R,
                    bool const autoAlphaSelection, float const alpha,
                    float const varThresRatio, bool parallel,
                    unsigned int const nAlphas, float const maxLogAlpha,
                    bool const keepReports) {
  fit(K, R, autoAlphaSelection, alpha, varThresRatio, parallel, nAlphas,
      maxLogAlpha, keepReports);
  return cpca_.transform(concatMat_);
}

void CCPCA::fit(Eigen::MatrixXf const &K, Eigen::MatrixXf const &R,
                bool const autoAlphaSelection, float const alpha,
                float const varThresRatio, bool parallel,
                unsigned int const nAlphas, float const maxLogAlpha,
                bool const keepReports) {
  if (autoAlphaSelection) {
    fitWithBestAlpha(K, R, varThresRatio, parallel, nAlphas, maxLogAlpha,
                     keepReports);
  } else {
    fitWithManualAlpha(K, R, alpha);
  }
}

void CCPCA::fitWithBestAlpha(Eigen::MatrixXf const &K, Eigen::MatrixXf const &R,
                             float const varThresRatio, bool parallel,
                             unsigned int const nAlphas,
                             float const maxLogAlpha, bool const keepReports) {
  bestAlpha(K, R, varThresRatio, parallel, nAlphas, maxLogAlpha, keepReports);
  cpca_.updateComponents(bestAlpha_);
  featContribs_ = cpca_.getLoading(0);
}

void CCPCA::fitWithManualAlpha(Eigen::MatrixXf const &K,
                               Eigen::MatrixXf const &R, float const alpha) {
  Eigen::Index nSamplesK = K.rows();
  Eigen::Index nSamplesR = R.rows();

  if (K.cols() != R.cols()) {
    std::cerr << "# of rows of K and all matrix in R must be the same."
              << std::endl;
  }

  Eigen::MatrixXf concatMat_(nSamplesK + nSamplesR, K.cols());
  concatMat_ << K, R;

  cpca_.fit(concatMat_, R, alpha);
  featContribs_ = cpca_.getLoading(0);
}

Eigen::MatrixXf CCPCA::transform(Eigen::MatrixXf const &X) {
  return cpca_.transform(X);
}

float CCPCA::bestAlpha(Eigen::MatrixXf const &K, Eigen::MatrixXf const &R,
                       float const varThresRatio, bool parallel,
                       unsigned int const nAlphas, float const maxLogAlpha,
                       bool const keepReports) {
  Eigen::Index nSamplesK = K.rows();
  Eigen::Index nSamplesR = R.rows();

  if (K.cols() != R.cols()) {
    std::cerr << "# of rows of K and all matrix in R must be the same."
              << std::endl;
  }

  Eigen::MatrixXf concatMat_(nSamplesK + nSamplesR, K.cols());
  concatMat_ << K, R;

  cpca_.fit(concatMat_, R, 0.0f);

  Eigen::VectorXf bestProjK = cpca_.transform(K).col(0);
  Eigen::VectorXf bestProjR = cpca_.transform(R).col(0);

  bestAlpha_ = 0.0f;
  auto baseVarK = scaledVar(bestProjK, bestProjR).first;
  auto bestDiscrepancy =
      1.0f / std::max(float(histIntersect(bestProjK, bestProjR)),
                      std::numeric_limits<float>::min());

  reports_.clear();
  if (keepReports) {
    reports_.push_back(std::make_tuple(0.0, float(bestDiscrepancy), baseVarK,
                                       bestProjK, bestProjR,
                                       cpca_.getLoading(0)));
    if (parallel) {
      parallel = false;
      std::cout << "current version keepReports only support non-parallel "
                   "running. parallel is turned off."
                << std::endl;
    }
  }

  auto alphas = cpca_.logspace(-1, maxLogAlpha, nAlphas - 1);

  if (!parallel) {
    // non-parallel ver
    for (auto const &alpha : alphas) {
      cpca_.updateComponents(alpha);

      Eigen::VectorXf tmpProjK = cpca_.transform(K).col(0);
      Eigen::VectorXf tmpProjR = cpca_.transform(R).col(0);

      auto varK = scaledVar(tmpProjK, tmpProjR).first;
      auto discrepancy =
          1.0f / std::max(float(histIntersect(tmpProjK, tmpProjR)),
                          std::numeric_limits<float>::min());

      if (varK >= baseVarK * varThresRatio && discrepancy > bestDiscrepancy) {
        bestDiscrepancy = discrepancy;
        bestAlpha_ = alpha;
      }

      if (keepReports) {
        reports_.push_back(std::make_tuple(alpha, float(bestDiscrepancy), varK,
                                           tmpProjK, tmpProjR,
                                           cpca_.getLoading(0)));
      }
    }
  } else {
    // parallel version
    auto numWorkers = std::max(std::thread::hardware_concurrency(),
                               1u); // obtain max thread num
    std::vector<std::thread> worker;
    auto n = alphas.size();
    std::vector<std::pair<float, float>> varAndDiscpSet(n, {0.0f, 0.0f});

    for (size_t i = 0; i < numWorkers; ++i) {
      worker.emplace_back(
          [&](size_t id) {
            auto r0 = n / numWorkers * id + std::min(n % numWorkers, id);
            auto r1 =
                n / numWorkers * (id + 1) + std::min(n % numWorkers, id + 1);

            for (auto j = r0; j < r1; ++j) {
              Eigen::VectorXf component;
              if (numWorkers > 4) {
                // less mtx locks but more steps
                mtx.lock();
                auto alpha = alphas[j];
                Eigen::MatrixXf diffCov = cpca_.getDiffCov(alpha);
                mtx.unlock();

                Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> es(diffCov);
                component = es.eigenvectors().rightCols(1);
              } else {
                // less steps but more mtx locks
                mtx.lock();
                auto alpha = alphas[j];
                cpca_.updateComponents(alpha);
                component = cpca_.getComponent(0);
                mtx.unlock();
              }

              Eigen::VectorXf tmpProjK = K * component;
              Eigen::VectorXf tmpProjR = R * component;

              auto varK = scaledVar(tmpProjK, tmpProjR).first;
              auto discrepancy =
                  1.0f / std::max(float(histIntersect(tmpProjK, tmpProjR)),
                                  std::numeric_limits<float>::min());
              mtx.lock();
              varAndDiscpSet[j] = {varK, discrepancy};
              mtx.unlock();
            }
          },
          i);
    }
    for (auto &w : worker)
      w.join();

    for (size_t i = 0; i < n; ++i) {
      auto var = varAndDiscpSet[i].first;
      auto discrepancy = varAndDiscpSet[i].second;
      if (var >= baseVarK * varThresRatio && discrepancy > bestDiscrepancy) {
        bestDiscrepancy = discrepancy;
        bestAlpha_ = alphas[i];
      }
    }
  }

  return bestAlpha_;
}

std::pair<float, float> CCPCA::scaledVar(Eigen::VectorXf const &a,
                                         Eigen::VectorXf const &b) {
  float minVal = std::min(a.minCoeff(), b.minCoeff());
  float maxVal = std::max(a.maxCoeff(), b.maxCoeff());
  float range = std::max(maxVal - minVal, std::numeric_limits<float>::min());

  float varA = ((a.array() - a.mean()) / range).array().square().mean();
  float varB = ((b.array() - b.mean()) / range).array().square().mean();

  return {varA, varB};
}

float CCPCA::binWidthScott(Eigen::VectorXf const &vals) {
  auto n = vals.size();
  float sd = 0.0f;
  if (n > 1) {
    sd = std::sqrt((vals.array() - vals.mean()).square().sum() / float(n - 1));
  }
  float denom = std::max(std::pow(float(n), 1.0f / 3.0f),
                         std::numeric_limits<float>::min());
  float binWidth = 3.5f * sd / denom;

  return binWidth;
}

int CCPCA::histIntersect(Eigen::VectorXf const &a, Eigen::VectorXf const &b) {
  float minVal = std::min(a.minCoeff(), b.minCoeff());
  float maxVal = std::max(a.maxCoeff(), b.maxCoeff());
  float range = std::max(maxVal - minVal, std::numeric_limits<float>::min());
  auto nA = a.size();
  auto nB = b.size();

  Eigen::VectorXf tmpA = (a.array() - minVal) / range;
  Eigen::VectorXf tmpB = (b.array() - minVal) / range;
  Eigen::VectorXf tmpAB(nA + nB);
  tmpAB << tmpA, tmpB;

  float binW =
      std::max(binWidthScott(tmpAB), std::numeric_limits<float>::min());
  unsigned int nBins = static_cast<unsigned int>(1.0f / binW) + 1;

  std::vector<int> countsA(nBins, 0);
  std::vector<int> countsB(nBins, 0);

  for (Eigen::Index i = 0; i < nA; ++i) {
    int binIndex = int(tmpA(i) / binW);
    countsA[binIndex]++;
  }
  for (Eigen::Index i = 0; i < nB; ++i) {
    int binIndex = int(tmpB(i) / binW);
    countsB[binIndex]++;
  }

  int histIntersect = 0;
  for (unsigned int i = 0; i < nBins; ++i) {
    histIntersect += std::min(countsA[i], countsB[i]);
  }

  return histIntersect;
}


/**
 * @file line_search_test.cpp
 * @author Chenzhe Diao
 *
 * Test file for line search optimizer.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */

#include <mlpack/core.hpp>
#include <mlpack/core/optimizers/line_search/line_search.hpp>
#include <mlpack/core/optimizers/fw/test_func_fw.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace std;
using namespace arma;
using namespace mlpack;
using namespace mlpack::optimization;

BOOST_AUTO_TEST_SUITE(LineSearchTest);

/**
 * Simple test of Line Search with TestFuncFW function.
 */
BOOST_AUTO_TEST_CASE(FuncFWTest)
{
  vec x1 = zeros<vec>(3);
  vec x2;
  x2 << 0.2 << 0.4 << 0.6;

  TestFuncFW f;
  LineSearch s;

  double result = s.Optimize(f, x1, x2);

  BOOST_REQUIRE_SMALL(result, 1e-10);
  BOOST_REQUIRE_SMALL(x2[0] - 0.1, 1e-10);
  BOOST_REQUIRE_SMALL(x2[1] - 0.2, 1e-10);
  BOOST_REQUIRE_SMALL(x2[2] - 0.3, 1e-10);
}

BOOST_AUTO_TEST_SUITE_END();


#pragma once

#include <boost/multiprecision/gmp.hpp>
#include <math.h>
#include <time.h>

#include "FiniteFields.hpp"
#include "FastFourierTransform.hpp"
#include "NumberTheoreticTransform.hpp"

namespace ligero {

class SecretSharingNTT {
	public:
		/* Constructors allowing some flexibility in the choice of domains */
		~SecretSharingNTT() {
			delete _smallDomain;
			delete _largeDomain;
		}
		SecretSharingNTT(size_t modulusIdx, size_t lSecretLength, size_t kDegree, size_t nShares, void (*computeSmall) (uint64_t*,uint64_t const*), void (*computeLarge) (uint64_t*,uint64_t const*)) : modulusIdx_(modulusIdx), lSecretLength_(lSecretLength), kDegree_(kDegree), nNumberShares_(nShares) {
			this->dCompositeDomainSize_ = nShares;

		    _smallDomain = new NTT(modulusIdx, this->kDegree_, computeSmall);
		    _largeDomain = new NTT(modulusIdx, this->nNumberShares_, computeLarge);
		};

		int share(uint64_t *secret);
		int reconstruct(uint64_t *eval, bool exapnded = false);

		int padMany(uint64_t *secrets, size_t lrows);
		int shareMany(uint64_t *secrets, size_t lrows);
		int reconstructMany(uint64_t *secrets, size_t lrows);

		bool degreeTest(uint64_t *eval);
		bool zeroTest(uint64_t *eval, bool expanded = false);
		bool zeroSumTest(uint64_t *eval);
		bool zeroSumTestSigmaProtocol(uint64_t *eval, std::vector<uint64_t> beta);

	protected:
		int padSecret(uint64_t *secret);
		int padPolynominal(uint64_t *secret);
		int padShares(uint64_t *secret);
		int padIntrablocRandomness(uint64_t *secret);

		size_t  modulusIdx_;
		size_t	lSecretLength_;
		size_t	kDegree_;
		size_t	nNumberShares_;
		size_t	dCompositeDomainSize_;

		NTT 	*_smallDomain;
		NTT		*_largeDomain;
};

template<typename FieldT>
class SecretSharingInterface: SecretSharingNTT {
	public:
		/* Constructors allowing some flexibility in the choice of domains */
		SecretSharingInterface(size_t modulusIdx, size_t lSecretLength, size_t kDegree, size_t nShares, void (*computeSmall) (uint64_t*,uint64_t const*), void (*computeLarge) (uint64_t*,uint64_t const*)) : SecretSharingNTT(modulusIdx, lSecretLength, kDegree, nShares, computeSmall, computeLarge) {
			this->dCompositeDomainSize_ = nShares;
		};

		using SecretSharingNTT::degreeTest;
		using SecretSharingNTT::zeroTest;
		using SecretSharingNTT::zeroSumTest;
		using SecretSharingNTT::share;
		using SecretSharingNTT::reconstruct;

		int share(FieldT *secret);
		int reconstruct(FieldT *eval, bool expanded = false);

		int padMany(FieldT *secrets, size_t lrows);
		int padIntrablocRandomness(FieldT *secret);
		int shareMany(FieldT *secrets, size_t lrows);
		int reconstructMany(FieldT *secrets, size_t lrows);

		bool degreeTest(FieldT *eval);
		bool zeroTest(FieldT *eval, bool expanded = false);
		bool zeroSumTestSigmaProtocol(FieldT *eval, std::vector<uint64_t> beta);
		bool zeroSumTest(FieldT *eval);
		FieldT sumReconstruct(FieldT *eval);

	protected:
		int padSecret(FieldT *secret);
		int padPolynominal(FieldT *secret);
		int padShares(FieldT *secret);

		// adapter
		std::vector<uint64_t> demote(FieldT *eval, size_t length);
		void promote(uint64_t *eval, size_t length, FieldT *dest);
};

// Implementation
// =============================================================================================

// Secret Sharing Interface
template <typename FieldT>
inline int SecretSharingInterface<FieldT>::padSecret(FieldT *secret) {
	FieldT::randomVector(secret + this->lSecretLength_, this->kDegree_ - this->lSecretLength_, true);
	return 0;
}

template <typename FieldT>
inline int SecretSharingInterface<FieldT>::padIntrablocRandomness(FieldT *secret) {
	for (size_t i = this->lSecretLength_; i < kDegree_ ;i++) {
		secret[i] = FieldT(0);
	}

	return 0;
}

template <typename FieldT>
inline int SecretSharingInterface<FieldT>::padPolynominal(FieldT *secret) {
	for (size_t i = this->kDegree_; i < nNumberShares_;i++) {
		secret[i] = FieldT(0);
	}

	return 0;
}

template <typename FieldT>
inline int SecretSharingInterface<FieldT>::padShares(FieldT *secret) {
	for (size_t i = this->nNumberShares_; i < dCompositeDomainSize_ ;i++) {
		secret[i] = FieldT(0);
	}

	return 0;
}

template <typename FieldT>
int SecretSharingInterface<FieldT>::padMany(FieldT *secret, size_t lrows) {

	// Add randomness for padding
	for (size_t i = 0; i < lrows; i++) {
		this->padSecret(secret + i * this->nNumberShares_);
	}

	return 0;
}

template <typename FieldT>
int SecretSharingInterface<FieldT>::share(FieldT *secret) {

	std::vector<uint64_t> secret64 = demote(secret, this->nNumberShares_);
	this->share(&secret64[0]);
	promote(&secret64[0], this->nNumberShares_, secret);

	return 0;
}

// this method is used when the resulting degree of the polynomial is larger than the domain space of the secret
template <typename FieldT>
int SecretSharingInterface<FieldT>::reconstruct(FieldT *eval, bool expandedDegree) {
	std::vector<uint64_t> eval64 = demote(eval, this->nNumberShares_);
	this->reconstruct(&eval64[0], expandedDegree);
	promote(&eval64[0], this->kDegree_, eval);

	return 0;
}

template <typename FieldT>
std::vector<uint64_t> SecretSharingInterface<FieldT>::demote(FieldT *eval, size_t length) {
	std::vector<uint64_t> temp(length);
	for (size_t idx = 0; idx < length; idx++) {
		temp[idx] = static_cast<uint64_t>(eval[idx].getValue());
	}

	return temp;
}

template <typename FieldT>
void SecretSharingInterface<FieldT>::promote(uint64_t *eval, size_t length, FieldT *dest) {
	for (size_t idx = 0; idx < length; idx++) {
		dest[idx] = FieldT(eval[idx]);
	}
}

template <typename FieldT>
bool SecretSharingInterface<FieldT>::degreeTest(FieldT *eval) {

	std::vector<uint64_t> eval64 = demote(eval,this->nNumberShares_);
	return this->degreeTest(&eval64[0]);
}

template <typename FieldT>
bool SecretSharingInterface<FieldT>::zeroTest(FieldT *eval, bool largerDegree) {

	std::vector<FieldT> localCopy(eval, eval + this->nNumberShares_);
	reconstruct(&localCopy[0], largerDegree);

	for (size_t i = 0; i < this->lSecretLength_; i++) {
		if (!(localCopy[i] == FieldT(0))) return false;
	}

	return true;
}

template <typename FieldT>
bool SecretSharingInterface<FieldT>::zeroSumTest(FieldT *eval) {

	std::vector<FieldT> localCopy(eval, eval+ this->nNumberShares_);
	reconstruct(&localCopy[0], true);

	FieldT sum = FieldT(0);
	for (size_t i = 0; i < this->lSecretLength_; i++) {
		sum += localCopy[i];
	}

	if (sum == FieldT(0)) return true;
	else return false;
}

template <typename FieldT>
FieldT SecretSharingInterface<FieldT>::sumReconstruct(FieldT *eval) {

	std::vector<FieldT> localCopy(eval, eval+ this->nNumberShares_);
	reconstruct(&localCopy[0], true);

	FieldT sum = FieldT(0);
	for (size_t i = 0; i < this->lSecretLength_; i++) {
		sum += localCopy[i];
	}

	return sum;
}

template <typename FieldT>
int SecretSharingInterface<FieldT>::shareMany(FieldT *secret, size_t lrows) {

	// std::cout << "Performing FFTs: " << lrows << std::endl;
	for (size_t i = 0; i < lrows; i++) {
		share(secret + i * this->nNumberShares_);
	}

	return 0;
}

template <typename FieldT>
int SecretSharingInterface<FieldT>::reconstructMany(FieldT *secret, size_t lrows) {

	for (size_t i = 0; i < lrows; i++) {
		reconstruct(secret + i * this->nNumberShares_);
	}

	return 0;
}

template <typename FieldT>
bool SecretSharingInterface<FieldT>::zeroSumTestSigmaProtocol(FieldT *eval, std::vector<uint64_t> beta) {
	std::vector<uint64_t> eval64 = demote(eval, this->nNumberShares_);

	return true;
}

// Secret Sharing NTT

int SecretSharingNTT::padSecret(uint64_t *secret) {

	// uint64_t::randomVector(secret + this->lSecretLength_, this->kDegree_ - this->lSecretLength_, true);
	return 0;
}

int SecretSharingNTT::padIntrablocRandomness(uint64_t *secret) {

	for (size_t i = this->lSecretLength_; i < kDegree_ ;i++) {
		secret[i] = uint64_t(0);
	}

	return 0;
}

int SecretSharingNTT::padPolynominal(uint64_t *secret) {

	for (size_t i = this->kDegree_; i < nNumberShares_;i++) {
		secret[i] = uint64_t(0);
	}

	return 0;
}

int SecretSharingNTT::padShares(uint64_t *secret) {

	for (size_t i = this->nNumberShares_; i < dCompositeDomainSize_ ;i++) {
		secret[i] = uint64_t(0);
	}

	return 0;
}

int SecretSharingNTT::padMany(uint64_t *secret, size_t lrows) {

	// Add randomness for padding
	for (size_t i = 0; i < lrows; i++) {
		this->padSecret(secret + i * this->nNumberShares_);
	}

	return 0;
}

int SecretSharingNTT::share(uint64_t *data) {

	// Interpolate Poly from Secret
	_smallDomain->inv_ntt(data);

	this->padPolynominal(data);

	// Evaluate Publicly Shared Data from Poly
	_largeDomain->ntt(data);

	return 0;
}

// this method is used when the resulting degree of the polynomial is larger than the domain space of the secret
int SecretSharingNTT::reconstruct(uint64_t *eval, bool expandedDegree) {
	size_t degree = kDegree_;

	// Interpolate Poly from Publicly Shared Data
	_largeDomain->inv_ntt(eval);

	// if (expandedDegree) degree = kDegree_*2;
	// Because we evaluate on roots of unity, we can use coefs[i] + coefs[i+k]
	// on k coefficients instead of a 2k evaluation
	if (expandedDegree) {
		for (size_t i = 0; i < kDegree_; i++) {
				if (eval[i+kDegree_] > eval[i]) {
					eval[i] = eval[i] - eval[i + kDegree_] + params<uint64_t>::P[this->modulusIdx_];
				} else {
					eval[i] = eval[i] - eval[i + kDegree_];
				}
			}
	}

	// Evaluate Poly Into Secret
	_smallDomain->ntt(eval);

	// rebuild accounting for the stride implied by the larger degree
	size_t stride = degree/this->kDegree_;
	for (size_t col = 0; col < this->lSecretLength_; col++) {
		eval[col] = eval[col*stride];
	}

	return 0;
}

bool SecretSharingNTT::degreeTest(uint64_t *eval) {

	// Interpolate Poly from Publicly Shared Data
	std::vector<uint64_t> localCopy(eval, eval + this->nNumberShares_);
	_largeDomain->inv_ntt(&localCopy[0]);

	for (size_t i = kDegree_; i < nNumberShares_; i++) {
		if (!(localCopy[i] == uint64_t(0))) {
			DBG("val:"<< localCopy[i] << ",degree>" << i);
			return false;
		}
	}

	return true;
}

bool SecretSharingNTT::zeroTest(uint64_t *eval, bool largerDegree) {

	std::vector<uint64_t> localCopy(eval, eval + this->nNumberShares_);
	reconstruct(&localCopy[0], largerDegree);

	for (size_t i = 0; i < this->lSecretLength_; i++) {
		if (!(localCopy[i] == uint64_t(0))) return false;
	}

	return true;
}

bool SecretSharingNTT::zeroSumTest(uint64_t *eval) {

	std::vector<uint64_t> localCopy(eval, eval+ this->nNumberShares_);
	reconstruct(&localCopy[0], true);

	uint64_t sum = uint64_t(0);
	for (size_t i = 0; i < this->lSecretLength_; i++) {
		sum += localCopy[i];
	}

	if (sum == uint64_t(0)) return true;
	else return false;
}

bool SecretSharingNTT::zeroSumTestSigmaProtocol(uint64_t *eval, std::vector<uint64_t> beta) {

	std::vector<uint64_t> localCopy(eval, eval+ this->nNumberShares_);
	reconstruct(&localCopy[0], true);

	for (size_t i = 0; i < this->lSecretLength_; i++) {
		if (localCopy[i] != beta[i]) return false;
	}

	return true;
}

int SecretSharingNTT::shareMany(uint64_t *secret, size_t lrows) {

	// std::cout << "Performing FFTs: " << lrows << std::endl;
	for (size_t i = 0; i < lrows; i++) {
		share(secret + i * this->nNumberShares_);
	}

	return 0;
}

int SecretSharingNTT::reconstructMany(uint64_t *secret, size_t lrows) {

	for (size_t i = 0; i < lrows; i++) {
		reconstruct(secret + i * this->nNumberShares_);
	}

	return 0;
}

}


/**
 * \file SD1OverdriveFilter.cpp
 */

#include <ATK/Distortion/SD1OverdriveFilter.h>

#include <stdexcept>

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

#include <ATK/Utility/fmath.h>
#include <ATK/Utility/ScalarNewtonRaphson.h>

namespace ATK
{
  template<typename DataType_>
  class SD1OverdriveFilter<DataType_>::SD1OverdriveFunction
  {
  public:
    using DataType = DataType_;
  protected:
    const DataType R;
    const DataType R1;
    const DataType C;
    const DataType Q;
    DataType drive = 0.5;
    const DataType is;
    const DataType vt;

    DataType ieq = 0;
    DataType i = 0;

    DataType expdiode_y1_p = 1;
    DataType expdiode_y1_m = 1;

  public:
    SD1OverdriveFunction(DataType dt, DataType R, DataType C, DataType R1, DataType Q, DataType is, DataType vt)
      :R(R), R1(R1), C(2 * C / dt), Q(Q), is(is), vt(vt)
    {
    }

    void set_drive(DataType drive)
    {
      this->drive = (R1 + drive * Q);
    }

    std::pair<DataType, DataType> operator()(const DataType* ATK_RESTRICT input, DataType* ATK_RESTRICT output, DataType y1)
    {
      auto x1 = input[0];
      y1 -= x1;
      expdiode_y1_p = fmath::exp(y1 / vt);
      expdiode_y1_m = 1 / expdiode_y1_p;

      DataType diode1 = is * (expdiode_y1_p - 2 * expdiode_y1_m + 1);
      DataType diode1_derivative = is * (expdiode_y1_p + 2 * expdiode_y1_m) / vt;

      i = (C * x1 - ieq) / (1 + R * C);

      return std::make_pair(y1 / drive + diode1 - i, 1 / drive + diode1_derivative);
    }

    void update_state(const DataType* ATK_RESTRICT input, DataType* ATK_RESTRICT output)
    {
      auto x1 = input[0];
      ieq = 2 * C * (x1 - i * R) - ieq;
    }

    DataType estimate(const DataType* ATK_RESTRICT input, DataType* ATK_RESTRICT output)
    {
      auto x0 = input[-1];
      auto x1 = input[0];
      auto y0 = output[-1];
      return affine_estimate(x0, x1, y0);
    }

    DataType affine_estimate(DataType x0, DataType x1, DataType y0)
    {
      y0 -= x0;
      auto sinh = is * (expdiode_y1_p - 2 * expdiode_y1_m + 1);
      auto cosh = is * (expdiode_y1_p + 2 * expdiode_y1_m);
      auto i = (C * x1 - ieq) / (1 + R * C);

      return (i - (sinh - y0 / vt * cosh)) / (cosh / vt + (1 / drive)) + x1;
    }
  };

  template <typename DataType>
  SD1OverdriveFilter<DataType>::SD1OverdriveFilter()
    :TypedBaseFilter<DataType>(1, 1)
  {
    input_delay = 1;
    output_delay = 1;
  }

  template <typename DataType>
  SD1OverdriveFilter<DataType>::~SD1OverdriveFilter()
  {
  }

  template <typename DataType>
  void SD1OverdriveFilter<DataType>::setup()
  {
    Parent::setup();
    optimizer = std::make_unique<ScalarNewtonRaphson<SD1OverdriveFunction, num_iterations, true>>(SD1OverdriveFunction(static_cast<DataType>(1. / input_sampling_rate),
      static_cast<DataType>(4.7e3), static_cast<DataType>(0.047e-6), static_cast<DataType>(33e3),
      static_cast<DataType>(1e6), static_cast<DataType>(1e-12), static_cast<DataType>(26e-3)));

    optimizer->get_function().set_drive(drive);
  }

  template <typename DataType_>
  void SD1OverdriveFilter<DataType_>::set_drive(DataType_ drive)
  {
    if(drive < 0 || drive > 1)
    {
      throw std::out_of_range("Drive must be a value between 0 and 1");
    }
    this->drive = drive;
    if(optimizer)
    {
        optimizer->get_function().set_drive(drive);
    }
  }

  template <typename DataType_>
  DataType_ SD1OverdriveFilter<DataType_>::get_drive() const
  {
    return drive;
  }

  template <typename DataType>
  void SD1OverdriveFilter<DataType>::process_impl(gsl::index size) const
  {
    const DataType* ATK_RESTRICT input = converted_inputs[0];
    DataType* ATK_RESTRICT output = outputs[0];
    for(gsl::index i = 0; i < size; ++i)
    {
      optimizer->optimize(input + i, output + i);
      optimizer->get_function().update_state(input + i, output + i);
    }
  }

#if ATK_ENABLE_INSTANTIATION
  template class SD1OverdriveFilter<float>;
#endif
  template class SD1OverdriveFilter<double>;
}


#include "CEGO/CEGO.hpp"
#include <Eigen/Dense>
#if defined(PYBIND11)
#include <pybind11/embed.h>
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <pybind11/stl.h>
#include <pybind11/eigen.h>
namespace py = pybind11;
#include <atomic>

std::atomic_size_t Ncalls(0);

inline bool ValidNumber(double x)
{
    // Idea from http://www.johndcook.com/IEEE_exceptions_in_cpp.html
    return (x <= DBL_MAX && x >= -DBL_MAX);
};

enum fitting_options{ FIT_P, FIT_RHOV, FIT_RHOL};

class RatPolyAncillary {
public:
    double m_Tt, m_Tc, m_pc, m_Dc;
    std::size_t m_Nnum, m_Nden;
    Eigen::ArrayXd m_LHS, m_THETA, m_T;
    std::string m_name;
    fitting_options to_fit = FIT_RHOV;
    RatPolyAncillary(const std::string &name, const std::size_t Nnum, const std::size_t Nden) : m_Nnum(Nnum), m_Nden(Nden), m_name(name)
    {
        py::module ct = py::module::import("ctREFPROP.ctREFPROP"); // Import cytpes wrapper of REFPROP dll
        std::string RPPREFIX(getenv("RPPREFIX"));
        auto R = ct.attr("REFPROPFunctionLibrary")(RPPREFIX + "/REFPRP64.DLL");
        auto version = R.attr("RPVersion")().cast<std::string>();
        auto setup_info = R.attr("SETUPdll")(1, name+".FLD", "HMX.BNC","DEF");
        if (setup_info.attr("ierr").cast<int>() != 0) {
            throw std::invalid_argument( setup_info.attr("herr").cast<std::string>() );
        }
        auto info = R.attr("INFOdll")(1);
        m_pc = info.attr("Pc").cast<double>();
        m_Tc = info.attr("Tc").cast<double>();
        m_Tt = info.attr("Ttrp").cast<double>();
        m_Dc = info.attr("Dc").cast<double>();

        std::size_t N = 300;
        m_LHS.resize(N);
        m_THETA.resize(N);
        m_T.resize(N);
        std::vector<double> z(1,1);
        Eigen::ArrayXd Tvec = Eigen::ArrayXd::LinSpaced(N, m_Tc*0.999, 0.5*m_Tt);
        Eigen::Index j = 0;

        for (auto i = 0; i < Tvec.size(); ++i){
            auto o = R.attr("TQFLSHdll")(Tvec[i], 1, &z, 0);
            int ierr = o.attr("ierr").cast<int>();
            if (ierr <= 100){
                //std::cout << Tvec(i) << " " << o << std::endl;
                if (to_fit == FIT_P){
                    double p = o.attr("P").cast<double>();
                    if (!ValidNumber(p)){ continue; }
                    m_LHS(j) = log(p/m_pc)*Tvec(i)/m_Tc;
                }
                else if (to_fit == FIT_RHOL) {
                    double DL = o.attr("Dl").cast<double>();
                    if (!ValidNumber(DL)) { continue; }
                    m_LHS(j) = DL/m_Dc;
                }
                else if (to_fit == FIT_RHOV) {
                    double DV = o.attr("Dv").cast<double>();
                    if (!ValidNumber(DV)) { continue; }
                    m_LHS(j) = log(DV / m_Dc);//*Tvec(i)/m_Tc;
                }
                else {
                    throw std::invalid_argument("Argument for what to fit is invalid");
                }
                m_THETA(j) = 1-(Tvec(i)/m_Tc);
                m_T(j) = Tvec(i);
                j++;
            }
        }
        m_LHS.conservativeResize(j-1);
        m_THETA.conservativeResize(j-1);
        m_T.conservativeResize(j - 1);
    }
    void plot_curve(){
        py::module plt = py::module::import("matplotlib.pyplot"); // Import matplotlib
        plt.attr("plot")(m_THETA, m_LHS);
        plt.attr("show")();
    }
    void plot_deviation(const Eigen::ArrayXd &c) {

        py::module plt = py::module::import("matplotlib.pyplot"); // Import matplotlib
        Eigen::ArrayXd T = m_Tc*(1+m_THETA), deviation;
        std::string ylabel;
        if (to_fit == FIT_P){
            Eigen::ArrayXd pfit = eval_RHS(m_THETA, c).exp();
            Eigen::ArrayXd peos = m_LHS.exp();
            deviation = (pfit/peos-1)*100;
            ylabel = "Pressure deviation (%)";
        }
        else if (to_fit == FIT_RHOV) {
            Eigen::ArrayXd rhofit = eval_RHS(m_THETA, c).exp()*m_Dc;
            Eigen::ArrayXd rhoeos = m_LHS.exp()*m_Dc;
            deviation = (rhofit / rhoeos - 1) * 100;
            ylabel = "Density deviation (%)";
        }
        else if (to_fit == FIT_RHOL) {
            Eigen::ArrayXd rhofit = eval_RHS(m_THETA, c)*m_Dc;
            Eigen::ArrayXd rhoeos = m_LHS;
            deviation = (rhofit / rhoeos - 1) * 100;
            ylabel = "Density deviation (%)";
        }
        using namespace pybind11::literals;
        plt.attr("plot")(1 / (m_THETA + 1), deviation);
        plt.attr("ylabel")(ylabel);
        plt.attr("axvline")(m_Tt / m_Tc, "dashes"_a = std::vector<double>(2, 2));
        plt.attr("xlabel")("$T/T_c$");
        plt.attr("savefig")(m_name + ".pdf");
        plt.attr("close")();
    }
    Eigen::ArrayXd eval_RHS(const Eigen::ArrayXd& x, const Eigen::ArrayXd &c) {
        Eigen::ArrayXd num = Eigen::ArrayXd::Zero(x.size()), den = Eigen::ArrayXd::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);
    }
    double objective(const CEGO::AbstractIndividual *pind) {
        const std::vector<double> &c = static_cast<const CEGO::NumericalIndividual<double>*>(pind)->get_coefficients();
        return objective(Eigen::Map<const Eigen::ArrayXd>(&(c[0]), c.size()) );
    }
    double objective(const Eigen::ArrayXd &c) {
        //Ncalls++;
        return (eval_RHS(m_THETA, c) - m_LHS).square().sum();
    }
    void plot_trace(const std::vector<double> &best_costs) {
        py::module plt = py::module::import("matplotlib.pyplot"); // Import matplotlib
        plt.attr("plot")(best_costs);
        plt.attr("show")();
    }
};

int main()
{
    py::scoped_interpreter interp{};
    std::srand((unsigned int)time(0));

    std::size_t Nnum = 4, Nden = 4;

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

    auto Ncalls = 0;
    CEGO::CostFunction cost_wrapper = std::bind((double (RatPolyAncillary::*)(const CEGO::AbstractIndividual *)) &RatPolyAncillary::objective, &rp, std::placeholders::_1);
    auto Nlayers = 1;
    auto layers = CEGO::Layers<double>(cost_wrapper, bounds.size(), 40, Nlayers, 5);
    layers.parallel = true;
    layers.set_bounds(bounds);

    auto flags = layers.get_evolver_flags();
    flags["Nelite"] = 2;
    flags["Fmin"] = 0.5;
    flags["Fmax"] = 0.5;
    flags["CR"] = 0.9;
    layers.set_evolver_flags(flags);

    std::vector<double> best_costs;
    std::vector<std::vector<double> > objs;
    double VTR = 1e-6, best_cost = 999999.0;
    auto startTime = std::chrono::system_clock::now();
    for (auto counter = 0; counter < 50000; ++counter) {
        layers.do_generation();

        // Store the best objective function in each layer
        std::vector<double> oo;
        for (auto &&cost_coefficients : layers.get_best_per_layer()) {
            oo.push_back(std::get<0>(cost_coefficients));
        }
        objs.push_back(oo);

        // For the overall best result, print it, and write JSON to file
        auto best_layer = layers.get_best();
        best_cost = std::get<0>(best_layer); best_costs.push_back(best_cost);
        if (counter % 50 == 0) {
            std::cout << counter << ": best: " << best_cost << "\n ";// << CEGO::vec2string(best_coeffs) << "\n";
            //auto c = std::get<1>(best_layer);
            //rp.plot_deviation(Eigen::Map<const Eigen::ArrayXd>(&(c[0]), c.size()));
        }
        if (best_cost < VTR){ break; }
    }
    auto best_layer = layers.get_best();
    auto c = std::get<1>(best_layer);
    rp.plot_deviation(Eigen::Map<const Eigen::ArrayXd>(&(c[0]), c.size()));
    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;

}

#else
int main(){
    std::cout << "Due to missing support for pybind11, this file cannot be run\n";
}
#endif


#include <omp.h>

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

#include "qpp.h"

using namespace qpp;
using uint = unsigned int;

struct SimState {
  ket state = ket::Zero(1);
  int num_qubits = 0;
  SimState() { state << 1; }
};

typedef SimState* state_t;

extern "C" state_t empty() { return new SimState; }

extern "C" void discard(state_t s) { delete s; }

extern "C" int qinit(state_t s) {
  s->state = kron(s->state, 0_ket);
  return s->num_qubits++;
}

enum Gate : int {
  X = 0,
  Y = 1,
  Z = 2,
  H = 3,
  CNOT = 4,
  CZ = 5,
  TOF = 6,
  FRED = 7,
  PHASE = 8,
  CPHASE = 9
};

extern "C" void unitary1(state_t s, Gate g, uint q) {
  cmat u;
  switch (g) {
    case X:
      u = gt.X;
      break;
    case Y:
      u = gt.Y;
      break;
    case Z:
      u = gt.Z;
      break;
    case H:
      u = gt.H;
      break;
    default:
      abort();
  }
  s->state = apply(s->state, u, {q});
}

extern "C" void unitary2(state_t s, Gate g, uint q1, uint q2) {
  cmat u;
  switch (g) {
    case CNOT:
      u = gt.CNOT;
      break;
    case CZ:
      u = gt.CZ;
      break;
    default:
      abort();
  }
  s->state = apply(s->state, u, {q1, q2});
}

extern "C" void unitary3(state_t s, Gate g, uint q1, uint q2, uint q3) {
  cmat u;
  switch (g) {
    case TOF:
      u = gt.TOF;
      break;
    case FRED:
      u = gt.FRED;
      break;
    default:
      abort();
  }
  s->state = apply(s->state, u, {q1, q2, q3});
}

extern "C" void punitary1(state_t s, Gate g, uint q, double p) {
  cmat u = std::polar(1.0, M_PI * p) * gt.RZ(2 * M_PI * p);
  switch (g) {
    case PHASE:
      break;
    default:
      abort();
  }
  s->state = apply(s->state, u, {q});
}

extern "C" void punitary2(state_t s, Gate g, uint q1, uint q2, double p) {
  cmat u = std::polar(1.0, M_PI * p) * gt.RZ(2 * M_PI * p);
  switch (g) {
    case CPHASE:
      break;
    default:
      abort();
  }
  s->state = applyCTRL(s->state, u, {q1}, {q2});
}

extern "C" bool measure(state_t s, uint q) {
  --s->num_qubits;
  auto measured = measure_seq(s->state, {q});
  s->state = std::get<ST>(measured);
  return std::get<RES>(measured)[0];
}

inline uint swap_bits(uint x, uint p1, uint p2) {
  const uint y = ((x >> p1) & 1) ^ ((x >> p2) & 1);
  return x ^ ((y << p1) | (y << p2));
}

inline void swap(ket& state, const idx numdims, const uint* const qs, uint n) {
  using namespace Eigen;
  PermutationMatrix<Dynamic, Dynamic> perm(1UL << numdims);

#ifdef HAS_OPENMP
#pragma omp parallel for
#endif
  for (idx i = 0; i < 1UL << numdims; ++i) {
    idx j = i;
    for (uint k = 0; k < n; ++k) {
      j = swap_bits(j, numdims - qs[n - k - 1] - 1, k);
    }
    perm.indices()[i] = j;
  }

  state = perm * state;
}

extern "C" bool separable(state_t s, const uint* const qs, uint n) {
  ket k = s->state;
  swap(k, s->num_qubits, qs, n);
  const idx dim = 1UL << n;
  const idx rem = 1UL << (s->num_qubits - n);
  auto coeffs = schmidtcoeffs(k, {rem, dim});
  return std::count_if(coeffs.data(), coeffs.data() + coeffs.size(),
                       [](double coeff) { return fabs(coeff) > chop; }) == 1;
}

extern "C" void print(state_t s) {
  if (s->state.size() < 2) {
    std::cout << "(empty)" << std::endl;
  } else {
    for (idx i = 0; i < s->state.size(); ++i) {
      std::bitset<32> bs(i);
      std::cout << "|";
      for (int j = s->num_qubits - 1; j >= 0; --j) {
        std::cout << bs[j];
      }
      std::cout << "> : " << s->state[i].real() << " + " << s->state[i].imag()
                << "i" << std::endl;
    }
  }
}


#include <iostream>

#include <Eigen/Core>  // Eigen 核心部分
#include <Eigen/Dense>  // 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Geometry>  // Eigen/Geometry 模块提供了各种旋转和平移的表示

#include "sophus/se3.hpp"

#include <opencv2/core.hpp>  // 核心功能，包括基本数据结构
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>  // 高层GUI图像交互
#include <opencv2/features2d.hpp>  // 特征提取、描述、匹配

// 相机内参
const double f = 521, cx = 325.1, cy = 249.7;

bool ORBFeatureDetectAndMatch(
    cv::Mat &img_1, cv::Mat &img_2,
    std::vector<cv::KeyPoint> &kps_1,
    std::vector<cv::KeyPoint> &kps_2,
    std::vector<cv::DMatch> &good_matches
);

bool PoseEstimation2d2d(
    std::vector<cv::KeyPoint> &kps_1,
    std::vector<cv::KeyPoint> &kps_2,
    std::vector<cv::DMatch> &good_matches,
    cv::Mat &R,
    cv::Mat &t
){
    std::vector<cv::Point2f> points_1;
    std::vector<cv::Point2f> points_2;
    for(size_t i = 0; i < good_matches.size(); i++){
        points_1.push_back(kps_1[good_matches[i].queryIdx].pt);
        points_2.push_back(kps_2[good_matches[i].trainIdx].pt);
    }

    cv::Mat fundamental_matrix;
    fundamental_matrix = cv::findFundamentalMat(points_1, points_2, CV_FM_8POINT);
    std::cout << "fundamental matrix = \n" << fundamental_matrix << std::endl;

    const cv::Point2d principal_point(cx, cy);
    cv::Mat essential_matrix;
    essential_matrix = cv::findEssentialMat(points_1, points_2, f, principal_point);
    std::cout << "essential matrix = \n" << essential_matrix << std::endl;

    cv::recoverPose(essential_matrix, points_1, points_2, R, t, f, principal_point);
}

int main(int argc, char **argv){
    std::string file_path_1 = argv[1];
    std::string file_path_2 = argv[2];

    cv::Mat img_1 = cv::imread(file_path_1, cv::IMREAD_COLOR);
    cv::Mat img_2 = cv::imread(file_path_2, cv::IMREAD_COLOR);

    if (img_1.empty() || img_2.empty()){
        std::cerr << "cannot find image: " << file_path_1 << " or " << file_path_2 << std::endl;
        return -1;
    }

    // ORB feature detection and match
    std::vector<cv::KeyPoint> kps_1, kps_2;
    std::vector<cv::DMatch> good_matches;
    ORBFeatureDetectAndMatch(img_1, img_2, kps_1, kps_2, good_matches);

    // show the keypoints and matches
    cv::Mat img_matches;
    cv::drawMatches(img_1, kps_1, img_2, kps_2, good_matches, img_matches);
    cv::imshow("matches between img_1 & img_2", img_matches);

    cv::Mat R, t;
    PoseEstimation2d2d(kps_1, kps_2, good_matches, R, t);
    std::cout << "R = \n" << R << std::endl;
    std::cout << "t = \n" << t << std::endl;

    cv::waitKey(0);
    return 0;
}

bool ORBFeatureDetectAndMatch(
    cv::Mat &img_1, cv::Mat &img_2,
    std::vector<cv::KeyPoint> &kps_1,
    std::vector<cv::KeyPoint> &kps_2,
    std::vector<cv::DMatch> &good_matches
    ){
    // orb提取特征点和描述子
    cv::Mat descriptor_1, descriptor_2;
    cv::Ptr<cv::ORB> orb = cv::ORB::create();
    orb->detectAndCompute(img_1, cv::noArray(), kps_1, descriptor_1);
    orb->detectAndCompute(img_2, cv::noArray(), kps_2, descriptor_2);

    // cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");
    // std::vector<cv::DMatch> match;
    // matcher->match(descriptor_1, descriptor_2, match);

    // // --第四步：匹配点对 筛选
    // auto min_max = minmax_element(match.begin(), match.end(),
    //             [] (const cv::DMatch &m1, const cv::DMatch &m2) {return m1.distance < m2.distance;});
    // double min_dist = min_max.first->distance;
    // double max_dist = min_max.second->distance;

    // for (int i=0; i<descriptor_1.rows; i++){
    //     if(match[i].distance <= std::max(2*min_dist, 30.0)){
    //         good_matches.push_back(match[i]);
    //     }
    // }

    // 进行匹配
    cv::Ptr<cv::DescriptorMatcher> matcher =
        cv::DescriptorMatcher::create("BruteForce-Hamming");
    std::vector<cv::DMatch> matches;
    matcher->match(descriptor_1, descriptor_2, matches);

    double min_dist = 10000;
    for (auto &m: matches){
        if (m.distance < min_dist){
            min_dist = m.distance;
        }
    }

    std::cout << "min_dist = " << min_dist << std::endl;

    for(size_t i = 0; i < matches.size(); i++){
        if (matches[i].distance <= std::max(2 * min_dist, 30.0)){
            good_matches.push_back(matches[i]);
        }
    }
    std::cout << "have found " << good_matches.size() << " good matches. " << std::endl;
    return true;
}


#include <boost/thread.hpp>
#include <iostream>
#include <vector>
#include <cstdlib>
#include <ctime>
#include <math.h>
#include <c2_algorithm.h>

using namespace std;

c2_algorithm::c2_algorithm()
{
    pos = 0;
    t_pos = 0;
    vel = 0;
    t_vel = 0;
    acc = 0;
    t_acc = 0;
    last_vel = 0;
    last_acc = 0;
}
c2_algorithm::~c2_algorithm()
{
}

double c2_algorithm::sign(double x)
{
    if (x > 0)
    {
        return 1;
    }
    else if (x == 0)
    {
        return 0;
    }
    else
    {
        return -1;
    }
}

double c2_algorithm::power(double x, int n)
{
    double val = 1.0;
    while (n--)
        val *= x;
    return val;
}

void c2_algorithm::prepare()
{
    ek = (pos - t_pos) / jeck;
    ek1 = (vel - t_vel) / jeck;
}

double c2_algorithm::sat(double x)
{
    if (x < -1)
    {
        return -1;
    }
    else if (x >= -1 && x <= 1)
    {
        return x;
    }
    else
    {
        return 1;
    }
}

void c2_algorithm::c2_function()
{
    double ts = (double)1 / frequency;
    double zk = 1 / ts * (ek / ts + ek1 / 2);
    double zk1 = ek1 / ts;
    double m1 = 1 + 8 * abs(zk);
    double m = floor((1 + sqrt(m1)) / 2);
    double omk = zk1 + zk / m + (m - 1) / 2 * sign(zk);
    double uk1 = vel * sign(omk) + max_vel - ts * jeck;
    double uk2 = 1 + sign(uk1);
    uk = -1 * jeck * sat(omk) * uk2 / 2;
}

void c2_algorithm::get_qk(double l_vel, double l_pos, double &vel_, double &pos_)
{
    prepare();
    c2_function();
    double ts = (double)1 / frequency;
    qk1 = l_vel + ts * uk;
    qk = l_pos + ts / 2 * (qk1 + l_vel);

    vel = qk1;
    pos = qk;

    vel_ = qk1;
    pos_ = qk;
}

void c2_algorithm::start(double c_pos, double target_pos, double c_vel, double target_vel)
{
    pos = c_pos;
    t_pos = target_pos;
    vel = c_vel;
    t_vel = target_vel;
}

void c2_algorithm::init(double min_vel_, double max_vel_, double min_acc_, double max_acc_, double jeck_, int frequency_ = 100)
{
    min_vel = min_vel_;
    max_vel = max_vel_;
    min_acc = min_acc_;
    max_acc = max_acc_;
    jeck = jeck_;
    frequency = frequency_;
}


#include <iostream>                    // std::cout, std::flush
#include <fstream>                     // std::ifstream, std::ofstream
#include <filesystem>                  // std::filesystem
#include <string>                      // std::string
#include <vector>                      // std::vector
// For External Library
#include <boost/program_options.hpp>   // boost::program_options

#define NORMAL 0
#define ANOMALY 1

// Define Namespace
namespace fs = std::filesystem;
namespace po = boost::program_options;

// Function Prototype
bool judgement(double value, bool class_name, double threshold);

// ----------------------------
// Anomaly Detection Function
// ----------------------------
void anomaly_detection(po::variables_map &vm){

    // (0) Initialization and Declaration
    size_t i, j;
    size_t TP, FP, TN, FN;
    size_t total_data, idx;
    double data_one;
    double min, max, step, thresh;
    double SED, SED_min, cutoff_idx, cutoff_th;
    double TP_rate, FP_rate, TN_rate, FN_rate, pre_TP_rate, pre_FP_rate;
    double precision, recall, specificity;
    double accuracy, F, AUC;
    std::ifstream ifs;
    std::ofstream ofs;
    std::string result_dir, result_path;
    std::vector<double> data[2];

    // (1) Set Directory and Path
    result_dir = vm["AD_result_dir"].as<std::string>();
    result_path = vm["AD_result_dir"].as<std::string>() + "/accuracy.csv";
    fs::create_directories(result_dir);

    // (2.1) Set Anomaly Data
    ifs.open(vm["anomaly_path"].as<std::string>());
    ifs >> data_one;
    min = data_one;
    max = data_one;
    data[ANOMALY].push_back(data_one);
    while (true){
        ifs >> data_one;
        if (ifs.eof()) break;
        if (data_one > max){
            max = data_one;
        }
        else if (data_one < min){
            min = data_one;
        }
        data[ANOMALY].push_back(data_one);
    }
    ifs.close();

    // (2.2) Set Normal Data
    ifs.open(vm["normal_path"].as<std::string>());
    while (true){
        ifs >> data_one;
        if (ifs.eof()) break;
        if (data_one > max){
            max = data_one;
        }
        else if (data_one < min){
            min = data_one;
        }
        data[NORMAL].push_back(data_one);
    }
    ifs.close();

    // (3) Pre-Processing
    ofs.open(result_path);
    ofs << "threshold,TP,FP,TN,FN,TP rate,FP rate,TN rate,FN rate,SED,precision,recall,specificity,accuracy,F" << std::endl;
    total_data = data[NORMAL].size() + data[ANOMALY].size();
    std::cout << "total anomaly detection data : " << total_data << std::endl;

    // (4) Calculation of Step
    step = (double)(max - min) / (double)(vm["n_thresh"].as<size_t>() - 2);
    if (step == 0.0){
        step = 1.0;
    }

    // (5) Anomaly Detection
    AUC = 0.0;
    pre_TP_rate = 1.0;
    pre_FP_rate = 1.0;
    SED_min = 1.0;
    idx = 1;
    cutoff_idx = 1;
    cutoff_th = min;
    thresh = min;
    for (i = 0; i < vm["n_thresh"].as<size_t>(); i++){

        thresh += step;
        idx++;

        // (5.1) Get TP, FN, TN and FP
        TP = 0, FP = 0, TN = 0, FN = 0;
        for (j = 0; j < data[ANOMALY].size(); j++){
            if (judgement(data[ANOMALY][j], ANOMALY, thresh)){
                TP++;
            }
            else{
                FN++;
            }
        }
        for (j = 0; j < data[NORMAL].size(); j++){
            if (judgement(data[NORMAL][j], NORMAL, thresh)){
                TN++;
            }
            else{
                FP++;
            }
        }

        // (5.2) Calculation of Accuracy
        TP_rate = (double)TP / (double)data[ANOMALY].size();
        FP_rate = (double)FP / (double)data[NORMAL].size();
        TN_rate = (double)TN / (double)data[NORMAL].size();
        FN_rate = (double)FN / (double)data[ANOMALY].size();
        precision = (double)TP / (double)(TP + FP);
        recall = (double)TP / (double)(TP + FN);
        specificity = (double)TN / (double)(FP + TN);
        accuracy = (double)(TP + TN) / (double)total_data;
        F = (double)TP / ((double)TP + 0.5 * (double)(FP + FN));
        AUC += (pre_TP_rate + TP_rate) * (pre_FP_rate - FP_rate) * 0.5;
        pre_TP_rate = TP_rate;
        pre_FP_rate = FP_rate;

        // (5.3) Calculation of Cut-Off Value
        SED = (1.0 - TP_rate) * (1.0 - TP_rate) + FP_rate * FP_rate;
        if (SED < SED_min){
            SED_min = SED;
            cutoff_idx = idx;
            cutoff_th = thresh;
        }

        // (5.4) File Output
        ofs << thresh << "," << std::flush;
        ofs << TP << "," << FP << "," << TN << "," << FN << "," << std::flush;
        ofs << TP_rate << "," << FP_rate << "," << TN_rate << "," << FN_rate << "," << std::flush;
        ofs << SED << "," << std::flush;
        ofs << precision << "," << recall << "," << specificity << "," << std::flush;
        ofs << accuracy << "," << F << std::endl;

    }

    // (6) File Output
    ofs << std::endl;
    ofs << "ROC-AUC," << AUC << std::endl;
    ofs << "index(Cut-Off)," << cutoff_idx << std::endl;
    ofs << "threshold(Cut-Off)," << cutoff_th << std::endl;

    // Post Processing
    ofs.close();

    // End Processing
    return;

}

// ----------------------------
// Anomaly Judgement Function
// ----------------------------
bool judgement(double value, bool class_name, double threshold){

    bool judge;

    if (value < threshold){
        judge = false;
    }
    else{
        judge = true;
    }

    if (class_name == NORMAL){
        return !judge;
    }

    return judge;

}


/*******
edit_distance: STL and Boost compatible edit distance functions for C++

Copyright (c) 2013 Erik Erlandson

Author:  Erik Erlandson <erikerlandson@yahoo.com>

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

#include "edit_distance_common.hpp"

// get the edit_alignment() function
#include <boost/algorithm/sequence/edit_distance.hpp>
using boost::algorithm::sequence::edit_distance;
using namespace boost::algorithm::sequence::parameter;
using boost::algorithm::sequence::unit_cost;

int main(int argc, char** argv) {
    char const* str1 = "Oh, hello world.";
    char const* str2 = "Hello world!!";

    // Generate the edit script from str1 to str2.
    // The output object 'out' processes the edit script operations in sequence
    // An output class must define types value_type and cost_type (similar to
    // the cost function object for edit_distance), and the methods:
    //   insertion(v, c)        // element v inserted, with insertion cost c
    //   deletion(v, c)        // element v deleted, with deletion cost c
    //   substitution(v1, v2, c)   // v1 subsituted with v2, subst cost c
    //   equality(v1, v2)      // v1 == v2
    //
    // Defining substitution() is optional if substitution is compile-time disabled (the default)
    // To enable, pass the optional _substitution=boost::true_type(), or _substitution=<bool-value>
    stringstream_tuple_output<unit_cost, char const*> out;
    unsigned dist = edit_distance(str1, str2, _script = out);
    std::cout << "dist= " << dist << "   edit operations= " << out.ss.str() << "\n";

    return 0;
}


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  int array[24];
for(int i = 0; i < 24; ++i) array[i] = i;
cout << Map<MatrixXi, 0, Stride<Dynamic,2> >
         (array, 3, 3, Stride<Dynamic,2>(8, 2))
     << endl;

  return 0;
}


/*
 * Copyright Andrey Semashev 2020
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * https://www.boost.org/LICENSE_1_0.txt)
 */
/*!
 * \file bit_width.hpp
 *
 * This header defines \c bit_width algorithm, which produces the minimal number of
 * bits required to represent an integer value.
 */

#ifndef BOOST_BIT_OPS_POW2_BIT_WIDTH_HPP_INCLUDED_
#define BOOST_BIT_OPS_POW2_BIT_WIDTH_HPP_INCLUDED_

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

namespace boost {
namespace bit_ops {

/*!
 * \brief Returns the minimal number of bits required to represent \a value
 *
 * \pre \a value must not be zero
 */
template< typename T >
inline typename bit_ops::detail::enable_if<
    bit_ops::detail::is_integral< T >::value && bit_ops::detail::is_unsigned< T >::value,
    unsigned int
>::type bit_width_nz(T value) BOOST_NOEXCEPT
{
    return static_cast< unsigned int >(std::numeric_limits< T >::digits - bit_ops::countl_zero_nz(value));
}

//! Returns the minimal number of bits required to represent \a value or 0 if \a value is zero
template< typename T >
inline typename bit_ops::detail::enable_if<
    bit_ops::detail::is_integral< T >::value && bit_ops::detail::is_unsigned< T >::value,
    unsigned int
>::type bit_width(T value) BOOST_NOEXCEPT
{
    return value == 0u ? 0u : bit_ops::bit_width_nz(value);
}

} // namespace bit_ops
} // namespace boost

#endif // BOOST_BIT_OPS_POW2_BIT_WIDTH_HPP_INCLUDED_


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2::sum1 function"

#include <nt2/table.hpp>
#include <nt2/include/functions/idxy_bilinear.hpp>
#include <nt2/include/functions/linspace.hpp>
#include <nt2/include/functions/transpose.hpp>
#include <nt2/include/functions/isequal.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>
#include <boost/fusion/include/make_vector.hpp>

// NT2_TEST_CASE_TPL( idxy_bilinear2, (float)(double))//NT2_TYPES )
// {
//   using nt2::_;
//   nt2::table<T> x =  nt2::reshape(nt2::linspace(T(1),  T(16), 16), 4, 4);
//   nt2::table<T> xi=  nt2::linspace(T(1),  T(4), 7);
//   nt2::table<T> yi=  nt2::linspace(T(0),  T(6), 11);
//   NT2_DISPLAY(x);
//   NT2_DISPLAY(xi);
//   nt2::table<T> r;
//   r =nt2::idxy_bilinear(x, xi, xi, true);
//   NT2_DISPLAY(r);
//   r =nt2::idxy_bilinear(x, xi, yi, true);
//   NT2_DISPLAY(r);
//   r =nt2::idxy_bilinear(x, xi, yi);
//   NT2_DISPLAY(r);
//   r =nt2::idxy_bilinear(x, xi, yi, _, 1, 2);
//   NT2_DISPLAY(r);

//  }
NT2_TEST_CASE_TPL( idxy_bilinear2b, (float))//(double))//NT2_TYPES )
{
  using nt2::_;
  nt2::table<T> x =  nt2::reshape(nt2::linspace(T(1),  T(16), 16), 4, 4);
  nt2::table<T> xi=  nt2::linspace(T(1),  T(4), 7);
  nt2::table<T> yi=  nt2::linspace(T(1),  T(4), 11);
  NT2_DISPLAY(x);
  NT2_DISPLAY(xi);
  NT2_DISPLAY(yi);
  nt2::table<T> r, r0;
  r =nt2::idxy_bilinear(x, xi, yi);
  NT2_DISPLAY(r);
  r0 =nt2::idxy_bilinear(x, xi, yi, true);
  NT2_DISPLAY(r0);
  NT2_TEST(isequal(r0, r));
  r =nt2::idxy_bilinear(x, xi, yi, _, 2, 1);
  NT2_DISPLAY(r);
  NT2_TEST(isequal(r0, r));
 }
NT2_TEST_CASE_TPL( idxy_bilinear2c, (float))//(double))//NT2_TYPES )
{
  using nt2::_;
  nt2::table<T> x =  nt2::reshape(nt2::linspace(T(1),  T(16), 16), 4, 4);
  nt2::table<T> xi=  nt2::linspace(T(0),  T(5), 7);
  nt2::table<T> yi=  nt2::linspace(T(0),  T(5), 11);
  NT2_DISPLAY(x);
  NT2_DISPLAY(xi);
  NT2_DISPLAY(yi);
  nt2::table<T> r, r0;
  r =nt2::idxy_bilinear(x, xi, yi);
  NT2_DISPLAY(r);
  r0 =nt2::idxy_bilinear(x, xi, yi, true);
  NT2_DISPLAY(r0);
  r =nt2::idxy_bilinear(x, xi, yi, _, 2, 1);
  NT2_DISPLAY(r);
  NT2_TEST_COMPLETE("idxy_bilinear2c");
 }


// die.cpp
//
// Copyright (c) 2009
// Steven Watanabe
//
// 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)

//[die
/*`
    For the source of this example see
    [@boost://libs/random/example/die.cpp die.cpp].
    First we include the headers we need for __mt19937
    and __uniform_int_distribution.
*/
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>

/*`
  We use __mt19937 with the default seed as a source of
  randomness.  The numbers produced will be the same
  every time the program is run.  One common method to
  change this is to seed with the current time (`std::time(0)`
  defined in ctime).
*/
boost::random::mt19937 gen;
/*`
  [note We are using a /global/ generator object here.  This
  is important because we don't want to create a new [prng
  pseudo-random number generator] at every call]
*/
/*`
  Now we can define a function that simulates an ordinary
  six-sided die.
*/
int roll_die() {
    /*<< __mt19937 produces integers in the range [0, 2[sup 32]-1].
        However, we want numbers in the range [1, 6].  The distribution
        __uniform_int_distribution performs this transformation.
        [warning Contrary to common C++ usage __uniform_int_distribution
        does not take a /half-open range/.  Instead it takes a /closed range/.
        Given the parameters 1 and 6, __uniform_int_distribution
        can produce any of the values 1, 2, 3, 4, 5, or 6.]
    >>*/
    boost::random::uniform_int_distribution<> dist(1, 6);
    /*<< A distribution is a function object.  We generate a random
        number by calling `dist` with the generator.
    >>*/
    return dist(gen);
}
//]

#include <iostream>

int main() {
    for(int i = 0; i < 10; ++i) {
        std::cout << roll_die() << std::endl;
    }
}