#include "ParticlePhysics.h"
#include "Inputfile.hpp"
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <istream>
#include <random>
#include <cmath>
#include <boost/program_options.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <iostream>
#include <iterator>
#include <omp.h>
#include <sys/time.h>

using namespace std;
using namespace Eigen;
namespace po = boost::program_options;
namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

BOOST_GEOMETRY_REGISTER_POINT_2D(Eigen::Vector2d, double, boost::geometry::cs::cartesian, Eigen::Vector2d::x(), Eigen::Vector2d::y())

ParticlePhysics::ParticlePhysics(Options& o, LagrangianState& simulation, bool useRtree)
: options_(o), simulation_(&simulation), boundaryParticles_(NULL)
{
  samples_ = simulation_->getSamples();
  if (useRtree){
    contact_model_ = std::make_unique<RTreeContactForceModel>();
  } else {
    contact_model_ = std::make_unique<BruteForceContactForceModel>();
  }
  calculateParticleMasses();
}

ParticlePhysics::ParticlePhysics(Options& o, LagrangianState& simulation, LagrangianState& boundaryParticles, bool useRtree)
  : options_(o), simulation_(&simulation), boundaryParticles_(&boundaryParticles)
{
  samples_ = simulation_->getSamples();
  if (useRtree){
    contact_model_ = std::make_unique<RTreeContactForceModel>();
  } else {
    contact_model_ = std::make_unique<BruteForceContactForceModel>();
  }
  calculateParticleMasses();
}

ParticlePhysics::~ParticlePhysics() {

}

void ParticlePhysics::initializeParticleInteractionTracker() {
  interactions_ = MatrixXi::Zero(samples_,samples_);
}

void
BruteForceContactForceModel::
particleContact(const LagrangianState &simulation,
		Eigen::MatrixXd& forces)
{
  // Calculate all particle-wall contacts (N^2 brute force)
  const MatrixXd& XY = simulation.getXY();
  const MatrixXd& UV = simulation.getUV();
  const VectorXd& R  = simulation.getR();
  const int samples  = simulation.getSamples();

  forces.resizeLike(XY);
  forces.array() = 0;

# pragma omp parallel for
  for (int i=0; i<samples; i++) {
    for (int j=i+1; j<samples; j++) {
      Vector2d dij        = XY.row(j)-XY.row(i);
      double distance_ij  = dij.norm();
      if (distance_ij <= R(i)+R(j)) {
        double ui_tangent = UV.row(i).dot(dij)/distance_ij;
        double uj_tangent = UV.row(j).dot(dij)/distance_ij;
        if ((ui_tangent - uj_tangent) > 0) {
          const auto local_force = modelContactForces(R(i),R(j),dij,simulation);

          forces(i,0) -= local_force(0);
          forces(i,1) -= local_force(1);
          forces(j,0) += local_force(0);
          forces(j,1) += local_force(1);
        }
      }
    }
  }
}

void
BruteForceContactForceModel::
particleWallContact(const LagrangianState& simulation,
		    const LagrangianState& boundary,
		    Eigen::MatrixXd& forces)
{
  // Calculate all particle-particle contacts (N^2 brute force)
  const MatrixXd& XY   = simulation.getXY();
  const MatrixXd& UV   = simulation.getUV();
  const VectorXd& R    = simulation.getR();
  const int Nparticles = simulation.getSamples();
  const MatrixXd& XYb  = boundary.getXY();
  const MatrixXd& UVb  = boundary.getUV();
  const VectorXd& Rb   = boundary.getR();
  const int Nboundary  = boundary.getSamples();

# pragma omp parallel for
  for (int i=0; i<Nparticles; i++) {
    for (int j=0; j<Nboundary; j++) {
      Vector2d dij        = XYb.row(j)-XY.row(i);
      double distance_ij  = dij.norm();
      if (distance_ij <= R(i)+Rb(j)) {
        double ui_tangent = UV.row(i).dot(dij)/distance_ij;
        double uj_tangent = UV.row(j).dot(dij)/distance_ij;
        if ((ui_tangent - uj_tangent) > 0) {
	  const auto local_force = modelContactForces(R(i),Rb(j),dij,simulation);

          forces(i,0) -= local_force(0);
          forces(i,1) -= local_force(1);
        }
      }
    }
  }
}

void
RTreeContactForceModel::
particleContact(const LagrangianState &simulation,
                Eigen::MatrixXd &forces)
{
  const MatrixXd& XY = simulation.getXY();
  const MatrixXd& UV = simulation.getUV();
  const VectorXd& R  = simulation.getR();
  const int samples  = simulation.getSamples();

  forces.resizeLike(XY);
  forces.array() = 0;

  // Rtree construction
  typedef std::pair<Vector2d, unsigned> value;
  bgi::rtree< value, bgi::quadratic<16> > rtree;
  for (int i=0; i<samples; i++) {
    rtree.insert(std::make_pair(Vector2d(XY(i,0), XY(i,1)), i));
  }

  for (int i=0; i<samples; i++) {

    std::vector<value> contact_points;
    const Vector2d search_point(XY(i,0), XY(i,1));

    // rtree search using a lambda function that returns true for the distance
    // between the points less than the sum of their radii.
    rtree.query(bgi::satisfies([&](value const& v) {
      return bg::distance(v.first, search_point) <= R(i)+R(v.second); }),
      std::back_inserter(contact_points));

    for (const auto& found_point : contact_points){
      const int idx = found_point.second;
      const Vector2d dij( found_point.first[0] - XY(i,0),
                          found_point.first[1] - XY(i,1) );

      const double distance_ij = dij.norm();
      const double ui_tangent  = UV.row(i).dot(dij)/distance_ij;
      const double uj_tangent  = UV.row(idx).dot(dij)/distance_ij;

      if (((ui_tangent - uj_tangent) > 0) && idx > i) {
        const auto local_force = modelContactForces(R(i),R(idx),dij,simulation);

        forces(i,0)   -= local_force(0);
        forces(i,1)   -= local_force(1);
        forces(idx,0) += local_force(0);
        forces(idx,1) += local_force(1);
      }
    }
  }
}

void
RTreeContactForceModel::
particleWallContact(const LagrangianState& simulation,
		    const LagrangianState& boundary,
		    Eigen::MatrixXd& forces)
{
  return;
}

void ParticlePhysics::calculateParticleMasses() {
  // Assume all particles have the same density
  double rho = 1.0;
  mass_.resize(samples_);
  const VectorXd& R = simulation_->getR();
  mass_ = rho*0.5*M_PI*R.array().pow(2);

}

double ParticlePhysics::calculateTotalEnergy() {
  double energy = 0.0;
  MatrixXd UV = simulation_->getUV();
  for (int i=0; i<samples_; i++)
    energy += 0.5*mass_(i)*(pow(UV(i,0),2) + pow(UV(i,1),2));
  return energy;

}

VectorXd ParticlePhysics::calculateTotalMomentum() {
  VectorXd momentum(2);
  momentum(0) = 0.0; momentum(1) = 0.0;
  MatrixXd UV = simulation_->getUV();
  for (int i=0; i<samples_; i++) {
    momentum(0) += mass_(i)*UV(i,0);
    momentum(1) += mass_(i)*UV(i,1);
  }
  return momentum;

}

void ParticlePhysics::writeEnergyAndMomentum(double energy, VectorXd& momentum, const std::string& filename) const {
  ofstream xyout(filename);
  xyout << energy << ", " << momentum(0) << ", " << momentum(1) << std::endl;
  xyout.close();

}

void ParticlePhysics::zeroForces() {
  forces_  = MatrixXd::Zero(samples_,2);
}

const MatrixXd& ParticlePhysics::RHS() {
  zeroForces();
  contact_model_->particleContact(*simulation_, forces_);
  if (boundaryParticles_ != NULL) {
    contact_model_->particleWallContact(*simulation_, *boundaryParticles_, forces_);
  }
  for (int i=0; i<samples_; i++) {
    forces_(i,0) /= mass_(i);
    forces_(i,1) /= mass_(i);
  }
  return forces_;

}


/* Copyright 2020, 2021 Evandro Chagas Ribeiro da Rosa <evandro.crr@posgrad.ufsc.br>
 * Copyright 2020, 2021 Rafael de Santiago <r.santiago@ufsc.br>
 *
 * 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.
 */

#pragma once
#include "ket_bitwise.hpp"
#include <boost/unordered_set.hpp>
#include <stack>

class Simulator {
public:
    Simulator();
    void x(size_t idx, const ket::ctrl_list& ctrl = {});
    void y(size_t idx, const ket::ctrl_list& ctrl = {});
    void z(size_t idx, const ket::ctrl_list& ctrl = {});
    void h(size_t idx, const ket::ctrl_list& ctrl = {});
    void s(size_t idx, const ket::ctrl_list& ctrl = {});
    void sd(size_t idx, const ket::ctrl_list& ctrl = {});
    void t(size_t idx, const ket::ctrl_list& ctrl = {});
    void td(size_t idx, const ket::ctrl_list& ctrl = {});
    void p(double lambda, size_t idx, const ket::ctrl_list& ctrl = {});
    void u3(double theta, double phi, double lambda, size_t idx, const ket::ctrl_list& ctrl = {});
    void rz(double lambda, size_t idx, const ket::ctrl_list& ctrl = {});
    void measure(size_t idx);
    void alloc(size_t idx, bool dirty = false);
    void free(size_t idx, bool dirty = false);
    void apply_plugin(const boost::shared_ptr<ket::bitwise_api>& plugin, std::vector<size_t> idx, const std::string& args, bool adj, const ket::ctrl_list& ctrl);
    size_t get_bit(size_t idx);
    std::int64_t get_i64(size_t idx);
    std::int64_t i64_len() const;
    void set_i64(size_t idx, std::int64_t value);
    void print(size_t idx);
    std::string get_results();
    void dump(const std::vector<size_t>& idx);
    ket::dump_t get_dump(size_t idx) const;
    std::int64_t dumps_len() const;

private:
    inline ket::ctrl_list map_ctrl(const ket::ctrl_list& ctrl) const {
        ket::ctrl_list mapped_ctrl;
        for (auto i : ctrl) mapped_ctrl.push_back(allocated_qubits.at(i));
        return mapped_ctrl;
    }

    inline bool map_ctrl_for_plugin(ket::ctrl_list& ctrl) {
        if (ctrl.empty()) return true;

        ket::ctrl_list new_ctrl{};

        for (auto i: ctrl) {
            if (bitwise[i]->get_map().size() > 1) new_ctrl.push_back(i);
            else if (bitwise[i]->get_map().begin()->first.is_one(i)) continue;
            else return false;
        }

        ctrl.swap(new_ctrl);

        return true;
    }

    inline void merge_for_plugin(const ket::ctrl_list& ctrl) {
        if (ctrl.size() < 2) return;

        auto &ptr = bitwise[ctrl[0]];
        for (size_t i = 1; i < ctrl.size(); i++) if (ptr != bitwise[ctrl[i]]) {
            ptr = std::make_shared<ket::Bitwise>(*ptr, *bitwise[ctrl[i]]);

            auto &entangle_set = entangled[ctrl[0]];
            entangle_set->insert(entangled[ctrl[i]]->begin(), entangled[ctrl[i]]->end());

            auto entangle_backup = *entangled[ctrl[i]];
            for (auto j : entangle_backup)
                entangled[j] = entangle_set;

            for (auto j : *entangle_set)
                bitwise[j] = ptr;
        }

    }

    inline bool merge(size_t idx, ket::ctrl_list& ctrl) {
        if (ctrl.empty()) return true;

        ket::ctrl_list new_ctrl{};

        for (auto i: ctrl) {
            if (bitwise[i]->get_map().size() > 1) new_ctrl.push_back(i);
            else if (bitwise[i]->get_map().begin()->first.is_one(i)) continue;
            else return false;
        }

        ctrl.swap(new_ctrl);

        auto &ptr = bitwise[idx];
        for (auto i: ctrl) if (ptr != bitwise[i]) {
            ptr = std::make_shared<ket::Bitwise>(*ptr, *bitwise[i]);

            auto &entangle_set = entangled[idx];
            entangle_set->insert(entangled[i]->begin(), entangled[i]->end());

            auto entangle_backup = *entangled[i];
            for (auto j : entangle_backup)
                entangled[j] = entangle_set;

            for (auto j : *entangle_set)
                bitwise[j] = ptr;
        }

        return true;
    }

