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

    @copyright 2016 NumScale SAS

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

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

  /*!

    @ingroup group-arithmetic
    This function object computes the integer conversion of the @ref floor of its parameter.

    @par Header <boost/simd/function/ifloor.hpp>

    @par Note:
    This operation is properly saturated

    @see floor, ifloor, iround, ifix, inearbyint

    @par Example:

      @snippet ifloor.cpp ifloor

    @par Possible output:

      @snippet ifloor.txt ifloor

  **/
  as_integer_t<Value> ifloor(Value const& x);
} }
#endif

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

#endif


#pragma once
#ifndef _RAY_SURFACE_
#define _RAY_SURFACE_

#include <Eigen/Core>
#include "surface.hpp"
#include "ray.hpp"

namespace raytracer
{

using namespace Eigen;

class Plane : public Surface
{
public:

  // @param position of the plane
  // @param normal of the plane
  Plane(Vector3f position, Vector3f normal, std::string material_name = "") :
    Surface(position, material_name),
    normal_(normal.normalized())
  {
  }

  ~Plane()
  { }

  bool Intersect(const Ray& ray, HitData& hit) override
  {
    float denom = normal_.dot(ray.direction());

    if (std::fabs(denom) > 1e-6)
    {
      // Time of intersection with the plane
      float planeHitTime = normal_.dot(position_ - ray.position()) / denom;
      if (planeHitTime >= 0.0f)
      {
        hit.hit_surface = dynamic_cast<Surface*>(this);
        hit.hit_point = ray.evaluate(hit.t);
        hit.t = planeHitTime;
        return true;
      }
    }

    return false;
  }

  Vector3f normal() const override
  {
    return normal_;
  }
private:
  Vector3f normal_;
};

}   // end of namespace raytracer

#endif // _RAY_SURFACE_


// An RLC bandpass filter using Boost.odeint for simulation and Eigen for circuit manipulation
// Author: Jeff Trull <edaskel@att.net>

/*
Copyright (c) 2014 Jeffrey E. Trull

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include <iostream>
#include <array>
#include <cmath>

#include <boost/numeric/odeint.hpp>
#include <boost/math/constants/constants.hpp>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

#include "mna.hpp"

typedef std::array<double, 2> state_t;   // 0 = V_out, 1 = I_L

struct circuit {
    circuit(double r, double l, double c) : circuit(r, l, c, 0, true) {}

    circuit(double r, double l, double c,
            double freq,        // cycles per second for sine wave
            bool   step=false   // step response instead
        ) : freq_(boost::math::constants::two_pi<double>() * freq),  // radian/s conversion
        step_(step) {

        using namespace Eigen;

        typedef Matrix<double, 5, 5> matrix5_t;
        matrix5_t G = matrix5_t::Zero();
        matrix5_t C = matrix5_t::Zero();

        // state assignment: 0, 1, 2 = node voltages; 3 = I_L, 4 = I_in
        stamp_r(G, C, 2, r);
        stamp_c(G, C, 1, 2, c);
        stamp_l(G, C, 0, 1, 3, l);     // n1, n2, I_L
        stamp_i(G, C, 0, 4);           // V_in, I_in

        // input application vector - connects single input to appropriate equation
        typedef Matrix<double, 5, 1> vector5_t;
        vector5_t B;
        B << 0, 0, 0, 0, -1;

        // output extraction vector - connects state element to output
        Matrix<double, 1, 5> D;
        D << 0, 0, 1, 0, 0;         // state element idx 2 = V_out

        Matrix<double, 1, 1> E = Matrix<double, 1, 1>::Zero();
        Matrix<double, 5, 1> DT = D.transpose();
        auto momentvec = moments(G, C, B, DT, E, 4);

        // Now we have C*dX/dt = -G*X + B*u, and the output is = D * X

        // we could not use Su regularization here because after LU factorization Cprime was
        // not square (UL elements were 2x3 because one row was -1 times another row)
        Matrix<double, Dynamic, Dynamic> Gnew, Cnew;
        Matrix<double, Dynamic, 1> Bnew, Dnew;
        Matrix<double, 1, 1> Enew;
        std::tie(Gnew, Cnew, Bnew, Dnew, Enew) = regularize(G, C, B, DT);

        Matrix<double, Dynamic, 1> DNT = Dnew.transpose();
        momentvec = moments(Gnew, Cnew, Bnew, DNT, E, 4);

        // verify the new C is non-singular
        assert(Cnew.rows() == Cnew.fullPivLu().rank());
        // and there is no feedthrough term
        assert(Enew.isZero());

        // factor Cnew out of our equation by multiplying both sides by Cnew^-1
        // new equation will be dX/dt = - Cnew^-1 * Gnew * X + Cnew^-1 * Bnew * u
        drift_term_ = - Cnew.fullPivLu().solve(Gnew);
        input_term_ =   Cnew.fullPivLu().solve(Bnew);

        // Vout may have been moved in the reduction process, so we must supply a map
        s2o_        = Dnew;

        // calculate poles of resulting system
        // they are the eigenvalues of the drift term (a.k.a. the "system matrix")
        auto evs = EigenSolver<MatrixXd>(drift_term_).eigenvalues();
        std::cerr << "poles are:\n" << std::endl;
        for (int row = 0; row < drift_term_.rows(); ++row) {
            // convert to standard frequency (cycles/s) and display
            std::cerr << evs(row, 0).real() / boost::math::constants::two_pi<double>() << std::endl;
        }

    }

    double state2output(state_t const& x) const {
        Eigen::Map<const Eigen::Matrix<double, 2, 1> > xvec(x.data());

        return (s2o_ * xvec)(0, 0);
    }

    void operator()(state_t const& x, state_t& dxdt, double t) {
        using namespace Eigen;

        Map<const Matrix<double, 2, 1> > xvec(x.data());
        Map<Matrix<double, 2, 1> > result(dxdt.data());

        Matrix<double, 1, 1> input;
        if (step_) {
            input << 1.0;
        } else {
            // calculate current value of sine wave input
            input << std::sin(freq_ * t);
        }

        result = drift_term_ * xvec + input_term_ * input;

    }

private:
    Eigen::MatrixXd drift_term_;            // connects current state to development over time
    Eigen::MatrixXd input_term_;            // connects input to development
    Eigen::Matrix<double, 1, Eigen::Dynamic> s2o_; // transforms state to output
    double freq_;                           // remembers frequency for calculating input
    bool   step_;                           // indicates using step function instead of sine wave
};

int main() {
    using namespace boost::numeric::odeint;
    const double freq = 50e3;
    circuit ckt(100.0, 20e-6, 20e-9, freq);
    state_t x{0.0, 0.0};                    // initial conditions

    integrate( ckt, x, 0.0, 10/freq, 0.1e-6,  // time range and increment
               [ckt,freq](state_t const& x, double t) {
                   double inp = std::sin(freq*boost::math::constants::two_pi<double>()*t);
                   std::cout << t << " " << inp << " " << ckt.state2output(x) << std::endl;
               });
}


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm accumulator sum
#include <boost/test/unit_test.hpp>
#include "fern/algorithm/algebra/elementary/add.h"
#include "fern/algorithm/accumulator/sum.h"

namespace fa = fern::algorithm;
namespace faa = fa::accumulator;

BOOST_AUTO_TEST_CASE(default_construct)
{
    faa::Sum<int> sum;
    BOOST_CHECK_EQUAL(sum(), 0);
}

BOOST_AUTO_TEST_CASE(accumulate)
{
    faa::Sum<int> sum(5);
    BOOST_CHECK_EQUAL(sum(), 5);

    sum(2);
    BOOST_CHECK_EQUAL(sum(), 7);

    sum = 3;
    BOOST_CHECK_EQUAL(sum(), 3);
}

BOOST_AUTO_TEST_CASE(out_of_range)
{
    faa::Sum<int> sum(std::numeric_limits<int>::max());
    BOOST_CHECK_EQUAL(sum(), std::numeric_limits<int>::max());

    BOOST_CHECK( sum.operator()<fa::add::OutOfRangePolicy>(0));
    BOOST_CHECK(!sum.operator()<fa::add::OutOfRangePolicy>(1));
}

BOOST_AUTO_TEST_CASE(merge)
{
    auto sum(faa::Sum<int>(5) | faa::Sum<int>(6));
    BOOST_CHECK_EQUAL(sum(), 11);
}


//==============================================================================
//         Copyright 2009 - 2013 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          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
//==============================================================================

#include <boost/simd/sdk/simd/pack.hpp>
#include <boost/simd/include/functions/aligned_load.hpp>
#include <boost/simd/include/functions/aligned_store.hpp>
#include <boost/simd/include/functions/multiplies.hpp>
#include <boost/simd/include/functions/plus.hpp>
#include <boost/simd/include/functions/minus.hpp>
#include <boost/simd/memory/allocator.hpp>
#include <boost/fusion/include/at.hpp>
#include <iostream>

#include <nt2/sdk/bench/benchmark.hpp>
#include <nt2/sdk/bench/metric/cycles_per_element.hpp>
#include <nt2/sdk/bench/protocol/max_duration.hpp>
#include <nt2/sdk/bench/setup/geometric.hpp>
#include <nt2/sdk/bench/setup/combination.hpp>
#include <nt2/sdk/bench/stats/median.hpp>

using namespace nt2::bench;
using namespace nt2;

template<typename K, typename L>
BOOST_FORCEINLINE void rgb2yuv_work(const K& r, const K& g, const K& b, L *y, L *u, L *v)
{
  using boost::simd::aligned_store;

  K tmp_y = K(0.299f)*r + K(0.587f)*g + K(0.114f)*b;
  aligned_store(tmp_y, y);
  aligned_store(K(0.492f)*(b - tmp_y), u);
  aligned_store(K(0.877f)*(r - tmp_y), v);
}

template<typename T> struct rgb2yuv_simd
{
  template<typename Setup>
  rgb2yuv_simd(Setup const& s)
                    :  height(boost::fusion::at_c<0>(s))
                    ,  width(boost::fusion::at_c<1>(s))
                    ,  size_(height*width)
  {
    y.resize(size_);
    u.resize(size_);
    v.resize(size_);
    r.resize(size_);
    g.resize(size_);
    b.resize(size_);
    for(std::size_t i=0; i<size_; i++)
      r[i] = g[i] = b[i] = y[i] = u[i] = v[i] = T(i);
  }

  void operator()()
  {
    using boost::simd::pack;
    using boost::simd::aligned_load;

    typedef pack<T> type;
    std::size_t step_size  = boost::simd::meta::cardinal_of<type>::value;
    std::size_t aligned_sz = size_ & ~(step_size-1);
    std::size_t it         = 0;

    for(std::size_t m=aligned_sz; it != m; it+=step_size)
    {
      type x1 = aligned_load< type >(&r[it]);
      type x2 = aligned_load< type >(&g[it]);
      type x3 = aligned_load< type >(&b[it]);

      rgb2yuv_work(x1, x2, x3, &y[it], &u[it], &v[it]);
    }

    for(std::size_t m=size_; it != m; it++)
    {
      rgb2yuv_work(r[it], g[it], b[it], &y[it], &u[it], &v[it]);
    }
  }

  friend std::ostream& operator<<(std::ostream& os, rgb2yuv_simd<T> const& p)
  {
    return os << "(" << p.size() << ")";
  }

  std::size_t size() const { return size_; }

private:
  std::size_t height;
  std::size_t width;
  std::size_t size_;
  std::vector<T,boost::simd::allocator<T> > r, g, b, y, u, v;
};

NT2_REGISTER_BENCHMARK_TPL( rgb2yuv_simd, (float) )
{
  std::size_t hmin = args("hmin", 32);
  std::size_t hmax = args("hmax", 128);
  std::size_t hstep = args("hstep", 2);
  std::size_t wmin = args("wmin", 32);
  std::size_t wmax = args("wmax", 128);
  std::size_t wstep = args("wstep", 2);

  run_during_with< rgb2yuv_simd<T> >( 1.
                                    , and_( geometric(hmin,hmax,hstep)
                                          , geometric(wmin,wmax,wstep)
                                          )
                                    , cycles_per_element<stats::median_>()
                                    );
}


#ifndef LK_SE3_H
#define LK_SE3_H

#include <dvs_msgs/Event.h>
#include <image_geometry/pinhole_camera_model.h>
#include <pcl/common/transforms.h>

#include <Eigen/StdVector>
#include <deque>
#include <opencv2/core/core.hpp>
#include <sophus/se3.hpp>

/**
 * Generic LK tracker based on SE3
 */