    boost::unordered_map<size_t, std::shared_ptr<ket::Bitwise>> bitwise;
    boost::unordered_map<size_t, std::shared_ptr<boost::unordered_set<size_t>>> entangled;

    boost::unordered_map<size_t, size_t> allocated_qubits;
    std::stack<size_t> free_qubits;
    std::stack<size_t> dirty_qubits;

    boost::unordered_map<size_t, int> measurement;
    boost::unordered_map<size_t, std::int64_t> i64s;

    std::vector<ket::dump_t> dumps;
};


/* Copyright (C) 2010 Ion Torrent Systems, Inc. All Rights Reserved */

//#include <armadillo>
#include "LevMarFitterV2.h"
#include "BkgFitLevMarDat.h"
#include <float.h>

using namespace arma;

int LevMarFitterV2::Fit (bool gauss_newton,
                         int max_iter,
                         float *y,
                         float *params,
                         float *std_err)
{
  int iter=0;
  int done_cnt = 0;
  float fval[npts];
  float r_start = FLT_MAX;
  float r_chg = 0.0f;
  float bfjac[npts*nparams];
  float err_vect[npts];
  double bfjtj[nparams*nparams];
  double bfrhs[nparams];

  data->delta->set_size (nparams);
  data->delta->zeros (nparams);
  // evaluate where we are
  // always have something even if we never succeed in a step
  Evaluate (fval,params);
  if (enable_fval_cache)
    memcpy (fval_cache,fval,sizeof (float[npts]));

  r_start = CalcResidual (y, fval, npts, &err_vect[0]);

  bool converged = false;
  while (!(converged = DoneTest (iter,max_iter,data,lambda,done_cnt,r_start,r_chg)) and !ForceQuit(iter,max_iter, lambda))
  {

    // remember the last residual for comparison later
    r_chg = 0.0;
    MakeJacobian (bfjac, params, fval);
    CalculateJTJ (bfjtj,bfjac);
    CalculateRHS (bfrhs,bfjac, err_vect, params);

    // the following loop saves computation if the first attempt to adjust fails, but subsequent
    // attempts with larger values of lambda would succeed.  In this case, a new value of lambda
    // can be attempted, but the jtj and rhs matricies did not actually change and do not
    // need to be re-computed

    if (gauss_newton)
      TryGaussNewtonStep(y, &fval[0], params, &err_vect[0], &bfjtj[0], &bfrhs[0], done_cnt, r_start, r_chg);
    else
      TryLevMarStep (y, &fval[0], params, &err_vect[0], &bfjtj[0], &bfrhs[0], done_cnt, r_start, r_chg);
    iter++;
  }

  SetConverged(converged);
  residual = r_start;

  // compute parameter std errs
  if (std_err)
  {

    Mat<double> tjac = (trans (* (data->jac)));
    Mat<double> std_err_jtj = (tjac * (* (data->jac)));

    Mat<double> std_err_jtj_inv = inv (std_err_jtj);
    double mrv = (residual * npts) / (npts - nparams); // mean residual variance
    std_err_jtj_inv *= mrv;

    for (int i=0;i < nparams;i++)
      std_err[i] = sqrt (std_err_jtj_inv (i, i));
  }
  if (numException >0)
    std::cout << "LevMarFitterV2: numException = " << numException << std::endl;
  regularizer = 0.0f; // reset
  numException = 0; // reset messages for next invocation of the same entity
  return (iter);
}

void LevMarFitterV2::Initialize (int _nparams,int _npts,float *_x)
{
  nparams = _nparams;
  npts = _npts;
  x = _x;

  param_val = new float[npts];
  param_max = new float[npts];
  param_min = new float[npts];

  // make all matricies the correct size
  if (data == NULL)
  {
    data = new BkgFitLevMarDat();
  }
  data->jac->set_size (npts,nparams);
  data->jtj->set_size (nparams,nparams);
  data->lhs->set_size (nparams,nparams);
  data->rhs->set_size (nparams);
  data->delta->set_size (nparams);

  //err_vect.resize(npts);

  dp = new float[_nparams];
  for (int i=0;i<_nparams;i++)
    dp[i] = 0.001;

  residualWeight = new float[npts];
  wtScale = npts;
  for (int i=0;i<npts;i++)
    residualWeight[i] = 1.0f;

  fval_cache = new float[npts];
  enable_fval_cache = false; // pick a start
  numException = 0;
}

void LevMarFitterV2::SetParamMax (float *_param_max)
{
  //param_max = _param_max;
  for (int i=0; i<nparams; i++)
    param_max[i] = _param_max[i];
}
void LevMarFitterV2::SetParamMin (float *_param_min)
{
  //param_min = _param_min;
  for (int i=0; i<nparams; i++)
    param_min[i] = _param_min[i];
}

void LevMarFitterV2::SetParamVal (float *_param_val)
{
  //param_min = _param_min;
  for (int i=0; i<nparams; i++)
    param_val[i] = _param_val[i];
}

void LevMarFitterV2::SetNthParam(float val, int idx)
{
  if (idx<nparams)
    param_val[idx] = val;
}

float LevMarFitterV2::ReturnNthParam(int idx)
{
  if (idx<nparams)
    return( param_val[idx]);
  else
    return(0.0f);
}
// generic grid search
void LevMarFitterV2::GridSearch (int steps,float *y,float *params)
{
  // if we were never configured with min/max parameter values, just return
  // we only allow grid search if the region in which to search has been defined
  if ( (param_min == NULL) || (param_max == NULL))
    return;

  float param_step[nparams];
  float p[nparams];

  // calculate step size for each parameter
  for (int pn=0;pn < nparams;pn++)
  {
    param_step[pn] = (param_max[pn] - param_min[pn]) / (steps-1);
    p[pn] = param_min[pn];
  }

  int step_num[nparams];
  memset (step_num,0,sizeof (step_num));

  int inum;
  int total_steps;
  int digit;
  float *fval = new float[npts];
  float min_err = FLT_MAX;
  float r1;

  total_steps = 1;
  for (int i=0;i < nparams;i++)
    total_steps *= steps;

  for (inum = 0;inum < total_steps;inum++)
  {
    // convert to baseN (where N==steps).  Each 'digit' of inum in baseN corresponds to the
    // step position of a parameter
    int inum_tmp = inum;
    int place_val = total_steps/steps;
    for (digit=nparams-1;digit >= 0;digit--)
    {
      int val = inum_tmp / place_val;
      inum_tmp -= val * place_val;
      p[digit] = param_min[digit]+param_step[digit]*val;
      place_val /= steps;
    }

    Evaluate (fval,p);
    r1 = CalcResidual (y, fval, npts);

    if (r1 < min_err)
    {
      memcpy (params,p,sizeof (p));
      min_err = r1;
    }
  }

  if (enable_fval_cache)
    memcpy (fval_cache,fval,sizeof (float[npts]));

  delete [] fval;
}

static void LinSolveErrMessage( const char* str )
{
  static int maxMessages = 100; //limit complaining to some number to avoid overflowing the log file
  if( --maxMessages > 0 )
    std::cout << str << "\t:" << maxMessages << std::endl;
}

void LevMarFitterV2::TryLevMarStep (float *y,
                                    float *fval,
                                    float *params,
                                    float *err_vect,
                                    double *bfjtj,
                                    double *bfrhs,
                                    int done_cnt,
                                    float &r_start,
                                    float &r_chg)
{
  float r_trial;
  float tmp[npts];
  float params_new[nparams];
  double bflhs[nparams*nparams];

  bool cont_proc = false;
  if (lambda>lambda_threshold)
    cont_proc = true; // skip, we can't do anything if we've escaped
  while (!cont_proc)
  {
    // add lambda parameter to jtj to form lhs of matrix equation
    memcpy (bflhs,bfjtj,sizeof (bflhs));
    for (int i=0;i < nparams;i++)
      bflhs[i*nparams+i] *= (1.0 + lambda);

    // can actually check against zero here because that is the problem
    if ((nparams==1) & (bflhs[0]==0.0f)) // one entry, it is zero, and our regularizer is zero armadillo does not throw exception but NaN
      regularizer += REGULARIZER_VAL;  // this is a problem with our derivative calculation when we hit the top of the box

    // add regularizer in case we have trouble
    for (int i=0;i < nparams;i++)
      bflhs[i*nparams+i] += regularizer;

    // solve for delta
    try
    {
      {

        // armadillo just as fast for 1 and 2 so no special case required
        // don't forget all our time is spent in other functions - simplifying here lets us make the code better and faster in the
        // actual time-sinks

        for (int r=0;r < nparams;r++)
        {
          data->rhs->at (r) = bfrhs[r];

          for (int c=0;c < nparams;c++)
            data->lhs->at (r,c) = bflhs[r*nparams+c];
        }

        * (data->delta) = solve (* (data->lhs),* (data->rhs));
      }

      bool NaN_detected = false;
      for (int i=0;i < nparams;i++)
      {
        double tmp_eval = data->delta->at(i);

        // test for NaN
        if (tmp_eval != tmp_eval)
        {
          NaN_detected = true;
          data->delta->at (i) = 0.0;
          tmp_eval = 0.0;
        }
        // if tmp_eval out of range for float, it's a disaster all around
        tmp_eval += params[i]; // safe promotion from float to double
        if ((tmp_eval>FLT_MAX) or (tmp_eval<-FLT_MAX)) // no adjust if disaster
        {
          tmp_eval = params[i];
          data->delta->at(i) = 0.0;
        }
        // adjust parameter from current baseline
        params_new[i] = tmp_eval; // demotion from double to float
      }
      // make this virtual in case constraints need to be non-orthogonal boxes
      ApplyMoveConstraints (params_new);
      // apply limits if necessary

      if (!NaN_detected)
      {
        // re-calculate error
        Evaluate (tmp,params_new);

        // calculate error bw function and data
        r_trial = CalcResidual (y, tmp, npts, &err_vect[0]);

        r_chg = r_trial - r_start;
      }
      else{
          char my_message[1024];
          sprintf(my_message,"LevMarFitterV2.cpp: NaN in TryLevMarStep matrix solve - at %f %f %f", lambda, regularizer,params[0]);
          LinSolveErrMessage(my_message);
      }

      if (!NaN_detected && (r_trial < r_start))
      {
        // safe way to check if this is step is feasible given that lambda is a float
        if (lambda> LEVMAR_STEP_V2*FLT_MIN)
          lambda /= LEVMAR_STEP_V2;

        memcpy (params,params_new,sizeof (float[nparams]));
        memcpy (fval,tmp,sizeof (float[npts]));
        cont_proc = true;
        r_start = r_trial;
        if (enable_fval_cache)
          memcpy (fval_cache,fval,sizeof (float[npts]));

      }
      else
      {
        lambda *= LEVMAR_STEP_V2;
      }

      if (debug_trace)
        printf ("lambda = %f, done = %d\n",lambda,done_cnt);

    }
    catch (std::runtime_error &le)
    {
      // a failed solution of the matrix should be treated just like a failed attempt
      // at improving the fit...increase lambda and try again
      if (2.0*regularizer>REGULARIZER_VAL)
      {
        // you're not an exception until you're an exception with a probably nonzero determinant
        numException++;
        LinSolveErrMessage("LevMarFitterV2.cpp: Fit - exception runtime ...");
      }
      data->delta->set_size (nparams);
      data->delta->zeros (nparams);
      lambda *= LEVMAR_STEP_V2;
      regularizer += REGULARIZER_VAL; // keep adding until we're nonzero on the diagonal
    }

    // we need a way to bail out of this loop if lambda continues to increase but we never
    // get a better fit
    if (lambda > lambda_threshold)
      cont_proc = true;

    if (debug_trace)
      printf ("lambda = %f, done = %d\n",lambda,done_cnt);
  }
  // bailed out of loop
}

void LevMarFitterV2::TryGaussNewtonStep(float *y,
                                    float *fval,
                                    float *params,
                                    float *err_vect,
                                    double *bfjtj,
                                    double *bfrhs,
                                    int done_cnt,
                                    float &r_start,
                                    float &r_chg)
{
  float r_trial;
  float tmp[npts];
  float params_new[nparams];

  {

    // can actually check against zero here because that is the problem
    if ((nparams==1) & (bfjtj[0]==0.0f)) // one entry, it is zero, and our regularizer is zero armadillo does not throw exception but NaN
      regularizer += REGULARIZER_VAL;  // this is a problem with our derivative calculation when we hit the top of the box

    // add regularizer in case we have trouble
    for (int i=0;i < nparams;i++)
      bfjtj[i*nparams+i] += regularizer;

    // solve for delta
    try
    {
      for (int r=0;r < nparams;r++)
      {
        data->rhs->at (r) = bfrhs[r];

        for (int c=0;c < nparams;c++)
          data->lhs->at (r,c) = bfjtj[r*nparams+c];
      }

      * (data->delta) = solve (* (data->lhs),* (data->rhs));

      bool NaN_detected = false;
      for (int i=0;i < nparams;i++)
      {
        double tmp_eval = data->delta->at(i);

        // test for NaN
        if (tmp_eval != tmp_eval)
        {
          NaN_detected = true;
          data->delta->at (i) = 0.0;
          tmp_eval = 0.0;
        }
        // if tmp_eval out of range for float, it's a disaster all around
        tmp_eval += params[i]; // safe promotion from float to double
        if ((tmp_eval>FLT_MAX) or (tmp_eval<-FLT_MAX)) // no adjust if disaster
        {
          tmp_eval = params[i];
          data->delta->at(i) = 0.0;
        }
        // adjust parameter from current baseline
        params_new[i] = tmp_eval; // demotion from double to float
      }
      // make this virtual in case constraints need to be non-orthogonal boxes
      ApplyMoveConstraints (params_new);
      // apply limits if necessary

      if (!NaN_detected)
      {
        // re-calculate error
        Evaluate (tmp,params_new);
        // dynamic emphasis
        DetermineAndSetWeightVector(params_new[0]);

        // calculate error bw function and data
        r_trial = CalcResidual (y, tmp, npts, &err_vect[0]);
        r_chg = r_trial - r_start;
      }
      else{
          char my_message[1024];
          sprintf(my_message,"LevMarFitterV2.cpp: NaN in TrygaussNewtonStep matrix solve - at %f %f", regularizer,params[0]);
          LinSolveErrMessage(my_message);
      }

      if (!NaN_detected && (r_trial < r_start))
      {
        memcpy (params,params_new,sizeof (float[nparams]));
        memcpy (fval,tmp,sizeof (float[npts]));
        r_start = r_trial;
        if (enable_fval_cache)
          memcpy (fval_cache,fval,sizeof (float[npts]));
      }
      else {
        DetermineAndSetWeightVector(params[0]);
      }

      if (debug_trace)
        printf ("lambda = %f, done = %d\n",lambda,done_cnt);

    }
    catch (std::runtime_error &le)
    {
      // a failed solution of the matrix should be treated just like a failed attempt
      // at improving the fit...increase lambda and try again
      if (2.0*regularizer>REGULARIZER_VAL)
      {
        // you're not an exception until you're an exception with a probably nonzero determinant
        numException++;
        LinSolveErrMessage("LevMarFitterV2.cpp: Fit - exception runtime ...");
      }
      data->delta->set_size (nparams);
      data->delta->zeros (nparams);
      regularizer += REGULARIZER_VAL; // keep adding until we're nonzero on the diagonal
    }

    if (debug_trace)
      printf ("lambda = %f, done = %d\n",lambda,done_cnt);
  }
}

void LevMarFitterV2::CalculateJTJ (double *bfjtj, float *bfjac)
{
  // calculate jtj matrix
  for (int r=0;r < nparams;r++)
    for (int c=r;c < nparams;c++)
      bfjtj[r*nparams+c] = bfjtj[c*nparams+r] = sdot (npts,&bfjac[r],nparams,&bfjac[c],nparams);

}

void LevMarFitterV2::CalculateRHS (double *bfrhs,
                                   float *bfjac,
                                   float *err_vect,
                                   float *params)
{
  // calculate rhs
  //Blas_Mat_Trans_Vec_Mult(jac,err_vect,rhs,1.0,0.0);
  for (int r=0;r < nparams;r++)
    bfrhs[r] = sdot (npts,&bfjac[r],nparams,&err_vect[0],1);

}
void LevMarFitterV2::SetLambdaThreshold (float _lambda_threshold)
{
  lambda_threshold = _lambda_threshold;
}

void LevMarFitterV2::SetLambdaStart (float _initial_lambda)
{
  // reset the extra variables here, as we're starting a new optimization
  lambda = _initial_lambda;
  regularizer = 0.0f;
  numException = 0;
}

float LevMarFitterV2::GetLambda (void)
{
  return (lambda);
}

void LevMarFitterV2::SetDebugTrace (bool _enable)
{
  debug_trace = _enable;
}

// set the weighting vector
void LevMarFitterV2::SetWeightVector (float *vect)
{
  wtScale = 0.0;
  for (int i=0;i < npts;i++)
  {
    residualWeight[i] = vect[i];
    wtScale += vect[i];
  }
}

// get the mean squared error after the fit
float LevMarFitterV2::GetMeanSquaredError (float *y,bool use_fval_cache)
{
  if (y == NULL)
    return residual;
  else
  {
    float *tmp;
    float tmp_eval[npts];

    if (enable_fval_cache && use_fval_cache)
    {
      tmp = fval_cache;
    }
    else
    {
      Evaluate (tmp_eval);
      tmp = tmp_eval;
    }
    residual = CalcResidual (y, tmp, npts);
    return residual;
  }
}

void LevMarFitterV2::ReturnPredicted(float *f_predict, bool use_fval_cache)
{
  if (f_predict!=NULL)
  {
    if (enable_fval_cache && use_fval_cache)
      memcpy(f_predict,fval_cache, sizeof(float[npts]));
    else
      Evaluate(f_predict);
  }
}

void LevMarFitterV2::SetDataDeltaNull()
{
  (void) data->delta ;
}
double LevMarFitterV2::GetDataDelta (int i)
{
  return data->delta->at (i);
}

LevMarFitterV2::~LevMarFitterV2()
{
  delete [] residualWeight;
  delete [] dp;

  delete [] fval_cache;
  if (data != NULL)
    delete data;
  delete[] param_val;
  delete[] param_max;
  delete[] param_min;
}


#include <robot_body_filter/utils/tf2_sensor_msgs.h>

#include <robot_body_filter/utils/cloud.h>
#include <robot_body_filter/utils/string_utils.hpp>

#include <tf2_eigen/tf2_eigen.h>

#include <Eigen/Geometry>  // needs to be implementation-private as we want -march=native optimizations

#include <unordered_set>

namespace robot_body_filter {

const static std::unordered_map<std::string, CloudChannelType> XYZ_CHANNELS({
  {"", CloudChannelType::POINT},
});

const static std::unordered_map<std::string, CloudChannelType> DEFAULT_CHANNELS({
  {"vp_", CloudChannelType::POINT},
  {"normal_", CloudChannelType::DIRECTION}
});

bool fieldNameMatchesChannel(const std::string& fieldName, const std::string& channelName, CloudChannelType channelType)
{
  if (channelType == CloudChannelType::SCALAR) {
    return fieldName == channelName;
  } else if (channelName.empty()) {
    return fieldName == "x" || fieldName == "y" || fieldName == "z";
  } else {
    return fieldName.length() == channelName.length() + 1 && startsWith(fieldName, channelName) && (
        endsWith(fieldName, "x") || endsWith(fieldName, "y") || endsWith(fieldName, "z"));
  }
}

void transformChannel(const sensor_msgs::PointCloud2& cloudIn, sensor_msgs::PointCloud2& cloudOut,
                      const Eigen::Isometry3f& t, const std::string& channelPrefix, const CloudChannelType type)
{
  if (num_points(cloudIn) == 0)
    return;

  if (type == CloudChannelType::SCALAR)
    return;

  CloudConstIter x_in(cloudIn, channelPrefix + "x");
  CloudConstIter y_in(cloudIn, channelPrefix + "y");
  CloudConstIter z_in(cloudIn, channelPrefix + "z");

  CloudIter x_out(cloudOut, channelPrefix + "x");
  CloudIter y_out(cloudOut, channelPrefix + "y");
  CloudIter z_out(cloudOut, channelPrefix + "z");

  Eigen::Vector3f point;
  // the switch has to be outside the for loop for performance reasons
  switch (type)
  {
    case CloudChannelType::POINT:
      for (; x_in != x_in.end(); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out)
      {
        point = t * Eigen::Vector3f(*x_in, *y_in, *z_in);  // apply the whole transform
        *x_out = point.x();
        *y_out = point.y();
        *z_out = point.z();
      }
      break;
    case CloudChannelType::DIRECTION:
      for (; x_out != x_out.end(); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out)
      {
        point = t.linear() * Eigen::Vector3f(*x_in, *y_in, *z_in);  // apply only rotation
        *x_out = point.x();
        *y_out = point.y();
        *z_out = point.z();
      }
      break;
  }
}

void transformChannel(sensor_msgs::PointCloud2& cloud, const geometry_msgs::Transform& tf,
                      const std::string& channelPrefix, CloudChannelType type)
{
  const auto t = tf2::transformToEigen(tf).cast<float>();
  transformChannel(cloud, cloud, t, channelPrefix, type);
}

sensor_msgs::PointCloud2& transformWithChannels(
    const sensor_msgs::PointCloud2& in, sensor_msgs::PointCloud2& out, const geometry_msgs::TransformStamped& tf)
{
  return transformWithChannels(in, out, tf, DEFAULT_CHANNELS);
}

sensor_msgs::PointCloud2& transformWithChannels(
    const sensor_msgs::PointCloud2& in, sensor_msgs::PointCloud2& out, const geometry_msgs::TransformStamped& tf,
    const std::unordered_map<std::string, CloudChannelType>& channels)
{
  std::unordered_set<std::string> channelsPresent;
  for (const auto& field: in.fields) {
    for (const auto& channelAndType : channels)
    {
      const std::string& channel = channelAndType.first;
      const auto& channelType = channelAndType.second;
      if (channelType != CloudChannelType::SCALAR && fieldNameMatchesChannel(field.name, channel, channelType))
        channelsPresent.insert(channel);
    }
  }

  out = in;
  out.header = tf.header;

  const auto t = tf2::transformToEigen(tf).cast<float>();

  transformChannel(in, out, t, "", CloudChannelType::POINT);
  for (const auto& channel : channelsPresent)
    transformChannel(in, out, t, channel, channels.at(channel));

  return out;
}

sensor_msgs::PointCloud2& transformOnlyChannels(
    const sensor_msgs::PointCloud2& in, sensor_msgs::PointCloud2& out, const geometry_msgs::TransformStamped& tf,
    const std::unordered_map<std::string, CloudChannelType>& channels)
{
  std::unordered_set<std::string> channelsPresent;
  out.point_step = 0;
  for (const auto& field: in.fields) {
    for (const auto& channelAndType : channels)
    {
      const std::string& channel = channelAndType.first;
      const auto& channelType = channelAndType.second;
      if (fieldNameMatchesChannel(field.name, channel, channelType)) {
        channelsPresent.insert(channel);
        out.fields.push_back(field);
        out.fields.back().offset = out.point_step;
        out.point_step += sizeOfPointField(field.datatype);
      }
    }
  }

  out.header = tf.header;
  out.is_dense = in.is_dense;
  out.height = in.height;
  out.width = in.width;
  out.is_bigendian = in.is_bigendian;

  CloudModifier mod(out);
  mod.resize(num_points(in));

  const auto t = tf2::transformToEigen(tf).cast<float>();

  for (const auto& channel : channelsPresent) {
    const auto channelType = channels.at(channel);
    if (channelType != CloudChannelType::SCALAR) {
      transformChannel(in, out, t, channel, channelType);
    } else {
      copyChannelData(in, out, channel);
    }
  }

  return out;
}

sensor_msgs::PointCloud2& transformOnlyXYZ(const sensor_msgs::PointCloud2& in, sensor_msgs::PointCloud2& out,
                                           const geometry_msgs::TransformStamped& tf) {
  return transformOnlyChannels(in, out, tf, XYZ_CHANNELS);
}

sensor_msgs::PointCloud2& transformWithChannels(
    const sensor_msgs::PointCloud2& in, sensor_msgs::PointCloud2& out,
    tf2_ros::Buffer& tfBuffer, const std::string& targetFrame)
{
  return transformWithChannels(in, out, tfBuffer, targetFrame, DEFAULT_CHANNELS);
}

sensor_msgs::PointCloud2& transformWithChannels(
    const sensor_msgs::PointCloud2& in, sensor_msgs::PointCloud2& out,
    tf2_ros::Buffer& tfBuffer, const std::string& targetFrame,
    const std::unordered_map<std::string, CloudChannelType>& channels)
{
  const auto tf = tfBuffer.lookupTransform(targetFrame, in.header.frame_id, in.header.stamp);
  return transformWithChannels(in, out, tf, channels);
}

}


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