class LKSE3 {
    typedef Eigen::Matrix<float, 6, 6> Matrix6;
    typedef Eigen::Matrix<float, 8, 8> Matrix8;
    typedef Eigen::Matrix<float, 6, 1> Vector6;
    typedef Eigen::Matrix<float, 8, 1> Vector8;

   public:
    /**
     * Util structure to update the transformation, used to define the generic
     * jacobian and store the bearing vector and the intensity value of the
     * tracked frame
     */
    struct Keypoint {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        Eigen::Vector3f P;  ///< bearing vector
        float pixel_value;  ///< corresponding intensity value
        Vector8 J;          ///< Jacobian of the trasformation to fit
        Matrix6 JJt;        ///< J dot J^T

        Keypoint(Eigen::Vector3f _P, float _pixel_value, Vector8 _J,
                 Matrix6 _JJt)
            : P(_P), pixel_value(_pixel_value), J(_J), JJt(_JJt) {}
    };

    typedef pcl::PointXYZ Point;
    typedef pcl::PointCloud<Point> PointCloud;
    typedef std::deque<dvs_msgs::Event> EventQueue;

    typedef Sophus::SE3f SE3;

   protected:
    size_t batch_size_;      ///< number of keypoints processed together (batch)
    size_t max_iterations_;  ///< maximum number of iterations during the
                             ///< optimization step, @see updateTransformation,
                             ///< drawEvents, drawEventsNN
    size_t pyramid_levels_;  ///< number of levels in the image pyramid
                             ///< TODO: This could also be done relying on
                             ///< @see pyr_new_.size()
    size_t batches_;         ///< number of batches
                             ///< @see keypoints_, batches_size_

    float weight_scale_trans_;  ///< weight update due to translational residual
    float weight_scale_rot_;    ///< weight update due to rotational residual

    int map_blur_;        ///< Blur value of map projected onto the img_ref_
    float depth_median_;  ///< median depth in current scene (reference frame)

    std::vector<Keypoint, Eigen::aligned_allocator<Keypoint> >
        keypoints_;  ///< Keypoints in reference frame @see LKSE3::Keypoint
    int n_visible_;  ///< Number of keypoints of the reference frame visible in
                     ///< the current frame
    float kf_visibility_;  ///< Percentage of keypoints of the reference frame
                           ///< visible in the current frame
    PointCloud::Ptr map_;  ///< Map (built in the reference keyframe c_ref_)
    PointCloud::Ptr map_local_;  ///< Submap visible in the current frame

    cv::Mat depth_ref_;             ///< Depth map in the reference keyframe
    cv::Mat ref_img_;               ///< Image of the reference keyframe
    cv::Mat new_img_;               ///< Current image
    std::vector<cv::Mat> pyr_new_;  ///< Image pyramid of the new image

    image_geometry::PinholeCameraModel c_;      // camera at the current frame
    image_geometry::PinholeCameraModel c_ref_;  // camera at the reference frame
    float fx_;       ///< image horizontal focal length
    float fy_;       ///< image vertical focal length
    float cx_;       ///< image horizontal principal point
    float cy_;       ///< image vertical principal point
    size_t width_;   ///< image width
    size_t height_;  ///< image height
    cv::Rect rect_;  ///< image container, width x height

    // Poses
    Eigen::Affine3f
        T_world_kf_;  ///< Trasformation from keyframe to world frame
    Eigen::Affine3f
        T_kf_ref_;  ///< Trasformation from reference frame to keyframe
    Eigen::Affine3f
        T_ref_cam_;  ///< Trasformation from current frame to reference frame
    Eigen::Affine3f
        T_cur_ref_;  ///< Trasformation from reference frame to current frame

    Eigen::VectorXf x_ = Vector6::Zero();  ///< Twist vector <-> T_cur_ref

    /**
     * Creates a new reference frame
     *
     * Project map onto the current reference frame (img_ref_), build local map
     * (map_local_), evaluate quality of new reference frame (kf_visibility_,
     * n_visible_), compute median depth of the scene (depth_median_) and
     * precompute the data useful for the optimization in the tracking step.
     *
     * @see precomputeReferenceFrame
     */
    void projectMap();
    /**
     * Computes keypoints on the reference image ref_img_ to perform the KLT,
     * with the Jacobian evaluated at each point of the transformation to fit
     *
     * @see LKSE3::Keypoint
     */
    void precomputeReferenceFrame();

    /**
     * Draws events given in rectified image (when visible after rectification)
     * with bilinear interpolation
     *
     * @param ev_first iterator to first event in events container
     * @param ev_last iterator representing end of events container (not last
     * event!, e.g., vector.end())
     * @param out [output] image containing events drawed within
     *
     * @see rect_, drawEventsNN
     */
    void drawEvents(EventQueue::iterator ev_first, EventQueue::iterator ev_last,
                    cv::Mat &out);

    /**
     * Draws events given in rectified image (when visible after rectification)
     *
     * @param ev_first iterator to first event in events container
     * @param ev_last iterator representing end of events container (not last
     * event!, e.g., vector.end())
     * @param out [output] image containing events drawed within
     *
     * @see rect_, drawEvents
     */
    void drawEventsNN(EventQueue::iterator ev_first,
                      EventQueue::iterator ev_last, cv::Mat &out);

    /**
     * Updates transformation: one pass of stochastic gradient descent
     *
     * Remark: keypoints_ are already shuffled in precomputeReferenceFrame
     *
     * @param offset first keypoint to consider
     * @param N number of keypoints to consider
     * @param pyr_lvl level of the image pyramid
     */
    void updateTransformation(const int offset, const int N, size_t pyr_lvl);

    /**
     * Tracks frame updating the transformation at each pyramid_levels_ and for
     * a maximum of max_iterations_
     *
     * @see updateTransformation
     */
    void trackFrame();
};

#endif  // LK_SE3_H


#include <iostream>
#include <cassert>
#include <numeric>
#include <NTL/ZZ.h>
#include <NTL/mat_ZZ.h>

#define DEBUGGING

#include "tests.hpp" // define strings LWEVSS_TESTS::passed and LWEVSS_TESTS::failed
#include "utils.hpp"
#include "pedersen.hpp"
#include "regevProofs.hpp"

using namespace std;
using namespace ALGEBRA;
using namespace REGEVENC;

bool test_commit() {
    // Initialize a list of 5*ell generators
    PedersenContext ped("blah");
    std::vector<Point> gens(scalarsPerElement()*5);
    for (int i=0; i<gens.size(); i++)
        gens[i] = ped.getG(i);

    SVector svec;
    resize(svec,4);
    for (int i=0; i<4; i++) conv(svec[i], 2); // vector is (2,2,2,2)
    CRV25519::Scalar r;                  // to hold randomness
    auto com = commit(svec, 5, gens, r); // commitment wrt generators 5,6,7,8

    // com should be r*base + 2(g[5]+g[6]+g[7]+g[8])
    Point b = gens[5] +gens[6] +gens[7] +gens[8];
    if (com != Point::base()*r + b+b)
        return false;

    EVector evec;
    resize(evec, 3);
    for (int i=0; i<evec.length(); i++)
        randomizeElement(evec[i]);

    // A vector of CRV25519::Scalar's with the representation of evec
    std::vector<CRV25519::Scalar> evecRep(evec.length()*scalarsPerElement());
    int idx = 0;
    for (int i=0; i<evec.length(); i++) for (int j=0; j<scalarsPerElement(); j++)
        conv(evecRep[idx++], coeff(evec[i], j));

    // commitment for the representation of evec[1,2,...]
    // wrt generators 3,4,5,...,3*ell-1
    com = commit(evec, 3, gens, r, /*startIdx=*/1);

    return com == DLPROOFS::commit(&gens[3], &evecRep[scalarsPerElement()],
                                    evecRep.size()-scalarsPerElement(), r);
}

bool test_norms() {
    int iVec[] = {1, 2, -1, 0, 3, 2, -2, 0}; // norm^2 = 23
    BIVector biVec;
    resize(biVec, 8);
    for (int i=0; i<biVec.length(); i++)
        conv(biVec[i], iVec[i]);

    if (normSquaredBI(biVec) != toBigInt(23))
        return false;

    SVector sVec;
    conv(sVec, biVec);
    if (normSquaredBigInt(sVec) != toBigInt(23))
        return false;

    EVector eVec;
    resize(eVec, 2);
    int val = -4;
    int norm2 = 0;
    SVector tmp;
    resize(tmp, scalarsPerElement());
    for (auto& x: eVec) { // initialize one element in the vector
        for (int j=0; j<scalarsPerElement(); j++) {
            norm2 += val*val;
            conv(tmp[j], val++);
        }
        ALGEBRA::conv(x, tmp);
    }
    if (normSquaredBigInt(eVec) != toBigInt(norm2))
        return false;

    // Add to v four integers a,b,c,d such that the result
    // (v | a,b,c,d) has norm exactly equal to the bound

    BigInt bound = toBigInt(20);
    SVector sPad; resize(sPad, PAD_SIZE);
    pad2exactNorm(sVec, sPad, bound);
    if (normSquaredBigInt(sVec)+normSquaredBigInt(sPad) != bound*bound) {
        return false;
    }

    EVector ePad; resize(ePad, PAD_SIZE/scalarsPerElement());
    pad2exactNorm(eVec, ePad, bound);
    if (normSquaredBigInt(eVec)+normSquaredBigInt(ePad) != bound*bound) {
        return false;
    }
    return true;
}