#include <dlib/svm.h>
#include <dlib/rand.h>
#include <dlib/statistics.h>

#include "tester.h"

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.svm_c_linear_dcd");

// ----------------------------------------------------------------------------------------

    void test_sparse()
    {
        typedef std::map<unsigned long,double> sample_type;

        typedef sparse_linear_kernel<sample_type> kernel_type;

        svm_c_linear_trainer<kernel_type> linear_trainer_cpa;
        svm_c_linear_dcd_trainer<kernel_type> linear_trainer;

        svm_c_linear_dcd_trainer<kernel_type>::optimizer_state state;

        const double C = 0.2;
        linear_trainer.set_epsilon(1e-10);
        linear_trainer_cpa.set_epsilon(1e-10);

        std::vector<sample_type> samples;
        std::vector<double> labels;

        // make an instance of a sample vector so we can use it below
        sample_type sample;

        decision_function<kernel_type> df, df2, df3;

        dlib::rand rnd;
        // Now lets go into a loop and randomly generate 10000 samples.
        double label = +1;
        for (int i = 0; i < 100; ++i)
        {
            // flip this flag
            label *= -1;

            sample.clear();

            // now make a random sparse sample with at most 10 non-zero elements
            for (int j = 0; j < 5; ++j)
            {
                int idx = rnd.get_random_32bit_number()%10;
                double value = rnd.get_random_double();

                sample[idx] = label*value;
            }

            // Also save the samples we are generating so we can let the svm_c_linear_trainer
            // learn from them below.
            samples.push_back(sample);
            labels.push_back(label);

            if (samples.size() > 1)
            {
                linear_trainer_cpa.set_c_class1(C);
                linear_trainer_cpa.set_c_class2(1.5*C);
                linear_trainer.set_c_class1(C/samples.size());
                linear_trainer.set_c_class2(1.5*C/samples.size());

                df = linear_trainer.train(samples, labels, state);
                df2 = linear_trainer_cpa.train(samples, labels);
                df3 = linear_trainer.train(samples, labels);

                DLIB_TEST_MSG( dlib::distance(df.basis_vectors(0), df2.basis_vectors(0)) < 1e-8, dlib::distance(df.basis_vectors(0), df2.basis_vectors(0)));
                DLIB_TEST( std::abs(df.b - df2.b) < 1e-8);
                DLIB_TEST( dlib::distance(df.basis_vectors(0), df3.basis_vectors(0)) < 1e-8);
                DLIB_TEST( std::abs(df.b - df3.b) < 1e-8);
            }
        }
    }

// ----------------------------------------------------------------------------------------

    void test_normal_no_bias()
    {
        typedef matrix<double,10,1> sample_type;

        typedef linear_kernel<sample_type> kernel_type;

        svm_c_linear_trainer<kernel_type> linear_trainer_cpa;
        svm_c_linear_dcd_trainer<kernel_type> linear_trainer;

        svm_c_linear_dcd_trainer<kernel_type>::optimizer_state state;

        const double C = 1.0;
        linear_trainer.set_epsilon(1e-10);
        linear_trainer_cpa.set_epsilon(1e-10);

        linear_trainer.include_bias(false);

        std::vector<sample_type> samples, samples_explict_bias;
        std::vector<double> labels;

        // make an instance of a sample vector so we can use it below
        sample_type sample;

        decision_function<kernel_type> df, df2, df3;

        dlib::rand rnd;
        // Now lets go into a loop and randomly generate 10000 samples.
        double label = +1;
        for (int i = 0; i < 100; ++i)
        {
            // flip this flag
            label *= -1;

            sample = 0;

            // now make a random sparse sample with at most 10 non-zero elements
            for (int j = 0; j < 5; ++j)
            {
                int idx = rnd.get_random_32bit_number()%9;
                double value = rnd.get_random_double();

                sample(idx) = label*value;
            }

            // Also save the samples we are generating so we can let the svm_c_linear_trainer
            // learn from them below.
            samples.push_back(sample);
            labels.push_back(label);

            sample(9) = -1;
            samples_explict_bias.push_back(sample);

            if (samples.size() > 1)
            {
                linear_trainer_cpa.set_c_class1(C);
                linear_trainer_cpa.set_c_class2(1.5*C);
                linear_trainer.set_c_class1(C/samples.size());
                linear_trainer.set_c_class2(1.5*C/samples.size());

                df = linear_trainer.train(samples_explict_bias, labels, state);
                df2 = linear_trainer_cpa.train(samples, labels);
                df3 = linear_trainer.train(samples_explict_bias, labels);

                DLIB_TEST( std::abs(df2.basis_vectors(0)(9)) < 1e-7);
                DLIB_TEST_MSG( max(abs(colm(df.basis_vectors(0),0,9) - colm(df2.basis_vectors(0),0,9))) < 1e-6, max(abs(colm(df.basis_vectors(0),0,9) - colm(df2.basis_vectors(0),0,9))));
                DLIB_TEST( std::abs(df.basis_vectors(0)(9) - df2.b) < 1e-6);
                DLIB_TEST( max(abs(df.basis_vectors(0) - df3.basis_vectors(0))) < 1e-6);
                DLIB_TEST( std::abs(df.b - df3.b) < 1e-7);
            }
        }
    }

// ----------------------------------------------------------------------------------------

    void test_normal()
    {
        typedef matrix<double,10,1> sample_type;

        typedef linear_kernel<sample_type> kernel_type;

        svm_c_linear_trainer<kernel_type> linear_trainer_cpa;
        svm_c_linear_dcd_trainer<kernel_type> linear_trainer;

        svm_c_linear_dcd_trainer<kernel_type>::optimizer_state state;

        const double C = 1;
        linear_trainer.set_epsilon(1e-10);
        linear_trainer_cpa.set_epsilon(1e-10);

        std::vector<sample_type> samples;
        std::vector<double> labels;

        // make an instance of a sample vector so we can use it below
        sample_type sample;

        decision_function<kernel_type> df, df2, df3;

        dlib::rand rnd;
        // Now lets go into a loop and randomly generate 10000 samples.
        double label = +1;
        for (int i = 0; i < 100; ++i)
        {
            // flip this flag
            label *= -1;

            sample = 0;

            // now make a random sparse sample with at most 10 non-zero elements
            for (int j = 0; j < 5; ++j)
            {
                int idx = rnd.get_random_32bit_number()%10;
                double value = rnd.get_random_double();

                sample(idx) = label*value;
            }

            // Also save the samples we are generating so we can let the svm_c_linear_trainer
            // learn from them below.
            samples.push_back(sample);
            labels.push_back(label);

            if (samples.size() > 1)
            {
                linear_trainer_cpa.set_c_class1(C);
                linear_trainer_cpa.set_c_class2(1.5*C);
                linear_trainer.set_c_class1(C/samples.size());
                linear_trainer.set_c_class2(1.5*C/samples.size());

                df = linear_trainer.train(samples, labels, state);
                df2 = linear_trainer_cpa.train(samples, labels);
                df3 = linear_trainer.train(samples, labels);

                DLIB_TEST_MSG( max(abs(df.basis_vectors(0) - df2.basis_vectors(0))) < 1e-7, max(abs(df.basis_vectors(0) - df2.basis_vectors(0))));
                DLIB_TEST( std::abs(df.b - df2.b) < 1e-7);
                DLIB_TEST( max(abs(df.basis_vectors(0) - df3.basis_vectors(0))) < 1e-7);
                DLIB_TEST( std::abs(df.b - df3.b) < 1e-7);
            }
        }
    }

// ----------------------------------------------------------------------------------------

    void test_normal_force_last_weight(bool have_bias, bool force_weight)
    {
        typedef matrix<double,10,1> sample_type;
        dlog << LINFO << "have_bias: "<< have_bias << "   force_weight: "<< force_weight;

        typedef linear_kernel<sample_type> kernel_type;

        svm_c_linear_trainer<kernel_type> linear_trainer_cpa;

        svm_c_linear_dcd_trainer<kernel_type> linear_trainer;

        svm_c_linear_dcd_trainer<kernel_type>::optimizer_state state;

        const double C = 1;
        linear_trainer.set_epsilon(1e-10);
        linear_trainer_cpa.set_epsilon(1e-11);

        linear_trainer_cpa.force_last_weight_to_1(force_weight);

        linear_trainer.force_last_weight_to_1(force_weight);
        linear_trainer.include_bias(have_bias);

        std::vector<sample_type> samples;
        std::vector<double> labels;

        // make an instance of a sample vector so we can use it below
        sample_type sample;

        decision_function<kernel_type> df, df2;

        running_stats<double> rs;

        dlib::rand rnd;
        // Now lets go into a loop and randomly generate 10000 samples.
        double label = +1;
        for (int i = 0; i < 40; ++i)
        {
            // flip this flag
            label *= -1;

            sample = 0;

            // now make a random sparse sample with at most 10 non-zero elements
            for (int j = 0; j < 5; ++j)
            {
                int idx = rnd.get_random_32bit_number()%9;
                double value = rnd.get_random_double();

                sample(idx) = label*value + label;
            }

            sample(9) = 4;

            // Also save the samples we are generating so we can let the svm_c_linear_trainer
            // learn from them below.
            samples.push_back(sample);
            labels.push_back(label);

            linear_trainer.set_c(C);
            linear_trainer_cpa.set_c(C*samples.size());

            df = linear_trainer.train(samples, labels, state);

            if (force_weight)
            {
                DLIB_TEST(std::abs(df.basis_vectors(0)(9) - 1) < 1e-8);
                DLIB_TEST(std::abs(df.b) < 1e-8);

                if (samples.size() > 1)
                {
                    df2 = linear_trainer_cpa.train(samples, labels);
                    DLIB_TEST_MSG( max(abs(df.basis_vectors(0) - df2.basis_vectors(0))) < 1e-7, max(abs(df.basis_vectors(0) - df2.basis_vectors(0))));
                    DLIB_TEST( std::abs(df.b - df2.b) < 1e-7);
                }
            }

            if (!have_bias)
                DLIB_TEST(std::abs(df.b) < 1e-8);

            for (unsigned long k = 0; k < samples.size(); ++k)
            {
                //cout << "pred: "<< labels[k]*df(samples[k]) << endl;
                rs.add(labels[k]*df(samples[k]));
            }
        }
        DLIB_TEST_MSG(std::abs(rs.min()-1) < 1e-7, std::abs(rs.min()-1));
    }