bool test_constraints() {
    // choose secret variables, one vector over Z_p and one over GF(p^e)
    SVector sVec; resize(sVec,3);
    for (int i=0; i<sVec.length(); i++) randomizeScalar(sVec[i]);

    EVector eVec; resize(eVec,3);
    for (int i=0; i<eVec.length(); i++) randomizeElement(eVec[i]);

    // Choose a few random public coefficients
    EVector coeffs; resize(coeffs, 5);
    for (int i=0; i<coeffs.length(); i++) randomizeElement(coeffs[i]);

    // generate some constraints from the above and check them
    std::vector<LinConstraint> constr(3*scalarsPerElement());

    // first set of constraints: coeffs[0]*eVec[0]
    LinConstraint *cPtr = constr.data();
    DLPROOFS::PtxtVec w1;
    for (int j=0; j<scalarsPerElement(); j++)
        conv(w1[sVec.length() +j], coeff(eVec[0],j));
    expandConstraints(cPtr, sVec.length(), coeffs[0]);

    Element b = coeffs[0]*eVec[0];
    for (int j=0; j<scalarsPerElement(); j++) {
        ALGEBRA::conv(cPtr[j].equalsTo, coeff(b,j));
        if (!checkConstraint(cPtr[j], w1))
            return false;
    }

    // second set of constraints: \sum_{i=0}^2 coeffs[i+1]*eVec[i]
    cPtr += scalarsPerElement();
    DLPROOFS::PtxtVec w2;
    for (int i=0; i<eVec.length(); i++)
        for (int j=0; j<scalarsPerElement(); j++)
            conv(w2[sVec.length()+i*scalarsPerElement()+j], coeff(eVec[i],j));
    expandConstraints(cPtr, sVec.length(), coeffs, 1, 4);
    b = eVec[0]*coeffs[1] + eVec[1]*coeffs[2] + eVec[2]*coeffs[3];
    for (int j=0; j<scalarsPerElement(); j++) {
        ALGEBRA::conv(cPtr[j].equalsTo, coeff(b,j));
        if (!checkConstraint(cPtr[j], w2))
            return false;
    }

    // third set of constraints: \sum_{i=0}^2 coeffs[i+2]*sVec[i]
    cPtr += scalarsPerElement();
    DLPROOFS::PtxtVec w3;
    for (int i=0; i<sVec.length(); i++)
        conv(w3[i], sVec[i]);
    makeConstraints(cPtr, 0, coeffs, 2, 5);
    b = sVec[0]*coeffs[2] + sVec[1]*coeffs[3] + sVec[2]*coeffs[4];
    for (int j=0; j<scalarsPerElement(); j++) {
        ALGEBRA::conv(cPtr[j].equalsTo, coeff(b,j));
        if (!checkConstraint(cPtr[j], w3))
            return false;
    }

    // Make a single vector with all the secret scalars, as CRV25519::Scalas
    int idx = 0;
    DLPROOFS::PtxtVec w; // map i -> a_i
    for (int i=0; i<sVec.length(); i++) {
        CRV25519::Scalar& s = w[idx++]; // insert index idx to w
        conv(s, sVec[i]);         // convert to CRV25519::Scalar
    }
    for (int i=0; i<eVec.length(); i++)
        for (int j=0; j<scalarsPerElement(); j++) {
            CRV25519::Scalar& s = w[idx++];   // insert index idx to w
            conv(s, coeff(eVec[i], j)); // convert to CRV25519::Scalar
        }

    // merge the constraints from above and check again
    std::vector<CRV25519::Scalar> rnd(constr.size());
    for (auto& s: rnd) s.randomize();
    LinConstraint c;
    c.merge(constr, rnd);
    if (!checkConstraint(c, w))
        return false;

    return true;
}

bool test_proofs() {
    // The dimensions of the the CRX is k-by-m, but note that this is a
    // matrix over GF(p^2) so the lattice dimensions we get is twice that
    KeyParams kp(64);
#ifdef DEBUGGING
    std::cout << "n="<<kp.n<<", k="<<kp.k<<"->64\n";
#endif
    kp.k=128;// make smaller dimension for debugging

    GlobalKey gpk("testContext", kp);
    TernaryEMatrix::init();
    MerlinRegev mer;
    PedersenContext ped;
    SharingParams ssp(interval(1,gpk.enn+1), gpk.tee);
    VerifierData vd(gpk, ped, mer, ssp);
    ProverData pd(vd);

    // Generate/verify the proofs by the second party (idx=1)
    int partyIdx = 1;

    // Key generation for all the parties
    std::vector<ALGEBRA::EVector> kgNoise(gpk.enn);
    std::vector<ALGEBRA::EVector> sk(gpk.enn);
    std::vector<ALGEBRA::EVector> pk(gpk.enn);
    for (int i=0; i<gpk.enn; i++) {
        std::tie(sk[i],pk[i]) = gpk.genKeys(&kgNoise[i]);
        gpk.addPK(pk[i]);
    }
    gpk.setKeyHash();
#ifdef DEBUGGING
    std::cout <<"|kgNoise|^2=2^"<<log2BI(normSquaredBigInt(kgNoise[partyIdx]))<<std::endl;
#endif

    // encryption
    std::vector<ALGEBRA::SVector> ptxt1(gpk.enn);
    std::vector<GlobalKey::CtxtPair> ctxt1(gpk.enn);
    // secret sharing of a random value , the secret itself is sshr[0]
    ALGEBRA::SVector sshr;
    ssp.randomSharing(sshr);
    for (int i=0; i<gpk.enn; i++) {
        resize(ptxt1[i], gpk.enn);
        for (int j=0; j<gpk.enn; j++) ptxt1[i][j] = sshr[i+1];
        ctxt1[i] = gpk.encrypt(ptxt1[i]);
    }

    // decryption at party #1
    ALGEBRA::SVector ptxt2;    resize(ptxt2, gpk.tee);
    ALGEBRA::EVector decNoise; resize(decNoise, gpk.tee);
    for (int i=0; i<gpk.tee; i++) { // decrypt 2nd entry in i'th ctxt
        ptxt2[i] = gpk.decrypt(sk[partyIdx], partyIdx, ctxt1[i], &(decNoise[i]));
        if (ptxt2[i] != ptxt1[i][partyIdx])
            return false;
    }
#ifdef DEBUGGING
    std::cout <<"|decNoise|^2=2^"<<log2BI(normSquaredBigInt(decNoise))
        << ", B_decNoise=2^"<<log2BI(vd.B_dNoise) <<std::endl;
#endif

    // re-encryption at party #1
    ALGEBRA::SVector ptxt3;
    resize(ptxt3, gpk.enn);
    for (int j=0; j<gpk.enn; j++) ptxt3[j] = sshr[j+1];
    ALGEBRA::EVector encRnd;
    REGEVENC::GlobalKey::CtxtPair eNoise;
    auto ctxt2 = gpk.encrypt(ptxt3, encRnd, eNoise);
#ifdef DEBUGGING
    std::cout <<"|encNoise1|^2=2^"<<log2BI(normSquaredBigInt(eNoise.first))
    <<", |encNoise2|^2=2^"<<log2BI(normSquaredBigInt(eNoise.second))
    <<std::endl;
#endif

    // Copy the first t ciphertexts into a k x t matrix and another t-vector
    EMatrix ctxtMat;
    resize(ctxtMat, gpk.kay, gpk.tee);
    EVector ctxtVec;
    resize(ctxtVec, gpk.tee);
    for (int i=0; i<gpk.tee; i++) {
        for (int j=0; j<gpk.kay; j++)
            ctxtMat[j][i] = ctxt1[i].first[j];
        ctxtVec[i] = ctxt1[i].second[partyIdx];
    }

    // prepare for proof, commit to the secret key
    int origSize = sk[partyIdx].length();

    vd.sk1Com = commit(sk[partyIdx], vd.sk1Idx, vd.Gs, pd.sk1Rnd);

    SVector lagrange = vd.sp->lagrangeCoeffs(interval(1,gpk.tee+1));

    proveDecryption(pd, ctxtMat, ctxtVec, ptxt2, sk[partyIdx], decNoise);
    proveEncryption(pd, ctxt2.first, ctxt2.second, ptxt3, encRnd, eNoise.first, eNoise.second);
    proveKeyGen(pd, partyIdx, sk[partyIdx], kgNoise[partyIdx]);
    proveReShare(pd, lagrange, ptxt2, ptxt3);
    proveSmallness(pd);

    // Verify the commitments and constraints

    for (auto& lncstr: vd.linConstr) {
        if (!checkConstraintLoose(lncstr, pd.linWitness))
            return false;
    }
    for (auto& qdcstr: vd.normConstr) {
        if (!checkConstraintLoose(qdcstr, pd.quadWitnessG, pd.quadWitnessG))
            return false;
    }

    if (!checkQuadCommit(vd.normConstr[vd.dHiQuadIdx], vd.dCompHiCom,
        vd.dPadHiCom,  pd.dCompHiRnd, pd.dPadHiRnd,  pd.quadWitnessG, vd.ped))
        return false;
    if (!checkQuadCommit(vd.normConstr[vd.dLoQuadIdx], vd.dCompLoCom,
        vd.dPadLoCom,  pd.dCompLoRnd, pd.dPadLoRnd,  pd.quadWitnessG, vd.ped))
        return false;
    if (!checkQuadCommit(vd.normConstr[vd.rQuadIdx],   vd.rCompCom,
        vd.rPadCom,    pd.rCompRnd,   pd.rPadRnd,    pd.quadWitnessG, vd.ped))
        return false;
    if (!checkQuadCommit(vd.normConstr[vd.e1HiQuadIdx],vd.eComp1HiCom,
        vd.ePad1HiCom, pd.eComp1HiRnd,pd.ePad1HiRnd, pd.quadWitnessG, vd.ped))
        return false;
    if (!checkQuadCommit(vd.normConstr[vd.e1LoQuadIdx],vd.eComp1LoCom,
        vd.ePad1LoCom, pd.eComp1LoRnd,pd.ePad1LoRnd, pd.quadWitnessG, vd.ped))
        return false;
    if (!checkQuadCommit(vd.normConstr[vd.e2HiQuadIdx],vd.eComp2HiCom,
        vd.ePad2HiCom, pd.eComp2HiRnd,pd.ePad2HiRnd, pd.quadWitnessG, vd.ped))
        return false;
    if (!checkQuadCommit(vd.normConstr[vd.e2LoQuadIdx],vd.eComp2LoCom,
        vd.ePad2LoCom, pd.eComp2LoRnd,pd.ePad2LoRnd, pd.quadWitnessG, vd.ped))
        return false;
    if (!checkQuadCommit(vd.normConstr[vd.sk2QuadIdx], vd.sk2CompCom,
        vd.sk2PadCom,  pd.sk2CompRnd, pd.sk2PadRnd,  pd.quadWitnessG, vd.ped))
        return false;
    if (!checkQuadCommit(vd.normConstr[vd.kHiQuadIdx],   vd.kCompHiCom,
        vd.kPadHiCom,    pd.kCompHiRnd,   pd.kPadHiRnd,    pd.quadWitnessG, vd.ped))
        return false;
    if (!checkQuadCommit(vd.normConstr[vd.kLoQuadIdx],   vd.kCompLoCom,
        vd.kPadLoCom,    pd.kCompLoRnd,   pd.kPadLoRnd,    pd.quadWitnessG, vd.ped))
        return false;

    // commitments for re-sharing linear constraint
    if (!checkLinCommit(vd.linConstr[vd.reShrLinIdx].terms,
        vd.pt1Com +vd.pt2Com, pd.pt1Rnd +pd.pt2Rnd,
        pd.linWitness, vd.ped))
        { return false; }

    // aggregate the constraints and flatten everything before proving
    ReadyToProve rtp;
    rtp.aggregateProver(pd);

    // Make copies of the Merlin transcripts and specialize them
    // for the final constraints before proving/verifying them
    auto merLin = *vd.mer;
    merLin.processConstraint("linear", rtp.linCnstr);

    auto merQuad = *vd.mer;
    merQuad.processConstraint("quadratic", rtp.quadCnstr);

    // Flatten the statements, this relases the memory of the constraints
    // (hence the Merlin processing above must be done before doing this).
    rtp.flattenLinPrv(pd);
    rtp.flattenQuadPrv(pd);

    ReadyToVerify rtv = rtp; // a copy without the secret variables

    // prove and verify the linear statement
    auto merLinVer = merLin; // another copy for verification
    DLPROOFS::LinPfTranscript pfL("Linear");
    pfL.C = rtp.linCom;

    // The actual proof
    DLPROOFS::proveLinear(pfL, rtp.lComRnd, merLin, rtp.linWtns.data(),
            rtp.linStmnt.data(), rtp.linGs.data(), rtp.linGs.size());
    // The actual verification
    if (!DLPROOFS::verifyLinear(pfL, rtv.linStmnt.data(), rtv.linGs.data(),
                      rtv.linGs.size(), rtv.linCnstr.equalsTo, merLinVer))
        return false;

    // prove and verify the quadratic statement
    auto merQuadVer = merQuad; // another copy for verification
    DLPROOFS::QuadPfTranscript pfQ("Quadratic");
    pfQ.C = rtp.quadCom;
     // The actual proof
    DLPROOFS::proveQuadratic(pfQ, rtp.qComRnd, merQuad, rtp.quadGs.data(),
                rtp.quadWtnsG.data(), rtp.quadHs.data(), rtp.quadWtnsH.data(),
                rtp.quadGs.size());
    // The actual verification
    if (!DLPROOFS::verifyQuadratic(pfQ, rtv.quadGs.data(), rtv.quadHs.data(),
                        rtp.quadGs.size(), rtv.quadCnstr.equalsTo, merQuadVer,
                        rtv.offstG.data(), rtv.offstH.data()))
        return false;

    return true;
}