// ----------------------------------------------------------------------------------------

    void test_normal_1_sample(double label)
    {
        typedef matrix<double,10,1> sample_type;

        typedef linear_kernel<sample_type> kernel_type;

        svm_c_linear_dcd_trainer<kernel_type> linear_trainer;

        svm_c_linear_dcd_trainer<kernel_type>::optimizer_state state;

        const double C = 10;
        linear_trainer.set_epsilon(1e-10);
        linear_trainer.set_c(C);

        linear_trainer.force_last_weight_to_1(true);
        linear_trainer.include_bias(false);

        std::vector<sample_type> samples;
        std::vector<double> labels;

        // make an instance of a sample vector so we can use it below
        sample_type sample;

        sample = 0;
        sample(0) = -1;
        sample(1) = -1;
        sample(9) = 4;

        samples.push_back(sample);
        labels.push_back(label);

        for (int i = 0; i < 4; ++i)
        {
            decision_function<kernel_type> df;
            df = linear_trainer.train(samples, labels);

            if (label > 0)
            {
                DLIB_TEST(std::abs(df(samples[0])-4) < 1e-8);
            }
            else
            {
                DLIB_TEST(std::abs(df(samples[0])+1) < 1e-8);
            }
        }
    }

// ----------------------------------------------------------------------------------------

    void test_sparse_1_sample(double label)
    {
        typedef std::vector<std::pair<unsigned long,double> > sample_type;

        typedef sparse_linear_kernel<sample_type> kernel_type;

        svm_c_linear_dcd_trainer<kernel_type> linear_trainer;

        svm_c_linear_dcd_trainer<kernel_type>::optimizer_state state;

        const double C = 10;
        linear_trainer.set_epsilon(1e-10);
        linear_trainer.set_c(C);

        linear_trainer.force_last_weight_to_1(true);
        linear_trainer.include_bias(false);

        std::vector<sample_type> samples;
        std::vector<double> labels;

        // make an instance of a sample vector so we can use it below
        sample_type sample;

        sample.push_back(make_pair(0,-1));
        sample.push_back(make_pair(1,1));
        sample.push_back(make_pair(9,4));

        for (int i = 0; i < 4; ++i)
        {
            samples.push_back(sample);
            labels.push_back(label);

            decision_function<kernel_type> df;
            df = linear_trainer.train(samples, labels);

            if (label > 0)
            {
                DLIB_TEST(std::abs(df(samples[0])-4) < 1e-8);
            }
            else
            {
                DLIB_TEST(std::abs(df(samples[0])+1) < 1e-8);
            }
        }
    }

// ----------------------------------------------------------------------------------------

    class tester_svm_c_linear_dcd : public tester
    {
    public:
        tester_svm_c_linear_dcd (
        ) :
            tester ("test_svm_c_linear_dcd",
                "Runs tests on the svm_c_linear_dcd_trainer.")
        {}

        void perform_test (
        )
        {
            test_normal();
            print_spinner();
            test_normal_no_bias();
            print_spinner();
            test_sparse();
            print_spinner();
            test_normal_force_last_weight(false,false);
            print_spinner();
            test_normal_force_last_weight(false,true);
            print_spinner();
            test_normal_force_last_weight(true,false);
            print_spinner();
            test_normal_force_last_weight(true,true);
            print_spinner();
            test_normal_1_sample(+1);
            print_spinner();
            test_normal_1_sample(-1);
            print_spinner();
            test_sparse_1_sample(+1);
            print_spinner();
            test_sparse_1_sample(-1);
            print_spinner();
        }
    } a;

}


///////////////////////////////////////////////////////////////////////////////
// statistics::model::functional::log_prior_evaluator.hpp                    //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_MODEL_FUNCTIONAL_LOG_PRIOR_EVALUATOR_HPP_ER_2009
#define BOOST_MODEL_FUNCTIONAL_LOG_PRIOR_EVALUATOR_HPP_ER_2009
#include <boost/range.hpp>
#include <boost/statistics/model/wrap/unary/prior.hpp>

namespace boost{
namespace statistics{
namespace model{

    // Evaluates the log-likelihood at a parameter values given a model and
    // a dataset.
    //
    // T is the result_type
    template<typename T,typename D>
    class log_prior_evaluator{
        public:
        typedef T result_type;
        typedef prior_wrapper<D> prior_wrapper_type;

        // Constructor
        log_prior_evaluator();
        log_prior_evaluator(const prior_wrapper_type&);
        log_prior_evaluator(const log_prior_evaluator&);
        log_prior_evaluator& operator=(const log_prior_evaluator&);

        // Evaluate
        template<typename P> result_type operator()(const P& p)const;

        // Access
        const prior_wrapper_type& prior_wrapper()const;

        private:
        prior_wrapper_type p_;
    };

    // Implementation //

    // Construction
    template<typename T,typename D>
    log_prior_evaluator<T,D>::log_prior_evaluator(){}

    template<typename T,typename D>
    log_prior_evaluator<T,D>::log_prior_evaluator(
        const prior_wrapper_type& p
    ):p_(p){}

    template<typename T,typename D>
    log_prior_evaluator<T,D>::log_prior_evaluator(
        const log_prior_evaluator& that
    ):p_(that.p_){}

    template<typename T,typename D>
    log_prior_evaluator<T,D>&
    log_prior_evaluator<T,D>::operator=(
        const log_prior_evaluator& that
    ){
        if(&that!=this){
            p_ = (that.p_);
        }
        return (*this);
    }

    // Evaluate
    template<typename T,typename D>
    template<typename P>
    typename log_prior_evaluator<T,D>::result_type
    log_prior_evaluator<T,D>::operator()(const P& p)const{
        return log_unnormalized_pdf(
            (this->prior_wrapper()).prior(),
            p
        );
    }

    // Access
    template<typename T,typename D>
    const typename log_prior_evaluator<T,D>::prior_wrapper_type&
    log_prior_evaluator<T,D>::prior_wrapper()const{
        return (this->p_);
    }

}// model
}// statistics
}// boost

#endif


#pragma once
#ifndef BE_TESTING_BENCHMARK_HPP_
#define BE_TESTING_BENCHMARK_HPP_

#include <be/core/time.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/kurtosis.hpp>
#include <boost/accumulators/statistics/skewness.hpp>

namespace be::testing {
namespace acc = boost::accumulators;

///////////////////////////////////////////////////////////////////////////////
class BenchmarkStats : NonCopyable {
   using results_container = std::vector<F64>;
   using stats_container = acc::accumulator_set<F64, acc::stats<
      acc::tag::count,
      acc::tag::mean,
      acc::tag::min,
      acc::tag::max,
      acc::tag::lazy_variance,
      acc::tag::kurtosis,
      acc::tag::skewness
      >>;
public:
   void run_sample() {
      run_sample_();
   }

   const results_container& results() const {
      return results_;
   }

   U64 samples() const {
      return acc::count(stats_);
   }

   F64 mean() const {
      return acc::mean(stats_);
   }

   F64 min() const {
      return acc::min(stats_);
   }

   F64 max() const {
      return acc::max(stats_);
   }

   F64 variance() const {
      return acc::variance(stats_);
   }

   F64 std_dev() const {
      U64 n = samples();
      return std::sqrt(variance() * n / (n - 1));
   }

   F64 kurtosis() const {
      return acc::kurtosis(stats_);
   }

   F64 skewness() const {
      return acc::skewness(stats_);
   }

protected:
   BenchmarkStats(BenchmarkStats&& other)
      : results_(std::move(other.results_)),
        stats_(std::move(other.stats_))
   { }
   virtual ~BenchmarkStats() { }
   virtual void run_sample_() = 0;

   void swap_stats_(BenchmarkStats& other) {
      using std::swap;
      swap(results_, other.results_);
      swap(stats_, other.stats_);
   }

   results_container results_;
   stats_container stats_;
};

///////////////////////////////////////////////////////////////////////////////
template <typename Func>
class Benchmark final : private Func, BenchmarkStats {
   friend void swap(Benchmark<Func>& a, Benchmark<Func>& b) { a.swap_(b); }
public:
   Benchmark(Func func, bool save_all_samples = false)
      : Func(std::move(func)),
        save_all_(save_all_samples)
   { }

   Benchmark(Benchmark<Func>&& other)
      : Func(std::move(other)),
        BenchmarkStats(std::move(other)),
        save_all_(other.save_all_)
   { }

   Benchmark<Func>& operator=(Benchmark<Func> other) {
      swap_(other);
   }

   void operator()() {
      U64 start = perf_now();
      static_cast<Func&>(*this)();
      U64 end = perf_now();
      F64 result = perf_delta_to_seconds(end - start);
      if (save_all_) {
         results_.push_back(result);
      }
      stats_(result);
   }

private:
   virtual void run_sample_() {
      (*this)();
   }

   void swap_(Benchmark<Func>& other) {
      using std::swap;
      swap_stats_(other);
      swap(static_cast<Func&>(*this), static_cast<Func&>(other));
      swap(save_all_, other.save_all_);
   }

   bool save_all_;
};

} // be::testing

#endif


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward 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
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_SWAR_FUNCTION_SIMD_COMMON_COMPARATOR_HPP_INCLUDED
#define NT2_TOOLBOX_SWAR_FUNCTION_SIMD_COMMON_COMPARATOR_HPP_INCLUDED
#include <boost/mpl/vector.hpp>
#include <boost/fusion/tuple.hpp>
#include <nt2/sdk/meta/strip.hpp>
#include <nt2/include/functions/min.hpp>
#include <nt2/include/functions/max.hpp>
#include <nt2/include/functions/any.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::comparator_, tag::cpu_,
                             (A0)(A2)(X),
                             ((simd_<arithmetic_<A0>,X>))
                             ((simd_<arithmetic_<A0>,X>))
                             ((integer_<A2>))
                            );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::comparator_(tag::simd_<tag::arithmetic_, X> ,
                               tag::simd_<tag::arithmetic_, X> ,
                               tag::integer_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A2>
    struct result<This(A0,A0,A2)>
    {
      typedef typename meta::strip<A0>::type                       nA0;
      typedef boost::fusion::vector<nA0,nA0,int>                   type;
    };

    NT2_FUNCTOR_CALL(3)
    {
      typedef typename NT2_RETURN_TYPE(3)::type r_t;
      r_t res;
      if (a2)
	{
	  boost::fusion::at_c<1>(res) =  nt2::min(a0, a1);
	  boost::fusion::at_c<0>(res) =  nt2::max(a0, a1);
	}
      else
	{
	  boost::fusion::at_c<0>(res) =  nt2::min(a0, a1);
	  boost::fusion::at_c<1>(res) =  nt2::max(a0, a1);
	}
      boost::fusion::at_c<2>(res) = nt2::any(a0-boost::fusion::at_c<0>(res));
      return res;
    }

  };
} }

#endif
// modified by jt the 05/01/2011


#include <boost/numeric/odeint/stepper/rosenbrock4_dense_output.hpp>


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2014   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
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_LAPACK_MCSNE_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_LAPACK_MCSNE_HPP_INCLUDED

#include <nt2/linalg/functions/mcsne.hpp>
#include <nt2/include/functions/lange.hpp>
#include <nt2/include/functions/width.hpp>
#include <nt2/include/functions/height.hpp>
#include <nt2/include/functions/of_size.hpp>
#include <nt2/include/functions/qr.hpp>
#include <nt2/include/functions/sqrt.hpp>
#include <nt2/include/functions/colon.hpp>
#include <nt2/include/functions/linsolve.hpp>
#include <nt2/include/functions/mtimes.hpp>
#include <nt2/include/functions/maximum.hpp>
#include <nt2/include/functions/abs.hpp>
#include <nt2/include/functions/cast.hpp>
#include <nt2/linalg/details/utility/f77_wrapper.hpp>
#include <nt2/include/constants/eps.hpp>
#include <boost/dispatch/meta/downgrade.hpp>
#include <nt2/sdk/meta/as_real.hpp>
#include <boost/dispatch/meta/terminal_of.hpp>
#include <nt2/core/container/table/table.hpp>