int main(int, char**) {
    assert(scalarsPerElement()==REGEVENC::GlobalKey::ell); // ensuring the init function was called
    if (!test_commit() || !test_norms() || !test_constraints() || !test_proofs())
        std::cout << LWEVSS_TESTS::failed << std::endl;
    else
        std::cout << LWEVSS_TESTS::passed << std::endl;
    return 0;
}


#include <boost/python.hpp>
#include <numpy/arrayobject.h>

#include <cassert>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <numpy_eigen.h>
#include <vector>
#include <iostream>

PyObject* ewald_sum(const int &NLa, const double &CLa, const int &NSr, const double &CSr, PyObject *nV, const int &Nxy, const int &Nz)
{
    assert(((PyArrayObject *)nV)->dimensions[0] == NLa + NSr);
    import_array1(NULL);
    double sigma = M_PI;
    int N = NLa + NSr;
    double *C = new double[2*N],
           *pt_nV = (double *) PyArray_DATA((PyArrayObject *) nV);
    double *x = new double[2*N],
           *y = new double[2*N],
           *z = new double[2*N],
           *Vsi = new double[N];
    std::complex<double>  *Vli = new std::complex<double>[N];
    double sum_nV = 0.;
    for (int i = 0; i < N; ++i)
        sum_nV += pt_nV[i];

    for (int i = 0; i < N; ++i) {
        C[i] = pt_nV[i]/sum_nV * (-NLa*CLa - NSr*CSr);
        C[N+i] = (i < NLa) ? CLa : CSr;
        x[i] = y[i] = 0.;
        x[N+i] = y[N+i] = 0.5;
        z[i] = double(i);
        z[N+i] = -0.5+double(i);
        Vsi[i] = 0.;
        Vli[i] = std::complex<double>(0., 0.);
    }

    double e_charge = -1.;
    int Nx = Nxy, Ny = Nxy;
    double *R = new double[2*N],
           kv[3], ksquare = 0.;

    for (int i = 0; i < N; ++i)
        for (int m = -Nx; m < Nx; ++m)
            for (int n = -Ny; n < Ny; ++n)
                for (int l = - Nz; l < Nz; ++l) {
                    for (int k = 0; k < 2*N; ++k)
                        R[k] = sqrt(pow(m+x[k], 2) + pow(n+y[k],2) + pow(N*l+z[i]-z[k], 2));
                    // for short-range terms
                    if (m == 0 and n == 0 and l == 0) {
                        for (int k = 0; k < 2*N; ++k)
                            if (k != i)
                                Vsi[i] += e_charge*C[k] / R[k] * erfc(R[k]/sqrt(2)/sigma);
                        continue;
                    }
                    for (int k = 0; k < 2*N; ++k)
                        Vsi[i] += e_charge * C[k] / R[k] * erfc(R[k]/sqrt(2)/sigma);

                    // for long-range terms
                    kv[0] = 2*M_PI*n;
                    kv[1] = 2*M_PI*m;
                    kv[2] = 2*M_PI/N*l;
                    ksquare = pow(kv[0], 2) + pow(kv[1], 2) + pow(kv[2], 2);
                    for (int k = 0; k < 2*N; ++k)
                        Vli[i] += 4*M_PI/N * e_charge*C[k] / ksquare *
                            exp(std::complex<double>(0., -kv[0]*x[k] - kv[1]*y[k] + kv[2]*(z[i] - z[k]))) * exp(-pow(sigma,2)*ksquare/2.);
                }
    npy_intp Nconvert = npy_intp(N);
    PyArrayObject *result = (PyArrayObject *)PyArray_SimpleNew(1, &Nconvert, NPY_DOUBLE);
    Py_INCREF(result);
    double *result_buf = (double *)PyArray_DATA(result);
    for (int i = 0; i < N; ++i)
        result_buf[i] = Vsi[i] + Vli[i].real() - e_charge*C[i]*sqrt(2/M_PI)*1./sigma;

    delete[] R;
    delete[] C;
    delete[] x;
    delete[] y;
    delete[] z;
    delete[] Vsi;
    delete[] Vli;

    return PyArray_Return(result);
}

using namespace Eigen;

typedef struct {
    Vector3d r; // position in the unitcell
    double C;   // ion charge
    double ne;  // electron occupation
    double V;   // dimensionless electric potential (whose unit is smt like V :)
} AtomInfo;
typedef std::vector<AtomInfo> UnitCell;

void general_ewald_sum(UnitCell &uc, const Vector3d &a1, const Vector3d &a2, const Vector3d &a3, const Vector3i &Rcutoff);

PyObject* new_ewald_sum(const int &NLa, const double &CLa, const int &NSr, const double &CSr, PyObject *nVpy, const int &Nxy, const int &Nz)
{
    try {
        int N = NLa + NSr;
        VectorXd nV;
        numpy::from_numpy(nVpy, nV);

        UnitCell uc(2*N);
        for (int i = 0; i < N; ++i) {
            uc[2*i].C = (i < NLa) ? CLa : CSr;
            uc[2*i].ne = 0;
            uc[2*i].r << 0.5, 0.5, -0.5 + double(i);

            uc[2*i+1].C = 0;
            uc[2*i+1].ne = nV(i);
            uc[2*i+1].r << 0, 0, i;
        }

        Vector3d a1(1,0,0), a2(0,1,0), a3(0,0,N);
        Vector3i Rcutoff(Nxy, Nxy, Nz);
        general_ewald_sum(uc, a1, a2, a3, Rcutoff);

        VectorXd out(N);
        for (int i = 0; i < N; ++i)
            out(i) = -uc[2*i+1].V;
        return numpy::to_numpy(out);
    } catch (const char *str) {
        std::cerr << str << std::endl;
        return Py_None;
    }
}

void general_ewald_sum(UnitCell &uc, const Vector3d &a1, const Vector3d &a2, const Vector3d &a3, const Vector3i &Rcutoff)
{
    int N = uc.size();
    const double sigma = M_PI;

    // renormalize electron occupation
    double unrenorm_Ne = 0, Ne = 0;
    for (UnitCell::iterator it = uc.begin(); it < uc.end(); ++it) {
        unrenorm_Ne += it->ne;
        Ne += it->C;
    }
    for (UnitCell::iterator it = uc.begin(); it < uc.end(); ++it)
        it->ne *= Ne/unrenorm_Ne;

    // get reciprocal vectors
    Vector3d b1, b2, b3;
    double uc_vol = a1.dot(a2.cross(a3));
    b1 = 2*M_PI*a2.cross(a3)/uc_vol;
    b2 = 2*M_PI*a3.cross(a1)/uc_vol;
    b3 = 2*M_PI*a1.cross(a2)/uc_vol;

    // Ewald sum
    double Vs, Vl;
    Vector3d k;
    for (int id = 0; id < N; ++id) {
        Vs = 0; Vl = 0;
        for (int m = -Rcutoff[0]; m < Rcutoff[0]; ++m)
            for (int n = -Rcutoff[1]; n < Rcutoff[1]; ++n)
                for (int l = -Rcutoff[2]; l < Rcutoff[2]; ++l) {
                    k = n*b1 + m*b2 + l*b3;
                    double ksquare = k.dot(k);
                    for (int i = 0; i < N; ++i) {
                        double dR = (uc[id].r - (uc[i].r + m*a1 + n*a2 + l*a3)).norm();
                        // for long-range terms
                        if ( !(m == 0 && n == 0 && l == 0) )
                            Vl += 4*M_PI/uc_vol*(uc[i].C-uc[i].ne)/ksquare * cos(k.dot(uc[id].r-uc[i].r)) * exp(-pow(sigma,2)*ksquare/2.);

                        // for short-range terms
                        if ( !(m == 0 && n == 0 && l == 0 && i == id) )
                            Vs += (uc[i].C - uc[i].ne) / dR * erfc(dR/sqrt(2)/sigma);
                    }
                }
        uc[id].V = Vs + Vl - (uc[id].C - uc[id].ne)*sqrt(2/M_PI)/sigma;
    }
}