namespace nt2 { namespace ext
{
  template<class Domain, int N, class Expr>
  struct  size_of<tag::mcsne_,Domain,N,Expr>
        : meta::size_as<Expr,1>
  {};

  template<class Domain, int N, class Expr>
  struct  value_type<tag::mcsne_,Domain,N,Expr>
        : meta::value_as<Expr,1>
  {};
} }

namespace nt2{ namespace ext
{

  BOOST_DISPATCH_IMPLEMENT  ( mcsne_, tag::cpu_
                            , (A0)(A1)
                            , ((ast_<A0, nt2::container::domain>))
                              ((ast_<A1, nt2::container::domain>))
                            )
  {
    BOOST_DISPATCH_RETURNS(2, (A0 const& a0, A0 const& a1),
                           (boost::proto::
                            make_expr<nt2::tag::mcsne_, container::domain>
                            ( boost::cref(a0), boost::cref(a1) )
                           )
                          )
  };

  BOOST_DISPATCH_IMPLEMENT  ( run_assign_, tag::cpu_
                            , (A0)(A1)(N)
                            , ((ast_<A0, nt2::container::domain>))
                              ((node_<A1,nt2::tag::mcsne_,N,nt2::container::domain>))
                            )
  {
    typedef typename A1::proto_child1::proto_child0::value_type ctype_t;
    typedef typename nt2::meta::as_real<ctype_t>::type   type_t;
    typedef typename boost::dispatch::meta::downgrade<ctype_t>::type dtype;
    typedef typename meta::option<typename  A1::proto_child1::proto_child0::settings_type,nt2::tag::shape_>::type shape;
    typedef nt2::memory::container<tag::table_, ctype_t, nt2::settings(nt2::_2D)> desired_semantic;
    typedef nt2::memory::container<tag::table_, ctype_t, nt2::settings(nt2::_2D,shape)> desired_semantic1;
    typedef A0&                                                          result_type;

     result_type operator()(A0& x, A1& a1) const
    {
      NT2_AS_TERMINAL_IN(desired_semantic1,a,boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_IN(desired_semantic,b,boost::proto::child_c<1>(a1));

      nt2_la_int lda = a.leading_size();
      nt2_la_int na = nt2::width(a);
      nt2_la_int nb = nt2::width(b);
      x.resize(nt2::of_size(na,nb));

      double eps = boost::simd::Eps<double>();
      double anrm = nt2::lange( boost::proto::value(a),'I');
      double cte = anrm*eps*nt2::sqrt( double(na));
      double xnrm, rnrm;

      nt2::container::table<dtype> sa,sx;
      nt2::container::table<ctype_t> e;
      nt2::container::table<dtype,nt2::upper_triangular_> sr,sr1(nt2::of_size(na,na));
      sa = nt2::cast<dtype>(a);
      sr = nt2::qr(sa);

      size_t size_n = sizeof(dtype)*(na);

      for(nt2_la_int i=0; i<na; i++)
      {
        std::memcpy(sr1.data()+i*na, sr.data()+i*lda, size_n);
      }

      boost::proto::value(sr).swap(boost::proto::value(sr1) );

      for(nt2_la_int iterb = 1 ; iterb <= nb ; iterb++)
      {
        sx= nt2::mtimes(nt2::trans(sa),nt2::cast<dtype>(b( _ ,iterb) ));

        sx = nt2::linsolve(nt2::trans(sr),sx);
        sx = nt2::linsolve(sr,sx);

        x( _(1,na) ,iterb) = nt2::cast<ctype_t>(sx);

        e = b( _ ,iterb) - nt2::mtimes(a,x( _(1,na) ,iterb));

        for( size_t i = 1; i<=10;++i)
        {
          sx = nt2::cast<dtype> (nt2::mtimes(nt2::trans(a),e) ) ;

          sx = nt2::linsolve(nt2::trans(sr),sx);
          sx = nt2::linsolve(sr,sx);

          e = nt2::cast<ctype_t>(sx);
          rnrm = nt2::maximum(nt2::abs(e(_)));
          x( _(1,na) ,iterb) = x( _(1,na) ,iterb) + e;
          e = b( _ ,iterb) - nt2::mtimes(a,x( _(1,na) ,iterb) );

          xnrm = nt2::maximum(nt2::abs(x( _(1,na) ,iterb) ));

          if(rnrm < xnrm*cte) { break; }
        }
      }
      return x;
    }
  };

} }

#endif


#include "GaussianProcess.hpp"
#include "math.h"
#include <iostream>
#include <iomanip>
#include <cassert>
#include <boost/random.hpp>

//--------------------------------------------------------------------------------
GaussianProcess::GaussianProcess ( int n, double &delta_input, BlackBoxBaseClass* blackbox_input) :
  TriangularMatrixOperations ( n ),
  dim( n )
{
  lb.reserve( n+1 );
  ub.reserve( n+1 );
  gp_parameters.reserve(n+1);
  for (int i = 0; i < n+1; i++) {
    lb.push_back( 0e0  );
    ub.push_back( 10e0 );
    gp_parameters.push_back( 1e0 );
  }
  nb_gp_nodes = 0;
  gp_pointer = NULL;
  delta = &delta_input;
  blackbox = blackbox_input;
}
//--------------------------------------------------------------------------------

//--------------------------------------------------------------------------------
GaussianProcess::GaussianProcess ( int n, double &delta_input, BlackBoxBaseClass* blackbox_input, std::vector<double> gp_parameters_input ) :
  TriangularMatrixOperations ( n ),
  dim( n )
{
  if(gp_parameters_input.size() != n+1){
    std::cout << "gp_parameters_input wrong size! Should be: " << n+1 << ", is: " << gp_parameters_input.size() << std::endl;
    exit(-1);
  }

  lb.reserve( n+1 );
  ub.reserve( n+1 );
  gp_parameters.reserve(n+1);
  for (int i = 0; i < n+1; i++) {
    lb.push_back( 0e0  );
    ub.push_back( 10e0 );
    gp_parameters.push_back( gp_parameters_input[i] );
  }
  nb_gp_nodes = 0;
  gp_pointer = NULL;
  delta = &delta_input;
  blackbox = blackbox_input;
}
//--------------------------------------------------------------------------------

//--------------------------------------------------------------------------------
double GaussianProcess::evaluate_kernel ( std::vector<double> const &x,
                                          std::vector<double> const &y )
{
  return evaluate_kernel ( x, y, gp_parameters );
/*
  dist = 0e0;
  for ( int i = 0; i < dim; ++i )
    dist += pow( (x.at(i) - y.at(i)), 2e0) /  gp_parameters.at( i+1 );
  kernel_evaluation = exp(-dist / 2e0 );

  return kernel_evaluation * gp_parameters.at( 0 ) ;
*/
}
//--------------------------------------------------------------------------------
//--------------------------------------------------------------------------------
double GaussianProcess::evaluate_kernel ( std::vector<double> const &x,
                                          std::vector<double> const &y,
                                          std::vector<double> const &p )
{
  dist = 0e0;
  for ( int i = 0; i < dim; ++i )
    dist += pow( (x.at(i) - y.at(i)), 2e0) /  p.at( i+1 );
  kernel_evaluation = exp(-dist / 2e0 );

  return kernel_evaluation * p.at( 0 ) ;
}
//--------------------------------------------------------------------------------

//--------------------------------------------------------------------------------
double GaussianProcess::d_evaluate_kernel ( std::vector<double> const &x,
                                            std::vector<double> const &y,
                                            std::vector<double> const &p, int j )
{
  dist = 0e0;
  for ( int i = 0; i < dim; ++i )
    dist += pow( (x.at(i) - y.at(i)), 2e0) /  p.at(i+1);
  kernel_evaluation = exp(-dist / 2e0 );
  if ( j == 0 ) {
    return kernel_evaluation;
  } else {
    kernel_evaluation *= 2e0*pow( (x.at(j-1) - y.at(j-1)), 2e0) / pow( p.at(j-1), 2e0);
    return kernel_evaluation * p.at(0) ;
  }

}
//--------------------------------------------------------------------------------

bool GaussianProcess::test_for_parameter_estimation(const int& nb_values,
                                                const int& update_interval_length,
                                                const int& next_update,
                                                const std::vector<int>& update_at_evaluations){
  bool do_parameter_estimation = false;

  if ( nb_values >= next_update && update_interval_length > 0 ) {
    do_parameter_estimation = true;
    return do_parameter_estimation;
  }
  if ( update_at_evaluations.size( ) > 0 ) {
    if ( nb_values >= update_at_evaluations[0] ) {
      do_parameter_estimation = true;
    }
  }

  return do_parameter_estimation;
}

//--------------------------------------------------------------------------------
void GaussianProcess::build ( std::vector< std::vector<double> > const &nodes,
                              std::vector<double> const &values,
                              std::vector<double> const &noise )
{
    //std::cout << "GP build [" << nodes.size() << "]" << std::endl;
    //std::cout << "With Parameters: " << std::endl;
    //for ( int i = 0; i < dim+1; ++i )
    // std::cout << "gp_param = " << gp_parameters[i] << std::endl;
    //std::cout << std::endl;

    nb_gp_nodes = nodes.size();
    gp_nodes.clear();
    //gp_noise.clear();
    for ( int i = 0; i < nb_gp_nodes; ++i ) {
      gp_nodes.push_back ( nodes.at(i) );
      //gp_noise.push_back( noise.at(i) );
    }

//    auto minmax = std::minmax_element(values.begin(), values.end());
//    min_function_value = values.at((minmax.first - values.begin()));
//    max_function_value = values.at((minmax.second - values.begin()));

//    std::cout << "[ " << min_function_value << ", " << max_function_value << " ]" << std::endl;

    L.clear();
    L.resize( nb_gp_nodes );

    for (int i = 0; i < nb_gp_nodes; i++) {
      for (int j = 0; j <= i; j++){
        L.at(i).push_back (evaluate_kernel( gp_nodes[i], gp_nodes[j] ) );
      }
      L.at(i).at(i) += pow( noise.at(i) / 2e0 + noise_regularization, 2e0 );
    }

    /*
    for (int i = 0; i < nb_gp_nodes; i++) {
      for (int j = 0; j <=i; j++){
        std::cout << std::setprecision(16) << L[i][j] << ' ';
      }
      for (int j = i+1; j < nb_gp_nodes; j++){
        std::cout << std::setprecision(16) << L[j][i] << ' ';
      }

      std::cout << ';' << std::endl;
    }
    */

    CholeskyFactorization::compute( L, pos, rho, nb_gp_nodes );
    assert( pos == 0 );

    scaled_function_values.resize(nb_gp_nodes);
    for (int i = 0; i < nb_gp_nodes; i++) {
      scaled_function_values.at(i) = values.at(i);
//      scaled_function_values.at(i) = values.at(i) - min_function_value;
//      scaled_function_values.at(i) /= 5e-1*( max_function_value-min_function_value );
//      scaled_function_values.at(i) -= 1e0;
    }

    alpha = scaled_function_values;
    forward_substitution( L, alpha );
    backward_substitution( L, alpha );

    return;
}
//--------------------------------------------------------------------------------

//--------------------------------------------------------------------------------
void GaussianProcess::update ( std::vector<double> const &x,
                               double &value,
                               double &noise )
{
  //std::cout << "GP update [" << nb_gp_nodes+1 << "]" << std::endl;
  K0.resize( nb_gp_nodes );
  nb_gp_nodes += 1;
  gp_nodes.push_back( x );
  //gp_noise.push_back( noise );
  scaled_function_values.push_back ( value );
//  scaled_function_values.push_back ( ( value -  min_function_value ) /
//                                     ( 5e-1*( max_function_value-min_function_value ) ) - 1e0 );

  for (int i = 0; i < nb_gp_nodes-1; i++)
    K0.at(i) = evaluate_kernel( gp_nodes[i], x );

  forward_substitution( L, K0 );

  L.push_back ( K0 );

  L.at(L.size()-1).push_back(
    sqrt( evaluate_kernel( x, x ) + pow( noise / 2e0 + noise_regularization, 2e0 ) -
          VectorOperations::dot_product(K0, K0) ) );

  alpha = scaled_function_values;
  forward_substitution( L, alpha );
  backward_substitution( L, alpha );

  return;
}
//--------------------------------------------------------------------------------

//--------------------------------------------------------------------------------
void GaussianProcess::evaluate ( std::vector<double> const &x,
                                 double &mean, double &variance )
{
  K0.resize( nb_gp_nodes );

  for (int i = 0; i < nb_gp_nodes; i++)
    K0.at(i) = evaluate_kernel( gp_nodes[i], x );

  mean = VectorOperations::dot_product(K0, alpha);
//  mean = VectorOperations::dot_product(K0, alpha) + 1e0;
//  mean *= 5e-1*( max_function_value-min_function_value );
//  mean += min_function_value;

  forward_substitution( L, K0 );

  variance = evaluate_kernel( x, x ) - VectorOperations::dot_product(K0, K0);

  //std::cout << "GP evalute [" << gp_nodes.size() <<"] mean,variance " << mean << ", " << variance << std::endl;
  return;
}
//--------------------------------------------------------------------------------

//--------------------------------------------------------------------------------
void GaussianProcess::evaluate ( std::vector<double> const &x,
                                 double &mean)
{
  K0.resize( nb_gp_nodes );

  for (int i = 0; i < nb_gp_nodes; i++)
    K0.at(i) = evaluate_kernel( gp_nodes[i], x );

  mean = VectorOperations::dot_product(K0, alpha);
//  mean = VectorOperations::dot_product(K0, alpha) + 1e0;
//  mean *= 5e-1*( max_function_value-min_function_value );
//  mean += min_function_value;

  //std::cout << "GP evalute [" << gp_nodes.size() <<"] mean,variance " << mean << ", " << variance << std::endl;
  return;
}
//--------------------------------------------------------------------------------

//--------------------------------------------------------------------------------
void GaussianProcess::evaluate ( std::vector<double> const &x,
                                 std::vector<double> const &f_train,
                                 double &mean)
{
  assert(f_train.size() == nb_gp_nodes);

  K0.resize( nb_gp_nodes );

  for (int i = 0; i < nb_gp_nodes; i++)
    K0.at(i) = evaluate_kernel( gp_nodes[i], x );

  std::vector<double> local_f_train;
  local_f_train.resize(nb_gp_nodes);
  for (int i = 0; i < nb_gp_nodes; i++) {
    local_f_train[i] = f_train[i];
  }

  forward_substitution( L, local_f_train );
  backward_substitution( L, local_f_train );

  mean = VectorOperations::dot_product(K0, local_f_train);

  return;
}
//--------------------------------------------------------------------------------

//--------------------------------------------------------------------------------
void GaussianProcess::build_inverse ()
{
  L_inverse.resize(nb_gp_nodes);
  for (int i = 0; i < nb_gp_nodes; i++){
    L_inverse[i].resize(nb_gp_nodes);
    for (int j = 0; j < nb_gp_nodes; j++){
      if(i==j)
        L_inverse[i][j] = 1;
      else
        L_inverse[i][j] = 0;
    }
  }
  for (int i = 0; i < nb_gp_nodes; i++){
    forward_substitution( L, L_inverse[i]);
  }
  for (int i = 0; i < nb_gp_nodes; i++){
    backward_substitution( L, L_inverse[i]);
  }
  return;
}
//--------------------------------------------------------------------------------

//--------------------------------------------------------------------------------
double GaussianProcess::compute_var_meanGP ( std::vector<double>const& xstar, std::vector<double> const& noise)
{
  std::vector<double> k_xstar_X(nb_gp_nodes);
  std::vector<double> k_xstar_X_Kinv(nb_gp_nodes);
  std::vector<double> k_xstar_X_Kinv_squared(nb_gp_nodes);
  std::vector<double> gp_noise_squared(nb_gp_nodes);
  double var_meanGP = 0;

  for(int i = 0; i < nb_gp_nodes; ++i){
    k_xstar_X[i] = evaluate_kernel(xstar, gp_nodes[i]);
  }

  VectorOperations::vec_mat_product(L_inverse, k_xstar_X, k_xstar_X_Kinv);

  for(int i = 0; i < nb_gp_nodes; ++i){
    k_xstar_X_Kinv_squared[i] = k_xstar_X_Kinv[i]*k_xstar_X_Kinv[i];
    gp_noise_squared[i] = noise[i]*noise[i];
  }

  var_meanGP = VectorOperations::dot_product(k_xstar_X_Kinv_squared, gp_noise_squared);

  return var_meanGP;
}
//--------------------------------------------------------------------------------

//--------------------------------------------------------------------------------
double GaussianProcess::compute_cov_meanGPMC ( std::vector<double>const& xstar, int const& xstar_idx, double const& noise)
{
  //std::vector<double> k_xstar_X(nb_gp_nodes);
  double cov_meanGPMC = 0.;
  double cov_term = 0.;
  double L_inverse_term = 0.;
  double kernel_term = 0.;
  //std::cout << "Cov terms (idx, kernel, KK_inv, Prod): " << std::endl;
  assert(xstar_idx < nb_gp_nodes);
  for(int i = 0; i < nb_gp_nodes; ++i){
    //k_xstar_X[i] = evaluate_kernel(xstar, gp_nodes[i]);
    L_inverse_term = L_inverse[i][xstar_idx];
    kernel_term = evaluate_kernel(xstar, gp_nodes[i]);
    cov_term = kernel_term*L_inverse_term;
    cov_meanGPMC += cov_term;
    //std::cout << "(" << xstar_idx << ", "<< kernel_term << ", " << L_inverse_term << ", " << cov_term << ")" << std::endl;
  }
  cov_meanGPMC *= noise*noise;
  return cov_meanGPMC;
}
//--------------------------------------------------------------------------------

//--------------------------------------------------------------------------------
double GaussianProcess::bootstrap_diffGPMC ( std::vector<double>const& xstar, std::vector<std::vector<double>>const& samples, const unsigned int index, int max_bootstrap_samples, int inp_seed)
{
  double mean_xstar = 0.0;
  double mean_bootstrap = 0.0;
  double mean_bootstrap_eval = 0.0;
  double bootstrap_estimate = 0.0;
  int random_idx = -1;
  std::vector<double> f_train_bootstrap(nb_gp_nodes);
  std::vector<double> k_xstar_X_Kinv(nb_gp_nodes);
  std::vector<double> k_xstar_X(nb_gp_nodes);
  unsigned int nb_samples = samples[0].size();
  std::vector<double> bootstrap_samples;
  boost::random::uniform_int_distribution<> distr(0, nb_samples - 1);

  std::random_device rd; // obtain a random number from hardware
  if(inp_seed == -1) {
    inp_seed = rd();
  }
  //std::mt19937_64 bootstrap_eng(inp_seed);
  boost::random::mt19937 bootstrap_eng(inp_seed);

  for(int i = 0; i < nb_gp_nodes; ++i){
    k_xstar_X[i] = evaluate_kernel(xstar, gp_nodes[i]);
  }

  vec_mat_product(L_inverse, k_xstar_X, k_xstar_X_Kinv); //k_xstar_X (K_XX - sigma^2 I)^{-1}
  assert(samples.size() == nb_gp_nodes);

  bootstrap_samples.resize(nb_samples);
  //#pragma omp parallel for private(bootstrap_samples, eng) reduction(+ : mean_bootstrap)
  for(int i = 0; i < max_bootstrap_samples; ++i){
    for(int j = 0; j < nb_gp_nodes; ++j){ //sample randomly from training data
      for(int k = 0; k < nb_samples; ++k){
        bootstrap_samples[k] = samples[j][distr(bootstrap_eng)];
      }
      f_train_bootstrap[j] = blackbox->evaluate_samples(bootstrap_samples, index, xstar);
    }
    mean_bootstrap += dot_product(k_xstar_X_Kinv, f_train_bootstrap); // k_xstar_X (K_XX - sigma^2 I)^{-1} f_train_bootstrap
  }
  mean_bootstrap /= max_bootstrap_samples;
  evaluate(xstar, mean_xstar);
  bootstrap_estimate = mean_bootstrap - mean_xstar;

  return bootstrap_estimate;
}
//--------------------------------------------------------------------------------

//--------------------------------------------------------------------------------
void GaussianProcess::estimate_hyper_parameters ( std::vector< std::vector<double> > const &nodes,
                                                  std::vector<double> const &values,
                                                  std::vector<double> const &noise )
{
  //std::cout << "GP Estimator" << std::endl;
  nb_gp_nodes = nodes.size();
  gp_nodes.clear();
  gp_noise.clear();
  for ( int i = 0; i < nb_gp_nodes; ++i ) {
    gp_nodes.push_back ( nodes.at(i) );
    gp_noise.push_back ( noise.at(i) );
  }

  gp_pointer = this;

//  auto minmax = std::minmax_element(values.begin(), values.end());
//  min_function_value = values.at((minmax.first - values.begin()));
//  max_function_value = values.at((minmax.second - values.begin()));
  /*
  auto minmax = std::minmax_element(values.begin(), values.end());
  min_function_value = values.at((minmax.first - values.begin()));
  max_function_value = fabs(values.at((minmax.second - values.begin())));
  if ( fabs(min_function_value) > max_function_value )
    max_function_value = fabs( min_function_value );
  */
  L.clear();
  L.resize( nb_gp_nodes );
  for ( int i = 0; i < nb_gp_nodes; ++i)
    L.at(i).resize( i+1 );

  scaled_function_values.resize(nb_gp_nodes);
  for ( int i = 0; i < nb_gp_nodes; ++i) {
    scaled_function_values.at(i) = values.at(i);
//    scaled_function_values.at(i) = values.at(i) - min_function_value;
//    scaled_function_values.at(i) /= 5e-1*( max_function_value-min_function_value );
//    scaled_function_values.at(i) -= 1e0;
  }

  double optval;
  //adjust those settings to optimize GP approximation
  //--------------------------------------------------
//  double max_noise = 0e0;
//  for (int i = 0; i < nb_gp_nodes; i++) {
//    if (gp_noise.at( i ) > max_noise)
//      max_noise = gp_noise.at( i );
//  }

  //My Version
  /*lb[0] = 1e-3;
      ub[0] = 1e3;
      lb[0] = max_function_value - 1e2;
      if ( lb[0] < 1e-3 ) lb[0] = 1e-3;
      ub[0] = max_function_value + 1e2;
      if ( ub[0] > 1e3 ) ub[0] = 1e3;
      if ( ub[0] <= lb[0]) lb[0] = 1e-3;
  double delta_threshold = *delta;
  if (delta_threshold < 1e-2) delta_threshold = 1e-2;
  for (int i = 0; i < dim; ++i) {
      lb[i+1] = 1e-2 * delta_threshold; // 1e1
      ub[i+1] = 2.0 * delta_threshold; // 1e2
  }*/

  //Florians old version1:
  /*lb[0] = 1e-1;
  ub[0] = 1e1;
  lb[0] = max_function_value - 1e2;
  if ( lb[0] < 1e-2 ) lb[0] = 1e-2;
  ub[0] = max_function_value + 1e2;
  double delta_threshold = *delta;
  if (delta_threshold < 1e-2) delta_threshold = 1e-2;
  for (int i = 0; i < dim; ++i) {
      lb[i+1] = 1e-2 * delta_threshold; // 1e1
      ub[i+1] = 2.0 * delta_threshold; // 1e2
  }*/
  //Florians old version2:
  lb[0] = 1e-1; // * pow(1000e0 * max_noise / 2e0, 2e0);
  ub[0] = 1e1;// * pow(1000e0 * max_noise / 2e0, 2e0);
  double delta_threshold = *delta;
  if (delta_threshold < 1e-2) delta_threshold = 1e-2;
  for (int i = 0; i < dim; ++i) {
      lb[i+1] = 1e1 * delta_threshold;
      ub[i+1] = 1e2 * delta_threshold;
  }

  if (gp_parameters[0] < 0e0) {
    //gp_parameters[0] = max_function_value;
    gp_parameters[0] = lb[0]*5e-1 + 5e-1*ub[0];
    for (int i = 1; i < dim+1; ++i) {
      gp_parameters[i] = (lb[i]*5e-1 + 5e-1*ub[i]);
    }
  } else {
    for (int i = 0; i < dim+1; ++i) {
      if ( gp_parameters[i] <= lb[i] ) gp_parameters[i] = 1.1 * lb[i];
      if ( gp_parameters[i] >= ub[i] ) gp_parameters[i] = 0.9 * ub[i];
    }
  }
  //--------------------------------------------------

  //initialize optimizer from NLopt library
  int dimp1 = dim+1;
//  nlopt::opt opt(nlopt::LD_CCSAQ, dimp1);
  //nlopt::opt opt(nlopt::LN_BOBYQA, dimp1);
//
  nlopt::opt opt(nlopt::GN_DIRECT, dimp1); //Somehow GN_DIRECT is not deterministic, also nlopt::srand(seed) has no effect to fix that.
  //nlopt::opt opt(nlopt::LN_COBYLA, dimp1);

  //opt = nlopt_create(NLOPT_LN_COBYLA, dim+1);
  opt.set_lower_bounds( lb );
  opt.set_upper_bounds( ub );

  opt.set_max_objective( GaussianProcess::parameter_estimation_objective, gp_pointer);

  opt.set_xtol_abs(1e-5);
  opt.set_xtol_rel(1e-11);
//set timeout to NLOPT_TIMEOUT seconds
  //opt.set_maxtime(1.0);
  opt.set_maxeval(1000);
  //perform optimization to get correction factors

  int exitflag=-20;
  try {
    //nlopt::srand(1);
    exitflag = opt.optimize(gp_parameters, optval);
  } catch (...) {
    gp_parameters[0] = lb[0]*5e-1 + 5e-1*ub[0];
    for (int i = 1; i < dim+1; ++i) {
      gp_parameters[i] = (lb[i]*5e-1 + 5e-1*ub[i]);
    }
  }

  //std::cout << "exitflag = "<< exitflag<<std::endl;
  //std::cout << "OPTVAL .... " << optval << std::endl;
  //for ( int i = 0; i < gp_parameters.size(); ++i )
  //  std::cout << "gp_param = " << gp_parameters[i] << std::endl;
  //std::cout << std::endl;

  return;
}
//--------------------------------------------------------------------------------
void GaussianProcess::estimate_hyper_parameters_induced_only ( std::vector< std::vector<double> > const &nodes,
                                                  std::vector<double> const &values,
                                                  std::vector<double> const &noise )
{ this->estimate_hyper_parameters(nodes, values, noise);}

void GaussianProcess::estimate_hyper_parameters_ls_only ( std::vector< std::vector<double> > const &nodes,
                                                  std::vector<double> const &values,
                                                  std::vector<double> const &noise )
{ this->estimate_hyper_parameters(nodes, values, noise);}

//--------------------------------------------------------------------------------
double GaussianProcess::parameter_estimation_objective(std::vector<double> const &x,
                                                       std::vector<double> &grad,
                                                       void *data)
{

  GaussianProcess *d = reinterpret_cast<GaussianProcess*>(data);

  for (int i = 0; i < d->nb_gp_nodes; ++i) {
    for (int j = 0; j <= i; ++j)
      d->L.at(i).at(j) = d->evaluate_kernel( d->gp_nodes[i], d->gp_nodes[j], x );
    d->L.at(i).at(i) += pow( d->gp_noise.at(i) / 2e0 + d->noise_regularization, 2e0 );
  }

  d->CholeskyFactorization::compute( d->L, d->pos, d->rho, d->nb_gp_nodes );
  //assert(d->pos == 0);
  double result = 0;
  if(d->pos == 0){ //result stays 0 if L is not spd
    d->alpha = d->scaled_function_values;
    d->forward_substitution( d->L, d->alpha );
    d->backward_substitution( d->L, d->alpha );
    result = -0.5*d->VectorOperations::dot_product(d->scaled_function_values, d->alpha) +
                    -0.5*((double)d->nb_gp_nodes)*log( 2. * M_PI );
    for (int i = 0; i < d->nb_gp_nodes; ++i)
      result -= 0.5*log(d->L.at(i).at(i));
  }

/*
  //Eigen::ColPivHouseholderQR<Eigen::MatrixXd> S;
  d->S = (d->L).fullPivHouseholderQr();
  //d->S = (d->L).colPivHouseholderQr();
  d->K0 = (d->S).solve(d->scaled_function_values);

  double result = -0.5*(d->scaled_function_values).dot(d->K0) -
                  0.5*log((d->S).absDeterminant() + 1e-16) -
                  0.5*((double)d->nb_gp_nodes)*log(6.28);

  if ( !grad.empty() ) {
    for (int k = 0; k < d->dim+1; ++k) {
      for (int i = 0; i < d->nb_gp_nodes; i++) {
        for (int j = 0; j <= i; j++) {
          d->dK(i, j) = d->d_evaluate_kernel( d->gp_nodes[i], d->gp_nodes[j], x, k);
          if (i != j) d->dK(j, i) = d->dK(i, j);
        }
      }
      grad[k] = 0.5*((d->K0*(d->K0.transpose()))*d->dK - d->S.solve(d->dK)).trace() -
                0.5*((double)d->nb_gp_nodes)*0e-6;
    }
  }
*/
  return result;

}

const std::vector<std::vector<double>> &GaussianProcess::getGp_nodes() const {
    return gp_nodes;
}

void GaussianProcess::get_induced_nodes(std::vector<std::vector<double> > &) const {
    return;
}

std::vector<double> GaussianProcess::get_hyperparameters(){
    return this->gp_parameters;
}
void GaussianProcess::decrease_nugget(){
  return;
}
bool GaussianProcess::increase_nugget(){
  return true;
}

const std::vector<double> &GaussianProcess::get_gp_parameters() const {
  return gp_parameters;
}
//--------------------------------------------------------------------------------


#pragma once

#include <boost/geometry.hpp>
#if BOOST_VERSION < 107800
#include <boost/geometry/algorithms/detail/azimuth.hpp>
#endif //BOOST_VERSION < 107800

namespace taiton{

#if BOOST_VERSION < 107800
template<class Point1,class Point2>
inline long double azimuth(const Point1 &p1,const Point2 &p2){
        return azimuth<long double>(p1,p2);
}

template<typename T>
class point3_t : public boost::geometry::model::point<T,3,boost::geometry::cs::cartesian> {
        using Base = boost::geometry::model::point<T,3,boost::geometry::cs::cartesian>;
public:
        using Base::Base;