#ifndef STATIC_LINK_VW
#define BOOST_TEST_DYN_LINK
#endif

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

#include "test_common.h"

#include <vector>
#include "../../explore/explore.h"

#include "cb_explore_pdf.h"

using namespace VW::continuous_actions;
using std::vector;

struct pdf_seg
{
  float left;
  float right;
  float pdf_value;
};

bool continuous_action_range_check(std::vector<pdf_seg> scores, float range_min, float range_max)
{
  float chosen_value;
  float pdf_value;
  uint64_t seed = 7791;
  auto scode = exploration::sample_pdf(&seed, begin(scores), end(scores), chosen_value, pdf_value);
  BOOST_CHECK_EQUAL(scode, S_EXPLORATION_OK);
  return ((range_min <= chosen_value) && (chosen_value <= range_max));
}

BOOST_AUTO_TEST_CASE(sample_continuous_action)
{
  BOOST_CHECK(continuous_action_range_check(
      {{0.f, 20.f, 1.0f}, {20.f, 40.f, 2.0f}, {40.f, 50.f, 3.0f}, {50.f, 70.f, 4.0f}, {70.f, 100.f, 5.0f}}, .0f,
      100.f));

  BOOST_CHECK(continuous_action_range_check({{1000.f, 1020.f, 1.0f}, {1020.f, 1040.f, 2.0f}, {1040.f, 1050.f, 3.0f},
                                                {1050.f, 1070.f, 4.0f}, {1070.f, 1100.f, 5.0f}},
      1000.0f, 1100.f));
}

BOOST_AUTO_TEST_CASE(new_sample_pdf)
{
  probability_density_function the_pdf;
  the_pdf.push_back({2.f, 3.5f, 0.1f / 1.5f});
  the_pdf.push_back({3.5f, 4.5f, 0.8f / 1.0f});
  the_pdf.push_back({4.5f, 6.2f, 0.1f / 1.7f});
  const auto str_pdf = to_string(the_pdf);
  std::cout << str_pdf << std::endl;
  uint64_t seed = 7791;
  float chosen_action = 0.f;
  float pdf_value = 0.f;

  exploration::sample_pdf(&seed, std::begin(the_pdf), std::end(the_pdf), chosen_action, pdf_value);
  BOOST_CHECK(chosen_action >= the_pdf[0].left && chosen_action <= the_pdf.back().right && pdf_value > 0.f);
  exploration::sample_pdf(&seed, std::begin(the_pdf), std::end(the_pdf), chosen_action, pdf_value);
  BOOST_CHECK(chosen_action >= the_pdf[0].left && chosen_action <= the_pdf.back().right && pdf_value > 0.f);
}

struct bins_calc
{
  bins_calc(vector<float>&& bins) : _bins(bins), _counts(_bins.size() - 1), _total_samples(0) {}

  void add_to_bin(float val)
  {
    ++_total_samples;
    float left = _bins[0];
    for (int i = 1; i < _bins.size(); i++)
    {
      float right = _bins[i];
      if (left <= val && val < right)
      {
        ++_counts[i - 1];
        return;
      }
    }
    std::stringstream err_strm;
    err_strm << "Value " << val << " does not fall in a bin";
    throw std::logic_error(err_strm.str());
  }

  vector<float> _bins{};
  vector<uint32_t> _counts{};
  uint32_t _total_samples;
};

BOOST_AUTO_TEST_CASE(sample_continuous_action_statistical)
{
  std::vector<pdf_seg> scores = {
      {0.f, 10.f, .005f}, {10.f, 20.f, .01f}, {20.f, 30.f, .0125f}, {30.f, 40.f, .0075f}, {40.f, 100.f, .015f}};

  float chosen_value;
  float pdf_value;
  constexpr float range_min = .0f;
  constexpr float range_max = 100.0f;
  uint64_t random_seed = 7791;
  bins_calc bins({0.f, 10.f, 20.f, 30.f, 40.f, 100.f});

  constexpr uint32_t iterate_count = 100000;
  for (auto idx = 0; idx < iterate_count; idx++)
  {
    const auto scode = exploration::sample_pdf(&random_seed, begin(scores), end(scores), chosen_value, pdf_value);
    BOOST_CHECK_EQUAL(scode, S_EXPLORATION_OK);
    BOOST_CHECK((range_min <= chosen_value) && (chosen_value <= range_max));

    // keep track of where the chosen actions fall
    bins.add_to_bin(chosen_value);
  }

  const float total_mass = std::accumulate(std::begin(scores), std::end(scores), 0.f,
      [](const float& acc, const pdf_seg& rhs) { return acc + (rhs.pdf_value * (rhs.right - rhs.left)); });

  for (uint32_t idx = 0; idx < bins._counts.size(); ++idx)
  {
    BOOST_CHECK_CLOSE(bins._counts[idx] / (float)bins._total_samples,
        (scores[idx].pdf_value * (scores[idx].right - scores[idx].left)) / total_mass, 1.5f);
  }
}

BOOST_AUTO_TEST_CASE(sample_after_nomalizing_basic)
{
  std::vector<float> pdf = {1.0f, 2.0f, 3.0f, 4.0f, 5.0f};
  const std::vector<float> expected = {0.066666667f, 0.133333333f, 0.2f, 0.266666667f, 0.333333333f};
  uint32_t chosen_index;

  auto scode = exploration::sample_after_normalizing(7791, begin(pdf), end(pdf), chosen_index);
  BOOST_CHECK_EQUAL(scode, S_EXPLORATION_OK);
  check_collections_with_float_tolerance(pdf, expected, .0001f);

  scode = exploration::sample_after_normalizing(7791, begin(pdf), end(pdf), chosen_index);
  BOOST_CHECK_EQUAL(scode, S_EXPLORATION_OK);
  check_collections_with_float_tolerance(pdf, expected, .0001f);

  scode = exploration::sample_after_normalizing(7791, begin(pdf), end(pdf), chosen_index);
  BOOST_CHECK_EQUAL(scode, S_EXPLORATION_OK);
  check_collections_with_float_tolerance(pdf, expected, .0001f);

  scode = exploration::sample_after_normalizing(7791, begin(pdf), end(pdf), chosen_index);
  BOOST_CHECK_EQUAL(scode, S_EXPLORATION_OK);
  check_collections_with_float_tolerance(pdf, expected, .0001f);

  scode = exploration::sample_after_normalizing(7791, begin(pdf), end(pdf), chosen_index);
  BOOST_CHECK_EQUAL(scode, S_EXPLORATION_OK);
  check_collections_with_float_tolerance(pdf, expected, .0001f);
}

BOOST_AUTO_TEST_CASE(sample_after_nomalizing_degenerate)
{
  std::vector<float> pdf = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
  const std::vector<float> expected = {1.0f, 0.0f, 0.0f, 0.0f, 0.0f};
  uint32_t chosen_index;

  const auto scode = exploration::sample_after_normalizing(7791, begin(pdf), end(pdf), chosen_index);
  BOOST_CHECK_EQUAL(scode, S_EXPLORATION_OK);
  check_collections_with_float_tolerance(pdf, expected, .0001f);
}

BOOST_AUTO_TEST_CASE(swap_test)
{
  std::vector<float> pdf = {1.0f, 2.0f, 3.0f, 4.0f, 5.0f};
  const std::vector<float> expected_pdf = {0.066666667f, 0.133333333f, 0.2f, 0.266666667f, 0.333333333f};
  uint32_t chosen_index;

  auto scode = exploration::sample_after_normalizing(7791, begin(pdf), end(pdf), chosen_index);
  BOOST_CHECK_EQUAL(scode, S_EXPLORATION_OK);
  BOOST_CHECK_EQUAL(chosen_index, 3);
  check_collections_with_float_tolerance(pdf, expected_pdf, .0001f);

  scode = exploration::swap_chosen(begin(pdf), end(pdf), chosen_index);

  const std::vector<float> expected_pdf_2 = { 0.266666667f,	0.133333333f,	0.2f,	0.066666667f,	0.333333333f };
  check_collections_with_float_tolerance(pdf, expected_pdf_2, .0001f);
}


/**
 * @file crc16_test.cc
 *
 * Test the C implementation of CRC-16 against the boost library.
 */

#include "crc16.h"

#include <boost/crc.hpp>      // for boost::crc_basic, boost::crc_optimal
#include <boost/cstdint.hpp>  // for boost::uint16_t

#include <algorithm>          // for std::for_each
#include <cassert>            // for assert
#include <cstddef>            // for std::size_t
#include <iostream>           // for std::cout
#include <ostream>            // for std::endl

// All zeroes on the polynomial is uninteresting.
// Use that case as the default.
static uint16_t crc16_poly = 0x0000;

// This is "123456789" in ASCII
// For the polynomial 0x1021 the expected result is 0x29b1.
uint8_t const  test_data[] = { 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39 };
std::size_t const   test_data_length = sizeof(test_data) / sizeof(test_data[0]);

static void usage(char const *program_name)
{
    fprintf(stderr, "usage: %s polynomial\n", program_name);
    fprintf(stderr, "\texample: for CRC-16-CCITT use 0x1021\n");
    fprintf(stderr, "\t%s 0x1021\n", program_name);
    fprintf(stderr, "\n");
}

int main(int argc, char const * const * const argv)
{
    int error = 0;

    for (int argv_index = 0; argv_index < argc; ++argv_index) {
        if (*argv[argv_index] == '-') {      // Treat dash as an option.
            switch (argv[argv_index][1]) {   // Parse options.
            default:
                fprintf(stderr, "error: unknown argument: '%s'\n", argv[argv_index]);
                usage(argv[0]);
                error = EINVAL;
                break;
            }
        } else {
            // Treat the 'bare' argument as the polynomial.
            crc16_poly = (uint16_t) strtoul(argv[argv_index], NULL, 0);
        }
    }

    fprintf(stdout, "Using polynomial: 0x%04x\n", crc16_poly);
    if (crc16_poly == 0x0000) {
        usage(argv[0]);
    }

    // Simulate CRC-16-CCITT
    boost::crc_basic<16>  crc_ccitt1(crc16_poly, 0xFFFF, 0, false, false);
    crc_ccitt1.process_bytes(test_data, test_data_length);
    uint16_t const result1 = crc_ccitt1.checksum();

    uint16_t crc_table[256];
    crc16_table_init(crc_table, crc16_poly);
    uint16_t const result2 = crc16_calc(crc_table, CRC16_SEED, test_data, test_data_length);

    bool const success = (result1 == result2);

    fprintf(stdout, "results: 0x%04x, 0x%04x, %s\n", result1, result2, success? "PASS" : "FAIL");

    return success? 0 : -1;
}