        constexpr inline T x() noexcept {
                return this->template get<0>();
        }

        constexpr inline T y() noexcept {
                return this->template get<1>();
        }

        constexpr inline T z() noexcept {
                return this->template get<2>();
        }
};

#else //BOOST_VERSION < 107800

template<typename T>
using point3_t = boost::geometry::model::d3::point_xyz<T>;

#endif //BOOST_VERSION < 107800

template<typename T>
using point2_t = boost::geometry::model::d2::point_xy<T>;

template<class Point>
using line_t = boost::geometry::model::linestring<Point>;

template<class Point>
using segment_t = boost::geometry::model::segment<Point>;

template<class Point>
using box_t = boost::geometry::model::box<Point>;

template<class Point>
using polygon_t = boost::geometry::model::polygon<Point>;

template<class Point>
inline Point rotate(Point geo,double deg){
        boost::geometry::transform(geo,geo,boost::geometry::strategy::transform::rotate_transformer<boost::geometry::degree,long double,2,2>{deg});
        return geo;
}

template<class Point>
inline Point rotate(Point geo,const Point& org,double deg){
        geo.template set<0>(geo.template get<0>() - org.template get<0>());
        geo.template set<1>(geo.template get<1>() - org.template get<1>());

        geo = rotate(geo,deg);

        geo.template set<0>(geo.template get<0>() + org.template get<0>());
        geo.template set<1>(geo.template get<1>() + org.template get<1>());

        return geo;
}

} // namespace taiton


//////////////////////////////////////////////////////////////////////////////
// iterator::transform_iterator::deduce_sig.h                               //
//                                                                          //
//  (C) Copyright 2010 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef LIBS_ITERATOR_TRANSFORM_ITERATOR_DEDUCE_SIG_FP_ER_2010_HPP
#define LIBS_ITERATOR_TRANSFORM_ITERATOR_DEDUCE_SIG_FP_ER_2010_HPP
#include <ostream>
#include <vector>
#include <boost/range.hpp>
#include <boost/iterator/transform_iterator_deduce_sig_fp.hpp>
#include <boost/iterator/transform_iterator_deduce_sig_uf.hpp>

struct f{
	struct result_impl{ typedef bool type; };