// Copyright (c) 2016
// Author: Chrono Law
#include <std.hpp>
using namespace std;

#include <boost/mpl/if.hpp>
using namespace boost;
using namespace boost::mpl;

///////////////////////////////////////
#include <boost/type_traits.hpp>

void case1()
{
    typedef if_c<true, int, long>::type mdata1;
    assert((std::is_same<mdata1, int>::value));

    typedef if_<false_, float, double>::type mdata2;
    assert((std::is_same<mdata2, double>::value));

    typedef if_<std::is_integral<mdata2>,
        integral_promotion<mdata2>::type,
        floating_point_promotion<mdata2>::type
    >::type mdata3;
    assert((std::is_same<mdata3, double>::value));

}

///////////////////////////////////////

#include <boost/mpl/eval_if.hpp>
#include <boost/mpl/identity.hpp>

void case2()
{
    typedef eval_if_c<true,
        identity<int>, identity<long>>::type mdata1;
    assert((std::is_same<mdata1, int>::value));

    typedef eval_if<false_,
        identity<float>, identity<double>>::type mdata2;
    assert((std::is_same<mdata2, double>::value));

    typedef eval_if<std::is_integral<mdata2>,
        integral_promotion<mdata2>,
        floating_point_promotion<mdata2>
    >::type mdata3;
    assert((std::is_same<mdata3, double>::value));

}

///////////////////////////////////////

int main()
{
    std::cout << "hello if" << std::endl;

    case1();
    case2();
}


/****************************
 * 题目：给定一组世界坐标系下的3D点(p3d.txt)以及它在相机中对应的坐标(p2d.txt)，以及相机的内参矩阵。
 * 使用bundle adjustment 方法（g2o库实现）来估计相机的位姿T。初始位姿T为单位矩阵。
 *
* 本程序学习目标：
 * 熟悉g2o库编写流程，熟悉顶点定义方法。
 *
 * 公众号：计算机视觉life。发布于公众号旗下知识星球：从零开始学习SLAM
 * 时间：2019.02
****************************/

#include <vector>
#include <fstream>
#include <iostream>
#include <opencv2/core/core.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

using namespace Eigen;

using namespace cv;
using namespace std;

string p3d_file = "/home/lab-307/LIO-SAM_ws/src/Scout-LIO/test/data/p3d.txt";
string p2d_file = "/home/lab-307/LIO-SAM_ws/src/Scout-LIO/test/data/p2d.txt";

//// 自定义顶点，6DOF的姿态
class myVertex : public g2o::BaseVertex<6, g2o::SE3Quat>{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    myVertex() = default;
    bool read(std::istream& is) override{};
    bool write(std::ostream& os) const override{};
    void setToOriginImpl() override {
        //// 初始值设置为 SE3单位矩阵
        _estimate = g2o::SE3Quat();
    }

    void oplusImpl(const number_t* update_) override {
        Eigen::Map<const g2o::Vector6> update(update_);
        setEstimate(g2o::SE3Quat::exp(update)*estimate());        //更新方式
    }
};

//// 自定义边
class myEdge: public g2o::BaseBinaryEdge<2, g2o::Vector2 , g2o::VertexSBAPointXYZ, g2o::VertexSE3Expmap>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    myEdge() = default;
    bool read(istream& in) override{};
    bool write(ostream& out) const override{};
    void computeError() override
    {
        const g2o::VertexSE3Expmap* v1 = static_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
        const g2o::VertexSBAPointXYZ* v2 = static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
        const g2o::CameraParameters * cam
                = static_cast<const g2o::CameraParameters *>(parameter(0));
        g2o::Vector2 obs(_measurement);
        _error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
    }

    void linearizeOplus() override
    {
        g2o::VertexSE3Expmap * vj = static_cast<g2o::VertexSE3Expmap *>(_vertices[1]);
        g2o::SE3Quat T(vj->estimate());
        g2o::VertexSBAPointXYZ* vi = static_cast<g2o::VertexSBAPointXYZ*>(_vertices[0]);
        g2o::Vector3 xyz = vi->estimate();
        g2o::Vector3 xyz_trans = T.map(xyz);

        double x = xyz_trans[0];
        double y = xyz_trans[1];
        double z = xyz_trans[2];
        double z_2 = z*z;

        const g2o::CameraParameters * cam = static_cast<const g2o::CameraParameters *>(parameter(0));

        Matrix<double,2,3,Eigen::ColMajor> tmp;
        tmp(0,0) = cam->focal_length;
        tmp(0,1) = 0;
        tmp(0,2) = -x/z*cam->focal_length;

        tmp(1,0) = 0;
        tmp(1,1) = cam->focal_length;
        tmp(1,2) = -y/z*cam->focal_length;

        _jacobianOplusXi =  -1./z * tmp * T.rotation().toRotationMatrix();

        _jacobianOplusXj(0,0) =  x*y/z_2 *cam->focal_length;
        _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *cam->focal_length;
        _jacobianOplusXj(0,2) = y/z *cam->focal_length;
        _jacobianOplusXj(0,3) = -1./z *cam->focal_length;
        _jacobianOplusXj(0,4) = 0;
        _jacobianOplusXj(0,5) = x/z_2 *cam->focal_length;

        _jacobianOplusXj(1,0) = (1+y*y/z_2) *cam->focal_length;
        _jacobianOplusXj(1,1) = -x*y/z_2 *cam->focal_length;
        _jacobianOplusXj(1,2) = -x/z *cam->focal_length;
        _jacobianOplusXj(1,3) = 0;
        _jacobianOplusXj(1,4) = -1./z *cam->focal_length;
        _jacobianOplusXj(1,5) = y/z_2 *cam->focal_length;
    }
private:
    // data
};

void bundleAdjustment (
        const vector<Point3f> points_3d,
        const vector<Point2f> points_2d,
        Mat& K,
        bool useDefaultEdge
        );

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

    bool useDefaultEdge = true;
    vector< Point3f > p3d;
    vector< Point2f > p2d;

    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );

    // 导入3D点和对应的2D点

    ifstream fp3d(p3d_file);
    if (!fp3d){
        cout<< "No p3d.text file" << endl;
        return -1;
    }
    else {
        while (!fp3d.eof()){
            double pt3[3] = {0};
            for (auto &p:pt3) {
                fp3d >> p;
            }
            p3d.push_back(Point3f(pt3[0],pt3[1],pt3[2]));
        }
    }
    ifstream fp2d(p2d_file);
    if (!fp2d){
        cout<< "No p2d.text file" << endl;
        return -1;
    }
    else {
        while (!fp2d.eof()){
            double pt2[2] = {0};
            for (auto &p:pt2) {
                fp2d >> p;
            }
            Point2f p2(pt2[0],pt2[1]);
            p2d.push_back(p2);
        }
    }

    assert(p3d.size() == p2d.size());

    int iterations = 100;
    double cost = 0, lastCost = 0;
    int nPoints = p3d.size();
    cout << "points: " << nPoints << endl;

    bundleAdjustment ( p3d, p2d, K , useDefaultEdge);
    return 0;
}

void bundleAdjustment (
        const vector< Point3f > points_3d,
        const vector< Point2f > points_2d,
        Mat& K,
        bool useDefaultEdge )
{
    // creat g2o
    // new g2o version. Ref:https://www.cnblogs.com/xueyuanaichiyu/p/7921382.html

    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3
    // 第1步：创建一个线性求解器LinearSolver
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();

    // 第2步：创建 BlockSolver。并用上面定义的线性求解器初始化
    Block* solver_ptr = new Block (  std::unique_ptr<Block::LinearSolverType>(linearSolver) );

    // 第3步：创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::unique_ptr<Block>(solver_ptr) );

    // 第4步：创建稀疏优化器
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );
    optimizer.setVerbose(true);
//    // old g2o version
//    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3
//    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
//    Block* solver_ptr = new Block ( linearSolver );     // 矩阵块求解器
//    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
//    g2o::SparseOptimizer optimizer;
//    optimizer.setAlgorithm ( solver );

    // 第5步：定义图的顶点和边。并添加到SparseOptimizer中

    // ----------------------开始你的代码：设置并添加顶点，初始位姿为单位矩阵

//    g2o::VertexSE3Expmap *pose = new g2o::VertexSE3Expmap();
//    pose->setId(0);
//    pose->setEstimate(g2o::SE3Quat());
//    optimizer.addVertex(pose);
    myVertex *pose = new myVertex();
    pose->setId(0);
    pose->setEstimate(g2o::SE3Quat());
    optimizer.addVertex(pose);

    int index = 1;
    for ( const Point3f p:points_3d )   // landmarks
    {
        g2o::VertexSBAPointXYZ *point = new g2o::VertexSBAPointXYZ();
        point->setId ( index++ );
        point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
        point->setMarginalized ( true );
        optimizer.addVertex ( point );
    }
    // ----------------------结束你的代码

    // 设置相机内参
    g2o::CameraParameters* camera = new g2o::CameraParameters (
            K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0);
    camera->setId ( 0 );
    optimizer.addParameter ( camera );

    // 设置边
    if (useDefaultEdge){
        index = 1;
        for ( const Point2f p:points_2d )
        {
            g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
            edge->setId ( index );
            edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
            edge->setVertex ( 1, pose );
            edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );  //设置观测值
            edge->setParameterId ( 0,0 );
            edge->setInformation ( Eigen::Matrix2d::Identity() );
            optimizer.addEdge ( edge );
            index++;
        }
    } else {
        index = 1;
        for( const Point2f p:points_2d )
        {
            myEdge *XYZ2UV = new myEdge();
            XYZ2UV->setId(index);
            XYZ2UV->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
            XYZ2UV->setVertex ( 1, pose );
            XYZ2UV->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );  //设置观测值
            XYZ2UV->setParameterId ( 0,0 );
            XYZ2UV->setInformation ( Eigen::Matrix2d::Identity() );
            optimizer.addEdge ( XYZ2UV );
            index++;
        }
    }

    // 第6步：设置优化参数，开始执行优化
    optimizer.setVerbose ( false );
    optimizer.initializeOptimization();
    optimizer.optimize ( 100 );

    // 输出优化结果
    cout<< endl <<"after optimization:"<<endl;
    cout<< "T=" << endl << Eigen::Isometry3d ( pose->estimate() ).matrix() << endl;
}