	template<typename S>
    struct result{};

    template<typename F,typename T>
    struct result<F(const T&)>:result_impl{};

    template<typename F,typename T>
    struct result<F(T&)>:result_impl{};

    template<typename T>
    bool operator()(const T& x)const{ return (x==0); }

};

struct g{
	typedef bool result_type;

    template<typename T>
    bool operator()(const T& x)const{ return (x==0); }

};

template<typename T>
bool is_zero(const T& x){ return (x==0); }

void example_deduce_sig(std::ostream& os){
	using namespace boost;

	typedef int val_;
	typedef std::vector<val_> vec_;
    vec_ vec(10,0);
	vec[3] = 1;
	vec[6] = 2;
	vec[9] = 3;

// Cannot compile
/*
    std::copy(
		make_transform_iterator(
    		boost::begin(vec),
			g()
    	),
		make_transform_iterator(
    		boost::end(vec),
			g()
    	),
		std::ostream_iterator<val_>(os," ")
    );
*/
    std::copy(
		make_transform_iterator2(
    		boost::begin(vec),
			f()
    	),
		make_transform_iterator2(
    		boost::end(vec),
			f()
    	),
		std::ostream_iterator<val_>(os," ")
    );
    os << std::endl;
    std::copy(
		make_transform_iterator2(
    		boost::begin(vec),
			g()
    	),
		make_transform_iterator2(
    		boost::end(vec),
			g()
    	),
		std::ostream_iterator<val_>(os," ")
    );
	os << std::endl;

	// Note :const_begin, not begin
    std::copy(
		make_transform_iterator2<bool>(
    		boost::const_begin(vec),
			is_zero
    	),
		make_transform_iterator2<bool>(
    		boost::const_end(vec),
			is_zero
    	),
		std::ostream_iterator<val_>(os," ")
    );

}

#endif


#include "die.h"
#include <ctime>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>

boost::random::mt19937 DieT::gen(std::time(0));

//Constructor
DieT::DieT(int const& min_dice_value_, int const& max_dice_value_)
: min_dice_value(min_dice_value_)
, max_dice_value(max_dice_value_)
{Roll();} /* roll so the value isn't garbage */

DieT::DieT(int const& min_dice_value_, int const& max_dice_value_, int const& value_)
: min_dice_value(min_dice_value_)
, max_dice_value(max_dice_value_)
, value{value_}
{}

//Transformer
void DieT::Roll(void){
	if (min_dice_value <= max_dice_value){
		boost::random::uniform_int_distribution<> dist(min_dice_value, max_dice_value);
		value = dist(gen);
	}
	return;
}

//Observer
int DieT::Value(void){
	return value;
}

//Overload
std::ostream& operator << (std::ostream &out, DieT const& die){
	return out << die.value;
}