/*
 * Copyright (c) 2013-2017 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef PSA_PLOT_HPP
#define PSA_PLOT_HPP

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

#include <kv/psa.hpp>
#include <kv/matplotlib.hpp>

namespace kv {

namespace ub = boost::numeric::ublas;

template<class T1, class T2>
void psa_plot(const psa<T1>& x, const T2& offset, const matplotlib& g, int div = 50, const char * color = "blue")
{
	double s, e, off, p1, p2, l1, u1, l2, u2;
	int i;

	s = psa<T1>::domain().lower();
	e = psa<T1>::domain().upper();
	// off = mid(offset);
	off = offset;

	for (i=0; i<div; i++) {
		p1 = s + (e - s) * i / div;
		p2 = s + (e - s) * (i+1) / div;
		l1 = eval(x, (T1)p1).lower();
		u1 = eval(x, (T1)p1).upper();
		l2 = eval(x, (T1)p2).lower();
		u2 = eval(x, (T1)p2).upper();
		g.line(off + p1, l1, off + p2, l2, color);
		g.line(off + p1, u1, off + p2, u2, color);
	}
}

} // namespace kv

#endif // PSA_PLOT_HPP


// Copyright (C) 2009  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include "tester.h"
#include <dlib/svm.h>
#include <dlib/rand.h>
#include <dlib/string.h>
#include <vector>
#include <sstream>
#include <ctime>

namespace
{
    using namespace test;
    using namespace dlib;
    using namespace std;
    dlib::logger dlog("test.empirical_kernel_map");

    class empirical_kernel_map_tester : public tester
    {
        /*!
            WHAT THIS OBJECT REPRESENTS
                This object represents a unit test.  When it is constructed
                it adds itself into the testing framework.
        !*/
    public:
        empirical_kernel_map_tester (
        ) :
            tester (
                "test_empirical_kernel_map",       // the command line argument name for this test
                "Run tests on the empirical_kernel_map object.", // the command line argument description
                0                     // the number of command line arguments for this test
            )
        {
            // always use the same time so that tests are repeatable
            thetime = 0;//time(0);
        }

        time_t thetime;
        dlib::rand rnd;

        void test_projection_error()
        {
            for (int runs = 0; runs < 10; ++runs)
            {
                print_spinner();
                typedef matrix<double,0,1> sample_type;
                typedef radial_basis_kernel<sample_type> kernel_type;
                const kernel_type kern(0.2);

                empirical_kernel_map<kernel_type> ekm;

                // generate samples
                const int num = rnd.get_random_8bit_number()%50 + 1;
                std::vector<sample_type> samples;
                for (int i = 0; i < num; ++i)
                {
                    samples.push_back(randm(5,1,rnd));
                }

                ekm.load(kern, samples);
                DLIB_TEST(ekm.basis_size() == samples.size());

                double err;

                // the samples in the basis should have zero projection error
                for (unsigned long i = 0; i < samples.size(); ++i)
                {
                    ekm.project(samples[i], err);
                    DLIB_TEST_MSG(abs(err) < 1e-13, abs(err));

                }

                // Do some sanity tests on the conversion to distance functions while we are at it.
                for (int i = 0; i < 30; ++i)
                {
                    // pick two random samples
                    const sample_type samp1 = samples[rnd.get_random_32bit_number()%samples.size()];
                    const sample_type samp2 = samples[rnd.get_random_32bit_number()%samples.size()];

                    const matrix<double,0,1> proj1 = ekm.project(samp1);
                    const matrix<double,0,1> proj2 = ekm.project(samp2);

                    distance_function<kernel_type> df1 = ekm.convert_to_distance_function(proj1);
                    distance_function<kernel_type> df2 = ekm.convert_to_distance_function(proj2);

                    DLIB_TEST(df1.get_kernel() == kern);
                    DLIB_TEST(df2.get_kernel() == kern);

                    // make sure the norms are correct
                    DLIB_TEST(std::abs(df1.get_squared_norm()  -
                                       trans(df1.get_alpha())*kernel_matrix(df1.get_kernel(),df1.get_basis_vectors())*df1.get_alpha()) < 1e-10);
                    DLIB_TEST(std::abs(df2.get_squared_norm()  -
                                       trans(df2.get_alpha())*kernel_matrix(df2.get_kernel(),df2.get_basis_vectors())*df2.get_alpha()) < 1e-10);

                    const double true_dist = std::sqrt(kern(samp1,samp1) + kern(samp2,samp2) - 2*kern(samp1,samp2));
                    DLIB_TEST_MSG(abs(df1(df2) - true_dist) < 1e-7, abs(df1(df2) - true_dist));
                    DLIB_TEST_MSG(abs(length(proj1-proj2) - true_dist) < 1e-7, abs(length(proj1-proj2) - true_dist));

                    // test distance function operators
                    const decision_function<kernel_type> dec1 = ekm.convert_to_decision_function(proj1);
                    const decision_function<kernel_type> dec2 = ekm.convert_to_decision_function(proj2);
                    DLIB_TEST(dec1.kernel_function == kern);
                    DLIB_TEST(dec2.kernel_function == kern);

                    distance_function<kernel_type> temp;
                    temp = dec1;
                    DLIB_TEST(std::abs(temp.get_squared_norm() - df1.get_squared_norm()) < 1e-10);
                    temp = dec2;
                    DLIB_TEST(std::abs(temp.get_squared_norm() - df2.get_squared_norm()) < 1e-10);
                    temp = distance_function<kernel_type>(dec1.alpha, dec1.kernel_function, dec1.basis_vectors);
                    DLIB_TEST(std::abs(temp.get_squared_norm() - df1.get_squared_norm()) < 1e-10);

                    df1 = dec1;

                    temp = df1 + df2;
                    decision_function<kernel_type> dec3(temp.get_alpha(), 0, temp.get_kernel(), temp.get_basis_vectors());
                    DLIB_TEST(std::abs(temp.get_squared_norm()  -
                                       trans(temp.get_alpha())*kernel_matrix(temp.get_kernel(),temp.get_basis_vectors())*temp.get_alpha()) < 1e-10);
                    for (unsigned long j = 0; j < samples.size(); ++j)
                    {
                        DLIB_TEST(std::abs(dec3(samples[j]) - (dec1(samples[j]) + dec2(samples[j]))) < 1e-10);
                    }

                    temp = df1 - df2;
                    dec3 = decision_function<kernel_type>(temp.get_alpha(), 0, temp.get_kernel(), temp.get_basis_vectors());
                    DLIB_TEST(std::abs(temp.get_squared_norm()  -
                                       trans(temp.get_alpha())*kernel_matrix(temp.get_kernel(),temp.get_basis_vectors())*temp.get_alpha()) < 1e-10);
                    for (unsigned long j = 0; j < samples.size(); ++j)
                    {
                        DLIB_TEST(std::abs(dec3(samples[j]) - (dec1(samples[j]) - dec2(samples[j]))) < 1e-10);
                    }

                    temp = 3*(df1 - df2)*2;
                    dec3 = decision_function<kernel_type>(temp.get_alpha(), 0, temp.get_kernel(), temp.get_basis_vectors());
                    DLIB_TEST(std::abs(temp.get_squared_norm()  -
                                       trans(temp.get_alpha())*kernel_matrix(temp.get_kernel(),temp.get_basis_vectors())*temp.get_alpha()) < 1e-10);
                    for (unsigned long j = 0; j < samples.size(); ++j)
                    {
                        DLIB_TEST(std::abs(dec3(samples[j]) - 6*(dec1(samples[j]) - dec2(samples[j]))) < 1e-10);
                    }

                    distance_function<kernel_type> df_empty(kern);

                    temp = df_empty + (df1 + df2)/2 + df_empty - df_empty + (df_empty + df_empty) - (df_empty - df_empty);
                    dec3 = decision_function<kernel_type>(temp.get_alpha(), 0, temp.get_kernel(), temp.get_basis_vectors());
                    DLIB_TEST(std::abs(temp.get_squared_norm()  -
                                       trans(temp.get_alpha())*kernel_matrix(temp.get_kernel(),temp.get_basis_vectors())*temp.get_alpha()) < 1e-10);
                    for (unsigned long j = 0; j < samples.size(); ++j)
                    {
                        DLIB_TEST(std::abs(dec3(samples[j]) - 0.5*(dec1(samples[j]) + dec2(samples[j]))) < 1e-10);
                    }
                }
                // Do some sanity tests on the conversion to distance functions while we are at it.  This
                // time multiply one of the projections by 30 and see that it still all works out right.
                for (int i = 0; i < 30; ++i)
                {
                    // pick two random samples
                    const sample_type samp1 = samples[rnd.get_random_32bit_number()%samples.size()];
                    const sample_type samp2 = samples[rnd.get_random_32bit_number()%samples.size()];

                    matrix<double,0,1> proj1 = ekm.project(samp1);
                    matrix<double,0,1> proj2 = 30*ekm.project(samp2);

                    distance_function<kernel_type> df1 = ekm.convert_to_distance_function(proj1);
                    distance_function<kernel_type> df2 = ekm.convert_to_distance_function(proj2);

                    DLIB_TEST_MSG(abs(length(proj1-proj2) - df1(df2)) < 1e-7, abs(length(proj1-proj2) - df1(df2)));
                }

                // now generate points with projection error
                for (double i = 1; i < 10; ++i)
                {
                    sample_type test_point = i*randm(5,1,rnd);
                    ekm.project(test_point, err);
                    // turn into normal distance rather than squared distance
                    err = sqrt(err);
                    dlog << LTRACE << "projection error: " << err;

                    distance_function<kernel_type> df = ekm.convert_to_distance_function(ekm.project(test_point));

                    // the projection error should be the distance between the test_point and the point it gets
                    // projected onto
                    DLIB_TEST_MSG(abs(df(test_point) - err) < 1e-10, abs(df(test_point) - err));
                    // while we are at it make sure the squared norm in the distance function is right
                    double df_error = abs(df.get_squared_norm() - trans(df.get_alpha())*kernel_matrix(kern, samples)*df.get_alpha());
                    DLIB_TEST_MSG( df_error < 1e-10, df_error);
                }

            }
        }

        template <typename kernel_type>
        void test_with_kernel(const kernel_type& kern)
        {
            typedef typename kernel_type::sample_type sample_type;

            empirical_kernel_map<kernel_type> ekm, ekm2, ekm3;

            for (int j = 0; j < 10; ++j)
            {
                sample_type samp;
                std::vector<sample_type> samples;
                std::vector<sample_type> proj_samples;
                print_spinner();
                const int num = rnd.get_random_8bit_number()%200 + 1;
                // make some random samples
                for (int i = 0; i < num; ++i)
                {
                    samples.push_back(randm(4,1,rnd));
                }
                // add on a little bit to make sure there is at least one non-zero sample.  If all the
                // samples are zero then empirical_kernel_map_error will be thrown and we don't want that.
                samples.front()(0) += 0.001;

                ekm2.load(kern, samples);
                DLIB_TEST(ekm2.basis_size() == samples.size());
                for (unsigned long i = 0; i < samples.size(); ++i)
                    DLIB_TEST(dlib::equal(ekm2[i] , samples[i]));

                // test serialization
                ostringstream sout;
                serialize(ekm2, sout);
                ekm2.clear();
                istringstream sin(sout.str());
                deserialize(ekm3, sin);
                // also test swap
                ekm3.swap(ekm);
                DLIB_TEST(ekm.get_kernel() == kern);
                DLIB_TEST(ekm.out_vector_size() != 0);
                DLIB_TEST(ekm2.out_vector_size() == 0);
                DLIB_TEST(ekm3.out_vector_size() == 0);

                // project all the samples into kernel space
                for (unsigned long i = 0; i < samples.size(); ++i)
                {
                    proj_samples.push_back(ekm.project(samples[i]));
                }

                DLIB_TEST(max(abs(kernel_matrix(kern, samples) - kernel_matrix(linear_kernel<sample_type>(), proj_samples))) < 1e-12);
                DLIB_TEST(ekm.out_vector_size() == proj_samples[0].size());

                for (int i = 0; i < 30; ++i)
                {
                    const unsigned long idx1 = rnd.get_random_32bit_number()%samples.size();
                    const unsigned long idx2 = rnd.get_random_32bit_number()%samples.size();
                    decision_function<kernel_type> dec_funct = ekm.convert_to_decision_function(proj_samples[idx1]);
                    distance_function<kernel_type> dist_funct = ekm.convert_to_distance_function(proj_samples[idx1]);

                    // make sure the distances match
                    const double dist_error = abs(length(proj_samples[idx1] - proj_samples[idx2]) - dist_funct(samples[idx2]));
                    DLIB_TEST_MSG( dist_error < 1e-6, dist_error);
                    // make sure the dot products match
                    DLIB_TEST(abs(dot(proj_samples[idx1],proj_samples[idx2]) - dec_funct(samples[idx2])) < 1e-10);

                    // also try the dec_funct with samples that weren't in the original set
                    samp = 100*randm(4,1,rnd);
                    // make sure the dot products match
                    DLIB_TEST(abs(dot(proj_samples[idx1],ekm.project(samp)) - dec_funct(samp)) < 1e-10);
                    samp = randm(4,1,rnd);
                    // make sure the dot products match
                    DLIB_TEST(abs(dot(proj_samples[idx1],ekm.project(samp)) - dec_funct(samp)) < 1e-10);
                }

                proj_samples.clear();

                // now do the projection but use the projection_function returned by get_projection_function()
                projection_function<kernel_type> proj2 = ekm.get_projection_function();
                projection_function<kernel_type> proj;
                sout.clear();
                sout.str("");
                sin.clear();
                sin.str("");
                // test serialization
                serialize(proj2, sout);
                sin.str(sout.str());
                deserialize(proj, sin);

                for (unsigned long i = 0; i < samples.size(); ++i)
                {
                    proj_samples.push_back(proj(samples[i]));
                }

                DLIB_TEST(max(abs(kernel_matrix(kern, samples) - kernel_matrix(linear_kernel<sample_type>(), proj_samples))) < 1e-12);
                DLIB_TEST(ekm.out_vector_size() == proj_samples[0].size());
                DLIB_TEST(proj.out_vector_size() == proj_samples[0].size());

                ekm.clear();
                DLIB_TEST(ekm.out_vector_size() == 0);
                DLIB_TEST(ekm2.out_vector_size() == 0);
                DLIB_TEST(ekm3.out_vector_size() == 0);

                for (int i = 0; i < 30; ++i)
                {
                    const unsigned long idx1 = rnd.get_random_32bit_number()%samples.size();
                    const unsigned long idx2 = rnd.get_random_32bit_number()%samples.size();
                    decision_function<kernel_type> dec_funct = convert_to_decision_function(proj,proj_samples[idx1]);

                    // make sure the dot products match
                    DLIB_TEST(abs(dot(proj_samples[idx1],proj_samples[idx2]) - dec_funct(samples[idx2])) < 1e-10);

                    // also try the dec_funct with samples that weren't in the original set
                    samp = 100*randm(4,1,rnd);
                    // make sure the dot products match
                    DLIB_TEST(abs(dot(proj_samples[idx1],proj(samp)) - dec_funct(samp)) < 1e-10);
                    samp = randm(4,1,rnd);
                    // make sure the dot products match
                    DLIB_TEST(abs(dot(proj_samples[idx1],proj(samp)) - dec_funct(samp)) < 1e-10);
                }

            }

            for (int j = 1; j <= 20; ++j)
            {
                dlog << LTRACE << "j: " << j;
                sample_type samp, samp2;
                std::vector<sample_type> samples1;
                std::vector<sample_type> samples2;
                print_spinner();
                // make some random samples.  At the end samples1 will be a subset of samples2
                for (int i = 0; i < 5*j; ++i)
                {
                    samples1.push_back(randm(10,1,rnd));
                    samples2.push_back(samples1.back());
                }
                for (int i = 0; i < 5*j; ++i)
                {
                    samples2.push_back(randm(10,1,rnd));
                }
                // add on a little bit to make sure there is at least one non-zero sample.  If all the
                // samples are zero then empirical_kernel_map_error will be thrown and we don't want that.
                samples1.front()(0) += 0.001;
                samples2.front()(0) += 0.001;

                ekm.load(kern, samples1);
                for (unsigned long i = 0; i < samples1.size(); ++i)
                    DLIB_TEST(dlib::equal(ekm[i] , samples1[i]));
                DLIB_TEST(ekm.basis_size() == samples1.size());
                ekm2.load(kern, samples2);
                DLIB_TEST(ekm2.basis_size() == samples2.size());

                dlog << LTRACE << "ekm.out_vector_size(): " << ekm.out_vector_size();
                dlog << LTRACE << "ekm2.out_vector_size(): " << ekm2.out_vector_size();
                const double eps = 1e-6;

                matrix<double> transform;
                // Make sure transformations back to yourself work right.  Note that we can't just
                // check that transform is the identity matrix since it might be an identity transform
                // for only a subspace of vectors (this happens if the ekm maps points into a subspace of
                // all possible ekm.out_vector_size() vectors).
                transform = ekm.get_transformation_to(ekm);
                DLIB_TEST(transform.nr() == ekm.out_vector_size());
                DLIB_TEST(transform.nc() == ekm.out_vector_size());
                for (unsigned long i = 0; i < samples1.size(); ++i)
                {
                    samp = ekm.project(samples1[i]);
                    DLIB_TEST_MSG(length(samp - transform*samp) < eps, length(samp - transform*samp));
                    samp = ekm.project((samples1[0] + samples1[i])/2);
                    DLIB_TEST_MSG(length(samp - transform*samp) < eps, length(samp - transform*samp));
                }

                transform = ekm2.get_transformation_to(ekm2);
                DLIB_TEST(transform.nr() == ekm2.out_vector_size());
                DLIB_TEST(transform.nc() == ekm2.out_vector_size());
                for (unsigned long i = 0; i < samples2.size(); ++i)
                {
                    samp = ekm2.project(samples2[i]);
                    DLIB_TEST_MSG(length(samp - transform*samp) < eps, length(samp - transform*samp));
                    samp = ekm2.project((samples2[0] + samples2[i])/2);
                    DLIB_TEST_MSG(length(samp - transform*samp) < eps, length(samp - transform*samp));
                    //dlog << LTRACE << "mapping error: " << length(samp - transform*samp);
                }

                // now test the transform from ekm to ekm2
                transform = ekm.get_transformation_to(ekm2);
                DLIB_TEST(transform.nr() == ekm2.out_vector_size());
                DLIB_TEST(transform.nc() == ekm.out_vector_size());
                for (unsigned long i = 0; i < samples1.size(); ++i)
                {
                    samp = ekm.project(samples1[i]);
                    distance_function<kernel_type> df1 = ekm.convert_to_distance_function(samp);
                    distance_function<kernel_type> df2 = ekm2.convert_to_distance_function(transform*samp);
                    DLIB_TEST_MSG(df1(df2) < eps, df1(df2));
                    //dlog << LTRACE << "mapping error: " << df1(df2);

                    samp = ekm.project((samples1[0] + samples1[i])/2);
                    df1 = ekm.convert_to_distance_function(samp);
                    df2 = ekm2.convert_to_distance_function(transform*samp);
                    DLIB_TEST_MSG(df1(df2) < eps, df1(df2));
                }

            }
        }

        void perform_test (
        )
        {
            ++thetime;
            typedef matrix<double,0,1> sample_type;
            dlog << LINFO << "time seed: " << thetime;
            rnd.set_seed(cast_to_string(thetime));

            print_spinner();
            test_projection_error();
            print_spinner();
            dlog << LINFO << "test with linear kernel";
            test_with_kernel(linear_kernel<sample_type>());
            print_spinner();
            dlog << LINFO << "test with rbf kernel";
            test_with_kernel(radial_basis_kernel<sample_type>(0.2));
            print_spinner();
        }
    };

    // Create an instance of this object.  Doing this causes this test
    // to be automatically inserted into the testing framework whenever this cpp file
    // is linked into the project.  Note that since we are inside an unnamed-namespace
    // we won't get any linker errors about the symbol a being defined multiple times.
    empirical_kernel_map_tester a;

}


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

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

using namespace boost::simd;
using namespace boost::alignment;

struct g
{
  g(float seed = 0.0f, float step = 1.0f)
    : i_(seed),  step_(step)  {}
  template < typename T> T operator()(bs::as_<T>)
  {
    auto z = bs::enumerate<T>(i_, step_);
    i_+= bs::cardinal_of<T>::value*step_;
    return z;
  }
  float i_;
  float step_;
};

struct gstd
{
  gstd(float seed = 0.0f, float step = 1.0f)
    : i_(seed),  step_(step)  {}
  float operator()()
  {
    float z = i_;
    i_+= step_;
    return z;
  }
  float i_;
  float step_;
};

STF_CASE_TPL( "Check unary simd::generate", STF_NUMERIC_TYPES )
{
  static const int N = pack<T>::static_size;

  std::vector<T> values(2*N+1), ref(2*N+1);
  std::generate(ref.begin(), ref.end(), gstd());
  boost::simd::generate(values.data(), values.data()+values.size(), g());
  STF_EQUAL( values, ref );
  std::generate(ref.begin(), ref.end(), gstd(2, 3));
  boost::simd::generate(values.data(), values.data()+values.size(), g(2, 3));
  STF_EQUAL( values, ref );
}