// CHAP - The Channel Annotation Package
//
// Copyright (c) 2016 - 2018 Gianni Klesse, Shanlin Rao, Mark S. P. Sansom, and
// Stephen J. Tucker
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include <algorithm>
#include <limits>

#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <boost/math/tools/minima.hpp>
#include <boost/math/tools/roots.hpp>

#include "geometry/spline_curve_3D.hpp"
#include "geometry/cubic_spline_interp_3D.hpp"

/*!
 * Constructor for creating a spline curve of given degree from a knot vector
 * and associated control points.
 */
SplineCurve3D::SplineCurve3D(
        int degree,
        std::vector<real> knotVector,
        std::vector<gmx::RVec> ctrlPoints)
{
    // TODO: probably better to put some of these things into the initialiser
    // list

    nCtrlPoints_ = ctrlPoints.size();
    nKnots_ = knotVector.size();
    degree_ = degree;
    arcLengthTableAvailable_ = false;

    // ensure minimal number of control points for given degree:
    if( nCtrlPoints_ < degree_ + 1 )
    {
        std::cerr<<"ERROR: Need at least d + 1 control points!"<<std::endl;
        std::cerr<<"d = "<<degree_
                 <<" and n = "<<nCtrlPoints_
                 <<std::endl;
        std::abort();
    }

    // ensure minimal number of knots:
    if( nKnots_ < nCtrlPoints_ + degree_ + 1 )
    {
        std::cerr<<"ERROR: Need at least n + d + 1 knots!"<<std::endl;
        std::cerr<<"d = "<<degree_
                 <<" and n = "<<nCtrlPoints_
                 <<" and k = "<<nKnots_
                 <<std::endl;
        std::abort();
    }

    // assign knot vector and control points:
    knots_ = knotVector;
    ctrlPoints_ = ctrlPoints;
}

/*!
 * Default constructor for initialiser lists. Does not set any members!
 */
SplineCurve3D::SplineCurve3D()
{

}

/*!
 * Public interface for evaluation of spline curve. Returns the value of the
 * spline curve or its derivative at the given evaluation point. If the
 * evaluation point lies outside the knot range, linear extrapolation is used.
 */
gmx::RVec
SplineCurve3D::evaluate(
        const real &eval,
        unsigned int deriv)
{
    // extrapolation or interpolation?
    if( eval < knots_.front() || eval > knots_.back() )
    {
        return evaluateExternal(eval, deriv);
    }
    else
    {
        return evaluateInternal(eval, deriv);
    }
}

/*!
 * Auxiliary function for evaluating the spline curve at points inside the
 * range covered by knots.
 */
gmx::RVec
SplineCurve3D::evaluateInternal(const real &eval, unsigned int deriv)
{
    // container for basis functions or derivatives:
    SparseBasis basis;

    // derivative required?
    if( deriv == 0 )
    {
        // evaluate B-spline basis:
        basis = B_(eval, knots_, degree_);
    }
    else
    {
        // evaluate B-spline basis derivatives:
        basis = B_(eval, knots_, degree_, deriv);
    }

    // return value of spline curve (derivative) at given evaluation point:
    return computeLinearCombination(basis);
}

/*!
 * Auxiliary function for evaluating the spline curve at points outside the
 * range covered by knots. Linear extrapolation is used in this case.
 */
gmx::RVec
SplineCurve3D::evaluateExternal(const real &eval, unsigned int deriv)
{
    // which boundary is extrapolation based on?
    real boundary;
    if( eval < knots_.front() )
    {
        boundary = knots_.front();
    }
    else
    {
        boundary = knots_.back();
    }

    // derivative required?
    if( deriv == 0 )
    {
        // compute slope and offset:
        // TODO: this can be made more efficient by evaluating basis and derivs
        // in one go!
        SparseBasis basis = B_(boundary, knots_, degree_);
        gmx::RVec offset = computeLinearCombination(basis);
        basis = B_(boundary, knots_, degree_, 1);
        gmx::RVec slope = computeLinearCombination(basis);

        // return extrapolation point:
        svmul(eval - boundary, slope, slope);
        rvec_add(slope, offset, slope);
        return slope;

    }
    else if( deriv == 1 )
    {
        // simply return the slope at the endpoint:
        SparseBasis basis = B_(boundary, knots_, degree_, 1);
        return computeLinearCombination(basis);
    }
    else
    {
        // for linear extrapolation, second and higher order deriv are zero:
        return gmx::RVec(0.0, 0.0, 0.0);
    }
}

/*!
 * Evaluates the linear combination of basis functions weighted by control
 * points, i.e. computes
 *
 * \f[
 *      \sum_i \mathbf{c}_i B_{i,p}(x)
 * \f]
 *
 * where the sum only goes over the nonzero elements of the basis for
 * efficiency.
 */
gmx::RVec
SplineCurve3D::computeLinearCombination(const SparseBasis &basis)
{
    gmx::RVec value(gmx::RVec(0.0, 0.0, 0.0));
    for(auto b : basis)
    {
        gmx::RVec tmp;
        svmul(b.second, ctrlPoints_[b.first], tmp);
        rvec_add(value, tmp, value);
    }

    return value;
}

/*!
 * Change the internal representation of the curve such that it is
 * parameterised in terms of arc length.
 */
void
SplineCurve3D::arcLengthParam()
{
    // number of uniformly spaced arc length parameters:
    int nNew = 10*nCtrlPoints_;

    // create lookup table for arc length at knots:
    prepareArcLengthTable();

    // determine uniform arc length spacing:
    real arcLenStep = arcLengthTable_.back() / (nNew - 1);

    // initialise new control points and parameters:
    std::vector<real> newParams;
    std::vector<gmx::RVec> newPoints;

    // loop over uniformly spaced arc length intervals:
    for(int i = 0; i < nNew; i++)
    {
        // calculate target arc length:
        real newParam = i*arcLenStep;
        newParams.push_back(newParam);

        // find parameter value corresponding to arc length value:
        real oldParam = arcLengthToParam(newParam);

        //  evaluate spline to get new point:
        newPoints.push_back(this -> evaluate(oldParam, 0));
    }

    // interpolate new points to get arc length parameterised curve:
    CubicSplineInterp3D Interp;
    SplineCurve3D newSpl = Interp(newParams,
                                  newPoints,
                                  eSplineInterpBoundaryHermite);

    // update own parameters:
    this -> knots_ = newSpl.knots_;
    this -> ctrlPoints_ = newSpl.ctrlPoints_;
    this -> nKnots_ = newSpl.nKnots_;
    this -> nCtrlPoints_ = newSpl.nCtrlPoints_;
    this -> arcLengthTableAvailable_ = false;

    // reset reference points for mapping:
    refPoints_.clear();
}

/*!
 * Calculates the length along the arc of the curve between the two parameter
 * values a and b.
 */
real
SplineCurve3D::length(const real &lo, const real &hi)
{
    // do we need to form a lookup table:
    if( arcLengthTableAvailable_ == false || arcLengthTable_.size() == 0 )
    {
        prepareArcLengthTable();
    }

    // initialise length as zero::
    real length = 0.0;

    // find intervals of evaluation points:
    int idxLo = findInterval(lo);
    int idxHi = findInterval(hi);

    // add distance in endpoint intervals:
    if( idxHi == idxLo )
    {
        length += arcLengthBoole(lo, hi);
    }
    else
    {
        length += arcLengthBoole(lo, knots_[idxLo + 1]);
        length += arcLengthBoole(knots_[idxHi], hi);
    }

    // if necessary, loop over intermediate spline segments and sum up lengths:
    if( idxHi - idxLo > 1 )
    {
        for(int i = idxLo + 1; i < idxHi; i++)
        {
            // add length of current segment from lookup table:
            length += arcLengthTable_[i + 1] - arcLengthTable_[i];
        }
    }

    return length;
}

/*!
 * Convenience function to calculate arc length of curve between first and last
 * support point.
 */
real
SplineCurve3D::length()
{
    return length(knots_.front(), knots_.back());
}

/*!
 * Returns the tangent vector at the given evaluation point. Simply a wrapper
 * around the general evaluation function requesting the first derivative at
 * the given point.
 */
gmx::RVec
SplineCurve3D::tangentVec(const real &eval)
{
    return evaluate(eval, 1);
}

/*!
 * Returns the normal vector at the evaluation point.
 */
gmx::RVec
SplineCurve3D::normalVec(const real &eval)
{
    return evaluate(eval, 2);
}

/*!
 * Returns the speed of the curve at the given evaluation point, where speed
 * refers to the magnitude of the tangent vector.
 */
real
SplineCurve3D::speed(const real &eval)
{
    // return magnitude of tangent vector:
    return norm( this -> tangentVec(eval) );
}

/*!
 * Takes point in Cartesian coordinates and returns that points coordinates in
 * the curvilinear system defined by the spline curve. Return value is an RVec,
 * which contains the following information:
 *
 *      [0] - distance along the arc of the curve
 *      [1] - squared (!) distance from the curve at closest point
 *      [2] - angular coordinate; this is not yet implemented
 *
 * Note that this function assumes that the curve is parameterised by arc
 * length!
 *
 * \todo implement angular coordinate!
 */
gmx::RVec
SplineCurve3D::cartesianToCurvilinear(const gmx::RVec &cartPoint)
{
    // find index of interval containing closest point on spline curve:
    unsigned int idx = closestSplinePoint(cartPoint);

    // find closest point on this interval:
    gmx::RVec proj = projectionInInterval(
            cartPoint,
            knots_[idx + degree_],
            knots_[idx + degree_ + 1]);

    // check neighbouring knot intervals and extrapolate if necessary:
    gmx::RVec altProj;

    // next lower knot interval:
    if( idx == 0 )
    {
        altProj = projectionInExtrapRange(cartPoint, -1.0);
    }
    else
    {
        altProj = projectionInInterval(
                cartPoint,
                knots_[idx + degree_ - 1],
                knots_[idx + degree_]);
    }

    // does alternative projection give closer point:
    if( altProj[RR] < proj[RR] )
    {
        proj = altProj;
    }

    // next higher knot interval:
    if( idx == refPoints_.size() - 2 )
    {
        altProj = projectionInExtrapRange(cartPoint, 1.0);
    }
    else
    {
        altProj = projectionInInterval(
                cartPoint,
                knots_[idx + degree_ + 1],
                knots_[idx + degree_ + 2]);
    }

    // does alternative projection give closer point:
    if( altProj[RR] < proj[RR] )
    {
        proj = altProj;
    }

    // TODO: calculate angular coordinate!
    proj[PP] = 0.0;

    // return point in curvilinear coordinates:
    return proj;
}

/*!
 * Auxiliary function for finding the closest point on a spline curve that
 * returns the corresponding spline interval index. First, a set of reference
 * points is sampled from the spline curve at the location of the unique knots
 * (this step is skipped if the reference points have already been computed in
 * a previous call to this function). Secondly, a linear search over these
 * reference points is carried out to find the point closest to a given test
 * point.
 *
 * The return value is the index of the closest reference point, except for the
 * case where the closest reference point is the last point, which is mapped to
 * the last interval, i.e. the index of the penultimate reference point is
 * returned in this case.
 *
 * \todo Linear search may not be the most efficient.
 */
unsigned int
SplineCurve3D::closestSplinePoint(const gmx::RVec &point)
{
    // build lookup table:
    if( refPoints_.empty() )
    {
        refPoints_.reserve(uniqueKnots().size());
        for(auto s : uniqueKnots())
        {
            refPoints_.push_back( this -> evaluate(s, 0) );
        }
    }

    // find index of closest reference point on spline curve:
    unsigned int idxMinDist = 0;
    real minDist = std::numeric_limits<real>::infinity();
    for(unsigned int i = 0; i < refPoints_.size(); i++)
    {
        // find dist to control point:
        real dist = distance2(point, refPoints_[i]);

        // closer than previous closest point:
        if( dist < minDist )
        {
            minDist = dist;
            idxMinDist = i;
        }
    }

    // special case of last control point:
    if( idxMinDist == refPoints_.size() - 1 )
    {
        // simply map this to last interval:
        idxMinDist--;
    }

    // return index of interval of closest point:
    return idxMinDist;
}

/*!
 * Auxiliary function that maps a point in Cartesian coordinates onto an
 * internal segment of the spline curve. This is accomplished by iteratively
 * minimising the distance between the test point and a base point sampled from
 * the spline curve using the (derivative free) method of Brent.
 *
 * \throws A logic error is thrown if the minimum can not be converged within
 * a hardcoded number of 100 Brent iterations.
 */
gmx::RVec
SplineCurve3D::projectionInInterval(
        const gmx::RVec &point,
        const real &lo,
        const real &hi)
{
    // internal parameters:
    const boost::uintmax_t maxIter = 100;
    boost::uintmax_t iter = maxIter;

    // objective function binding:
    boost::function<real(real)> objFun;
    objFun = boost::bind(
            &SplineCurve3D::pointSqDist,
            this,
            point,
            _1);

    // find minimum via Brent's method:
    int bits = std::numeric_limits<double>::digits;
    std::pair<double, double> result;
    result = boost::math::tools::brent_find_minima(
            objFun,
            lo,
            hi,
            bits,
            iter);

    // make sure convergence has been reached:
    if( iter >= maxIter )
    {
        throw std::logic_error("Could not converge Brent iteration in "
                               "Cartesian to curvilinear mapping!");
    }

    // return curvilinear coordinates of point:
    // TODO: implement angular coordinate
    gmx::RVec curvPoint;
    curvPoint[SS] = result.first;
    curvPoint[RR] = result.second;
    return curvPoint;
}

/*!
 * Auxiliary function that projects a point in Cartesian coordinates onto the
 * extrapolation range beyond its two endpoints. As the curve is known to be a
 * line in this range the projection is solved for analytically as a projection
 * onto a ray. To achieve this, a ray is constructed by sampling two point
 * from the spline curve (its endpoint and a second point \f$ ds \f$  beyond
 * this endpoint.
 */
gmx::RVec
SplineCurve3D::projectionInExtrapRange(
        const gmx::RVec &point,
        const real &ds)
{
    // return variable:
    gmx::RVec proj;

    // lower or upper extrapolation range?
    gmx::RVec extrapPointA;
    gmx::RVec extrapPointB;
    real arcLenOffset;
    real arcLenSign;
    if( ds < 0.0 )
    {
        // lower range:
        extrapPointA = ctrlPoints_.front();
        arcLenOffset = knots_.front();
        arcLenSign = -1.0;
        extrapPointB = this -> evaluate(arcLenOffset + ds, 0);
    }
    else if( ds > 0.0 )
    {
        // upper range:
        extrapPointA = ctrlPoints_.back();
        arcLenOffset = knots_.back();
        arcLenSign = 1.0;
        extrapPointB = this -> evaluate(arcLenOffset + ds, 0);
    }
    else
    {
        throw std::logic_error("Parameter dt may not be zero!");
    }

    // (non-normalised) direction vector for the extrapolating line:
    gmx::RVec lineDirVector;
    rvec_sub(extrapPointB, extrapPointA, lineDirVector);

    // vector between the test point and the ray's endpoint:
    gmx::RVec endpointVector;
    rvec_sub(point, extrapPointA, endpointVector);

    // is ray endpoint the closest point?
    // NOTE: in this case the angle between line direction vector and endpoint
    // vector will be >= 90 degrees and the scalar product <= 0.0!
    real cosOfAngle = iprod(endpointVector, lineDirVector);
    if( cosOfAngle <= 0.0 )
    {
        proj[SS] = arcLenOffset;
        proj[RR] = distance2(point, extrapPointA);

        return proj;
    }

    // projection of test point position onto ray and base point:
    real b = cosOfAngle/iprod(lineDirVector, lineDirVector);
    gmx::RVec basePoint;
    svmul(b, lineDirVector, basePoint);
    rvec_add(basePoint, extrapPointA, basePoint);

    // curvilinear coordinates around this line:
    proj[SS] = arcLenOffset + arcLenSign*b;
    proj[RR] = distance2(point, basePoint);

    // return points in curvilinear coordinates:
    return proj;
}

/*!
 * Getter function for access to the spline curves control points.
 */
std::vector<gmx::RVec>
SplineCurve3D::ctrlPoints() const
{
    return ctrlPoints_;
}

/*!
 * Uses Newton-Cotes quadrature of curve speed to determine the length of the
 * arc between two given parameter values. The specific quadrature rule applied
 * is Boole's rule.
 */
real
SplineCurve3D::arcLengthBoole(const real &lo, const real &hi)
{
    // determine intermediate evaluation points:
    real h = (hi - lo)/4.0;
    real t2 = lo + 1.0*h;
    real t3 = lo + 2.0*h;
    real t4 = lo + 3.0*h;

    // evaluate speed at support points:
    real s1 = speed(lo);
    real s2 = speed(t2);
    real s3 = speed(t3);
    real s4 = speed(t4);
    real s5 = speed(hi);

    // evaluate Boole's law:
    return 2.0*h/45.0*(7.0*s1 + 32.0*s2 + 12.0*s3 + 32.0*s4 + 7.0*s5);
}

/*!
 * Prepares a lookup table that associates an arc length with each knot, where
 * the first knot is assigned an arc length of zero.
 */
void
SplineCurve3D::prepareArcLengthTable()
{
    arcLengthTable_.resize(knots_.size());
    real segmentLength;
    for(unsigned int i = 0; i < knots_.size() - 1; i++)
    {
        // calculate length of current segment:
        segmentLength = arcLengthBoole(knots_[i], knots_[i+1]);

        // add to arc length table:
        arcLengthTable_[i + 1] = arcLengthTable_[i] + segmentLength;
    }

    // set flag:
    arcLengthTableAvailable_ = true;
}

/*!
 * Returns arc length value at the control points by simply removing the
 * repeated knot values from the arc length lookup lookup table.
 */
std::vector<real>
SplineCurve3D::ctrlPointArcLength()
{
    // check availability:
    if( arcLengthTableAvailable_ == false )
    {
        prepareArcLengthTable();
    }

    // create copy of table and remove redundant elements:
    std::vector<real> arcLength(arcLengthTable_.begin() + degree_,
                                arcLengthTable_.end() - degree_);

    return arcLength;
}

/*!
 * Returns arc length value at first control point.
 */
real
SplineCurve3D::frstPointArcLength()
{
    // check availability:
    if( arcLengthTableAvailable_ == false )
    {
        prepareArcLengthTable();
    }

    return arcLengthTable_.front();
}

/*!
 * Returns arc length value at last control point.
 */
real
SplineCurve3D::lastPointArcLength()
{
    // check availability:
    if( arcLengthTableAvailable_ == false )
    {
        prepareArcLengthTable();
    }

    return arcLengthTable_.back();
}

/*!
 * Returns the parameter value (in the current parameterisation, typically
 * chord length) that corresponds to a given value of arc length. This is done
 * via bisection.
 */
real
SplineCurve3D::arcLengthToParam(real &arcLength)
{
    boost::uintmax_t maxIter = 100;
    real absTol = 0.01*std::sqrt(std::numeric_limits<real>::epsilon());

    // sanity check for arc length table:
    if( arcLengthTableAvailable_ != true )
    {
        prepareArcLengthTable();
    }

    // handle upper endpoint:
    if( arcLength == arcLengthTable_.back() )
    {
        return knots_.back();
    }

    // find appropriate interval:
    std::pair<std::vector<real>::iterator, std::vector<real>::iterator> bounds;
    bounds = std::equal_range(arcLengthTable_.begin(), arcLengthTable_.end(), arcLength);
    int idxLo = bounds.second - arcLengthTable_.begin() - 1;
    int idxHi = bounds.second - arcLengthTable_.begin();

    // handle query outside table range:
    // TODO: add case for query below lower bound and test!
    if( bounds.second == arcLengthTable_.end() )
    {
        return knots_.back() + arcLength - arcLengthTable_.back();
    }
    if( bounds.second == arcLengthTable_.begin() )
    {
        std::cerr<<"ERROR: arc length below table value range!"<<std::endl;
        std::abort();
    }

    // initialise bisection interval and lower limit:
    real tLo = knots_[idxLo];
    real tHi = knots_[idxHi];
    real tLi = tLo;

    // target arc length within this interval:
    real targetIntervalLength = arcLength - arcLengthTable_[idxLo];

    // termination condition binding:
    boost::function<bool(real, real)> termCond;
    termCond = boost::bind(&SplineCurve3D::arcLengthToParamTerm,
                           this,
                           _1,
                           _2,
                           absTol);

    // objective function binding:
    boost::function<real(real)> objFun;
    objFun = boost::bind(&SplineCurve3D::arcLengthToParamObj,
                         this,
                         tLi,
                         _1,
                         targetIntervalLength);

    std::pair<real, real> result;
    result = boost::math::tools::toms748_solve(objFun,
                                               tLo,
                                               tHi,
                                               termCond,
                                               maxIter);

    // best guess if middle of bracketing interval:
    return 0.5*(result.first + result.second);
}

/*!
 * Termination condition for re-parameterisation optimisation.
 */
bool
SplineCurve3D::arcLengthToParamTerm(real lo, real hi, real tol)
{
    return std::abs(hi - lo) <= tol;
}

/*!
 * Objective function for re-parameterisation optimisation.
 */
real
SplineCurve3D::arcLengthToParamObj(real lo, real hi, real target)
{
    return arcLengthBoole(lo, hi) - target;
}

/*!
 * Computes the squared Euclidean distance between some point and the point
 * on the spline curve with the given parameter value. For efficiency, the
 * square root is not drawn, hence the squared distance is returned.
 */
double
SplineCurve3D::pointSqDist(gmx::RVec point, double eval)
{
    // evaluate spline:
    gmx::RVec splPoint = evaluate(eval, 0);

    // return squared distance:
    return (splPoint[XX] - point[XX])*(splPoint[XX] - point[XX]) +
           (splPoint[YY] - point[YY])*(splPoint[YY] - point[YY]) +
           (splPoint[ZZ] - point[ZZ])*(splPoint[ZZ] - point[ZZ]);
}


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// 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.

#ifndef NETKET_ISING1D_HPP
#define NETKET_ISING1D_HPP

#include <mpi.h>
#include <Eigen/Dense>
#include <complex>
#include <iostream>
#include <vector>
#include "Utils/random_utils.hpp"
#include "abstract_hamiltonian.hpp"

namespace netket {

/**
  Transverse field Ising model on an arbitrary graph.
*/
template <class G>
class Ising : public AbstractHamiltonian {
  const int nspins_;
  double h_;
  double J_;

  const G &graph_;

  /**
    List of bonds for the interaction part.
  */
  std::vector<std::vector<int>> bonds_;

  /**
    Hilbert space descriptor for this hamiltonian.
  */
  Hilbert hilbert_;

 public:
  /**
    Json constructor.
    @param G is a graph from which the number of spins and the bonds are
    obtained.
    @param pars is a json list of parameters. The default value of J is 1.0
  */
  explicit Ising(const G &graph, const json &pars)
      : nspins_(graph.Nsites()),
        h_(FieldVal(pars["Hamiltonian"], "h")),
        J_(FieldOrDefaultVal(pars["Hamiltonian"], "J", 1.0)),
        graph_(graph) {
    Init();
  }

  void Init() {
    GenerateBonds();

    // Specifying the hilbert space
    json hil;
    hil["Hilbert"]["Name"] = "Spin";
    hil["Hilbert"]["Nspins"] = nspins_;
    hil["Hilbert"]["S"] = 0.5;

    hilbert_.Init(hil);

    InfoMessage() << "Transverse-Field Ising model created " << std::endl;
    InfoMessage() << "h = " << h_ << std::endl;
    InfoMessage() << "J = " << J_ << std::endl;
  }

  /**
    Member function generating the bonds on the lattice.
    bonds[i][k] contains the k-th bond for site i.
  */
  void GenerateBonds() {
    auto adj = graph_.AdjacencyList();

    bonds_.resize(nspins_);

    for (int i = 0; i < nspins_; i++) {
      for (auto s : adj[i]) {
        if (s > i) {
          bonds_[i].push_back(s);
        }
      }
    }
  }

  /**
  Member function finding the connected elements of the Hamiltonian.
  Starting from a given visible state v, it finds all other visible states v'
  such that the hamiltonian matrix element H(v,v') is different from zero.
  In general there will be several different connected visible units satisfying
  this condition, and they are denoted here v'(k), for k=0,1...N_connected.
  @param v a constant reference to the visible configuration.
  @param mel(k) is modified to contain matrix elements H(v,v'(k)).
  @param connector(k) for each k contains a list of sites that should be changed
  to obtain v'(k) starting from v.
  @param newconfs(k) is a vector containing the new values of the visible units
  on the affected sites, such that: v'(k,connectors(k,j))=newconfs(k,j). For the
  other sites v'(k)=v, i.e. they are equal to the starting visible
  configuration.
  */
  void FindConn(const Eigen::VectorXd &v,
                std::vector<std::complex<double>> &mel,
                std::vector<std::vector<int>> &connectors,
                std::vector<std::vector<double>> &newconfs) const override {
    connectors.clear();
    connectors.resize(nspins_ + 1);
    newconfs.clear();
    newconfs.resize(nspins_ + 1);
    mel.resize(nspins_ + 1);

    mel[0] = 0;
    connectors[0].resize(0);
    newconfs[0].resize(0);

    for (int i = 0; i < nspins_; i++) {
      // spin flips
      mel[i + 1] = -h_;
      connectors[i + 1].push_back(i);
      newconfs[i + 1].push_back(-v(i));

      // interaction part
      for (auto bond : bonds_[i]) {
        mel[0] -= J_ * v(i) * v(bond);
      }
    }
  }

  const Hilbert &GetHilbert() const override { return hilbert_; }
};

}  // namespace netket

#endif


///////////////////////////////////////////////////////////////////////////////
// value.hpp                                                                 //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_EMPIRICAL_DISTRIBUTION_KOLMOGOROV_SMIRNOV_STATISTIC_VALUE_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_EMPIRICAL_DISTRIBUTION_KOLMOGOROV_SMIRNOV_STATISTIC_VALUE_HPP_ER_2010
#include <cmath>
#include <utility>

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

#include <boost/numeric/conversion/converter.hpp>

#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/apply.hpp>
#include <boost/mpl/if.hpp>

#include <boost/foreach.hpp>
#include <boost/parameter/name.hpp>
#include <boost/parameter/binding.hpp>

#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/parameters/accumulator.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics/count.hpp>

#include <boost/statistics/detail/non_parametric/empirical_distribution/ordered_sample.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace empirical_distribution{
namespace kolmogorov_smirnov_statistic{

BOOST_PARAMETER_NAME(benchmark_distribution);

namespace impl{

    // T can be an integer or a float
    template<typename T,typename T1,typename Comp = std::less<T> >
	class value : public boost::accumulators::accumulator_base
    {
        typedef Comp comp_;
        typedef boost::accumulators::dont_care dont_care_;

        public:

        typedef T1 result_type;
        typedef T sample_type;

        value(dont_care_){}

        void operator()(dont_care_){}

        template<typename Args>
        result_type result(const Args& args)const{
            namespace ac = boost::accumulators;
            namespace ed = detail::empirical_distribution;
            namespace ks = ed::kolmogorov_smirnov_statistic;
            typedef T1 val_;
            typedef std::size_t size_;
            typedef ac::tag::count tag_n_;
            typedef ed::tag::ordered_sample tag_os_;
            typedef typename parameter::binding<
                Args, ac::tag::accumulator>::type cref_acc_set_;
            typedef typename boost::remove_cv<
               typename boost::remove_reference<
                   cref_acc_set_
                >::type
            >::type acc_set_;
            typedef typename ed::result_of::ordered_sample<
                acc_set_>::type ref_os_;
            typedef typename boost::remove_const< //in case ref changed to cref
            	typename boost::remove_reference<ref_os_>::type
            >::type os_;
            typedef typename boost::range_reference<os_>::type ref_elem_;
            typedef boost::numeric::converter<val_,size_> converter_;

            ref_os_ ref_os = ac::extract_result<tag_os_>(
                 args[ ac::accumulator ] );
            val_ m1 = converter_::convert( 0 );
            size_ i = 0;
            size_ n = boost::accumulators::extract::count(
                args[ ac::accumulator ] );

            BOOST_FOREACH(ref_elem_ e,ref_os){
                i += e.second;
                val_ ecdf = converter_::convert( i ) / converter_::convert( n );
                val_ true_cdf = cdf(
                    args[ ks::_benchmark_distribution ] ,
                    e.first
                );
                val_ m2
                	= (true_cdf > ecdf)?(true_cdf - ecdf) : (ecdf - true_cdf);
                if(m2 > m1){ m1 = m2; }
            }

            return m1;
        }

    };

}// impl
namespace tag
{
    template<typename T1 = double>
    struct value: boost::accumulators::depends_on<
        empirical_distribution::tag::ordered_sample,
        accumulators::tag::count
    >
    {
        struct impl{
            template<typename T,typename W>
            struct apply{
                typedef empirical_distribution
                    ::kolmogorov_smirnov_statistic::impl::value<T,T1> type;
            };
        };
    };
}// tag
namespace result_of{

    template<typename T1,typename AccSet,typename D>
    struct value{
    	typedef empirical_distribution
            ::kolmogorov_smirnov_statistic::tag::value<T1> tag_;
        typedef typename
            boost::accumulators::detail::template
            	extractor_result<AccSet,tag_>::type type;
    };

}
namespace extract
{

    template<typename T1,typename AccSet,typename D>
    typename kolmogorov_smirnov_statistic::result_of::template
        value<T1,AccSet,D>::type
  	value(AccSet const& acc,const D& dist)
    {
        namespace ed = detail::empirical_distribution;
        namespace ks = ed::kolmogorov_smirnov_statistic;
    	typedef ks::tag::value<T1> tag_;
        return boost::accumulators::extract_result<tag_>(
            acc,
            (ks::_benchmark_distribution = dist)
        );
  	}

}// extract
}// kolmogorov_smirnov_statistic
}// empirical_distribution
}// detail
}// statistics
}// boost

#endif


/** A simple neuron / genetic algorithm implemented in pipeline language */

#include <job_stream/job_stream.h>

#include <boost/lexical_cast.hpp>
#include <cmath>
#include <memory>
#include <random>

using std::unique_ptr;

std::mt19937 rngEngine;
std::uniform_real_distribution<float> rng(-1, 1);
float getRandom() {
    return rng(rngEngine);
}

class NeuralLayer {
public:
    NeuralLayer() {}
    NeuralLayer(const NeuralLayer& other) {
        this->inputs = other.inputs;
        this->neurons = other.neurons;
        this->weights = other.weights;
    }
    NeuralLayer(int inputs, int neurons) : inputs(inputs), neurons(neurons) {
        for (int i = 0; i < neurons * inputs; i++) {
            this->weights.push_back(getRandom());
        }
    }

    int inputs;
    int neurons;

    float* eval(float* inputs) {
        if (!this->lastResults) {
            this->lastResults.reset(new float[this->neurons]);
        }

        for (int i = 0; i < this->neurons; i++) {
            float v = 0.0;
            for (int j = 0; j < this->inputs; j++) {
                v += this->weights[i * this->inputs + j] * inputs[j];
            }
            this->lastResults.get()[i] = 1.f / (1.f + exp(-v));
        }

        return this->lastResults.get();
    }

    void initFrom(NeuralLayer* a, NeuralLayer* b) {
        this->inputs = a->inputs;
        this->neurons = a->neurons;
        std::uniform_int_distribution<> dist(1, a->weights.size() - 2);
        int split = dist(rngEngine);
        for (int i = 0; i < split; i++) {
            this->weights.push_back(a->weights[i]);
        }
        for (int i = split, m = a->weights.size(); i < m; i++) {
            this->weights.push_back(b->weights[i]);
        }

        //Mutate!
        std::uniform_int_distribution<> wdist(0, a->weights.size() / 2);
        std::uniform_int_distribution<> mut(0, this->weights.size());
        for (int j = 0, k = wdist(rngEngine); j < k; j++) {
            this->weights[mut(rngEngine)] += 0.2 * getRandom();
        }
    }

private:
    std::vector<float> weights;
    unique_ptr<float[]> lastResults;

    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive& ar, const unsigned int version) {
        ar & this->inputs;
        ar & this->neurons;
        ar & this->weights;
    }
};

class NeuralNet {
public:
    NeuralNet() {}
    NeuralNet(int neurons, int inputs, int outputs) {
        unique_ptr<NeuralLayer> layer;
        layer.reset(new NeuralLayer(inputs, neurons));
        this->layers.push_back(std::move(layer));
        layer.reset(new NeuralLayer(neurons, neurons));
        this->layers.push_back(std::move(layer));
        layer.reset(new NeuralLayer(neurons, outputs));
        this->layers.push_back(std::move(layer));
    }
    NeuralNet(const NeuralNet& other) {
        unique_ptr<NeuralLayer> layer;
        for (int i = 0, m = other.layers.size(); i < m; i++) {
            layer.reset(new NeuralLayer(*other.layers[i]));
            this->layers.push_back(std::move(layer));
        }
        this->score = other.score;
    }
    NeuralNet(NeuralNet& a, NeuralNet& b) {
        //Cross a and b
        unique_ptr<NeuralLayer> layer;
        for (int i = 0, m = a.layers.size(); i < m; i++) {
            layer.reset(new NeuralLayer());
            layer->initFrom(a.layers[i].get(), b.layers[i].get());
            this->layers.push_back(std::move(layer));
        }
    }

    float score;

    float getError(YAML::Node& array) {
        unique_ptr<float[]> inputs(new float[array.size()]);
        for (int i = 0, m = array.size(); i < m; i++) {
            inputs[i] = array[i].as<float>();
        }

        float* layerInput = inputs.get();
        for (int i = 0, m = this->layers.size(); i < m; i++) {
            layerInput = this->layers[i]->eval(layerInput);
        }

        //layerInput now == output
        float score = 0;
        for (int base = this->layers[0]->inputs, i = 0,
                m = array.size() - base; i < m; i++) {
            score += (layerInput[i] - inputs[base + i])
                    * (layerInput[i] - inputs[base + i]);
        }
        return score;
    }

private:
    std::vector<unique_ptr<NeuralLayer>> layers;

    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive& ar, const unsigned int version) {
        ar & this->score;
        int m = this->layers.size();
        ar & m;
        for (int i = 0; i < m; i++) {
            if (Archive::is_loading::value) {
                unique_ptr<NeuralLayer> ptr(new NeuralLayer());
                ar & *ptr;
                this->layers.push_back(std::move(ptr));
            }
            else {
                ar & *this->layers[i];
            }
        }
    }
};

class NetworkPopulace {
public:
    NetworkPopulace() {
    }

    NetworkPopulace(const NetworkPopulace& other) {
        for (size_t i = 0, m = other.networks.size(); i < m; i++) {
            this->networks.push_back(unique_ptr<NeuralNet>(
                    new NeuralNet(*other.networks[i])));
        }
    }

    void addNetwork(unique_ptr<NeuralNet> network) {
        this->networks.push_back(std::move(network));
    }

    void joinNetwork(unique_ptr<NetworkPopulace> other) {
        for (auto& mem : other->networks) {
            this->networks.push_back(std::move(mem));
        }
    }

    NeuralNet& bestNetwork() {
        float best = this->bestScore();
        for (int i = 0, m = this->networks.size(); i < m; i++) {
            auto* net = this->networks[i].get();
            if (net->score == best) {
                return *net;
            }
        }
        throw std::runtime_error("No best network?");
    }

    float bestScore() {
        float min = 1e35f;
        for (int i = 0, m = this->networks.size(); i < m; i++) {
            auto* net = this->networks[i].get();
            if (net->score < min) {
                min = net->score;
            }
        }
        return min;
    }

    void clear() {
        this->networks.clear();
    }

    void cross() {
        std::vector<NeuralNet*> newNets;
        for (int i = 0, m = this->networks.size() - 1; i < m; i++) {
            NeuralNet& src1 = this->monteCarlo(0);
            NeuralNet& src2 = this->monteCarlo(&src1);
            newNets.push_back(new NeuralNet(src1, src2));
        }
        //Elite!
        for (int i = 0, m = this->networks.size(); i < m; i++) {
            if (this->networks[i]->score == this->bestScore()) {
                newNets.push_back(this->networks[i].release());
                break;
            }
        }
        this->networks.clear();
        for (int i = 0, m = newNets.size(); i < m; i++) {
            unique_ptr<NeuralNet> ptr(newNets[i]);
            this->networks.push_back(std::move(ptr));
        }
    }

    NeuralNet& monteCarlo(NeuralNet* dontChoose) {
        float max = 0.0;
        float total = 0.0;
        int m = this->networks.size();
        for (int i = 0; i < m; i++) {
            if (this->networks[i].get() == dontChoose) continue;
            float score = this->networks[i]->score;
            if (score > max) max = score;
        }
        max *= 1.01;
        for (int i = 0; i < m; i++) {
            if (this->networks[i].get() == dontChoose) continue;
            total += max - this->networks[i]->score;
        }

        std::uniform_real_distribution<float> rngMonte(0, total);
        float slice = rngMonte(rngEngine);
        for (int i = 0; i < m; i++) {
            if (this->networks[i].get() == dontChoose) continue;
            slice -= max - this->networks[i]->score;
            if (slice <= 0.0) {
                return *this->networks[i];
            }
        }

        throw std::runtime_error("monteCarlo fell out bottom");
    }

    int size() {
        return this->networks.size();
    }

    NeuralNet& operator[](int index) {
        return *this->networks[index];
    }

private:
    std::vector<unique_ptr<NeuralNet>> networks;

    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive& ar, const unsigned int version) {
        int m = this->networks.size();
        ar & m;
        for (int i = 0; i < m; i++) {
            if (Archive::is_loading::value) {
                unique_ptr<NeuralNet> ptr(new NeuralNet());
                ar & *ptr;
                this->networks.push_back(std::move(ptr));
            }
            else {
                ar & *this->networks[i];
            }
        }
    }
};

class MakeNetworks : public job_stream::Job<MakeNetworks, int> {
public:
    static const char* NAME() { return "makeNetworks"; }

    void handleWork(unique_ptr<int> networkCount) {
        //Initialize networkCount networks
        auto conf = this->config;
        for (int i = 0; i < *networkCount; i++) {
            this->emit(NeuralNet(conf["neurons"].as<int>(),
                    conf["numInputs"].as<int>(),
                    conf["numOutputs"].as<int>()));
        }
    }
} makeNetworks;

class EvalNetwork : public job_stream::Job<EvalNetwork, NeuralNet> {
public:
    static const char* NAME() { return "evalNetwork"; }

    void handleWork(unique_ptr<NeuralNet> network) {
        float score = 0.0;
        auto tests = this->globalConfig["tests"].as<
                std::vector<YAML::Node>>();
        for (int i = 0, m = tests.size(); i < m; i++) {
            score += network->getError(tests[i]);
        }
        network->score = score;
        this->emit(*network);
    }
} evalNetwork;

class CheckErrorAndBreed : public job_stream::Reducer<CheckErrorAndBreed, NetworkPopulace,
        NeuralNet> {
public:
    static const char* NAME() { return "checkErrorAndBreed"; }

    void handleAdd(NetworkPopulace& current, unique_ptr<NeuralNet> work) {
        current.addNetwork(std::move(work));
    }

    void handleJoin(NetworkPopulace& current,
            unique_ptr<NetworkPopulace> other) {
        current.joinNetwork(std::move(other));
    }

    void handleDone(NetworkPopulace& current) {
        float bestScore = current.bestScore();
        NeuralNet& best = current.bestNetwork();
        if (bestScore < this->config["error"].as<float>()) {
            std::ostringstream ss;
            ss << "Done!  Best error: " << boost::lexical_cast<std::string>(
                    current.bestScore());
            ss << ".";
            this->emit(ss.str());
            return;
        }
        else {
            printf("Best score: %.3f (", bestScore);
            auto tests = this->globalConfig["tests"]
                    .as<std::vector<YAML::Node>>();
            for (int i = 0, m = tests.size(); i < m; i++) {
                if (i != 0) {
                    printf(", ");
                }
                printf("%f", best.getError(tests[i]));
            }
            printf(")\n");
        }

        current.cross();
        for (int i = 0, m = current.size(); i < m; i++) {
            this->recur(current[i]);
        }

        //Wait for those crossed networks to come through, so we maintain
        //population size.
        current.clear();
    }
} checkErrorAndBreed;

int main(int argc, char* argv[]) {
    job_stream::runProcessor(argc, argv);
    return 0;
}


#include "PID_3DOF.h"
#include <Eigen/Dense>
using Eigen::Matrix;
//Sets initial errors to zero
void initializePID(PID_3DOF* PID){
	Matrix<float, 3, 1> zeros = Matrix<float, 3, 1>::Zero(3, 1);

	PID->e_prop = zeros;
	PID->e_deriv = zeros;
	PID->e_integ = zeros;
}

//Sets integral error to zero
void resetIntegralErrorPID(PID_3DOF* PID){
	PID->e_integ = Matrix<float, 3, 1>::Zero(3, 1);
}

//Update Kp, Ki and Kd in the PID
void updateControlParamPID(PID_3DOF* PID, Matrix<float, 3, 1> K_p, Matrix<float, 3, 1> K_i, Matrix<float, 3, 1> K_d, Matrix<float, 3, 1> maxInteg){
	PID->K_p = K_p;
	PID->K_d = K_d;
	PID->K_i = K_i;
	PID->maxInteg = maxInteg;
}

//Update all errors
void updateErrorPID(PID_3DOF* PID, Matrix<float, 3, 1> feedForward, Matrix<float, 3, 1> e_prop, Matrix<float, 3, 1> e_deriv, float dt){

	PID->feedForward = feedForward;
	PID->e_prop = e_prop;
	PID->e_deriv = e_deriv;
	PID->e_integ = PID->e_integ+(e_prop*dt); //e_integ = e_integ + e_prop*dt

	//Saturate integral error
	for (int i = 0; i < 3; i++){
		if (PID->e_integ(i) > PID->maxInteg(i)){
			PID->e_integ(i) = PID->maxInteg(i);
		}
		else if (PID->e_integ(i) < -PID->maxInteg(i)){
			PID->e_integ(i) = -PID->maxInteg(i);
		}
	}
}

//Calculate output of PID
Matrix<float, 3, 1> outputPID(PID_3DOF PID){
	Matrix<float, 3, 1> PID_out;
	PID_out =  PID.feedForward +
				PID.e_prop.cwiseProduct(PID.K_p) +
				PID.e_deriv.cwiseProduct(PID.K_d) +
				PID.e_integ.cwiseProduct(PID.K_i);
	return PID_out;
}

//Helper functions to parse the config file
void split(const string &s, char delim, vector<string> &elems) {
    stringstream ss(s);
    string item;
    while (getline(ss, item, delim)) {
        elems.push_back(item);
    }
}

vector<string> split(const string &s, char delim) {
    vector<string> elems;
    split(s, delim, elems);
    return elems;
}

void updatePar(PID_3DOF *PID_att, PID_3DOF *PID_angVel, PID_3DOF *PID_pos,char const *AttFile,char const *PosFile) {
	Matrix<float, 3, 1> Zero_3x1 = Matrix<float, 3, 1>::Zero(3, 1);
	Matrix<float, 3, 1> KP_RPY = Zero_3x1, KD_RPY = Zero_3x1, KI_RPY = Zero_3x1, maxInteg_RPY = Zero_3x1;
	Matrix<float, 3, 1> KP_w = Zero_3x1, KD_w = Zero_3x1, KI_w = Zero_3x1, maxInteg_w = Zero_3x1;
	Matrix<float, 3, 1> KP_Pos = Zero_3x1, KD_Pos = Zero_3x1, KI_Pos = Zero_3x1, maxInteg_Pos = Zero_3x1;
	char AttParamPath[64];
	char PosParamPath[64];
    string line;
    vector<string> line_vec;

    sprintf(AttParamPath,"/home/root/%s",AttFile);
    sprintf(PosParamPath,"/home/root/%s",PosFile);

    // printf("%s\n",AttParamPath);

    //Get parameters for attitude controller
    ifstream myfile (AttParamPath);
    if (myfile.is_open()) {
		while (getline (myfile ,line)) {
		    line_vec = split(line, ' ');
		    if (line_vec[0] == "KP_R") {
	            KP_RPY(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KP_P") {
				KP_RPY(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KP_Y") {
				KP_RPY(2) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KD_R") {
	            	KD_RPY(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KD_P") {
				KD_RPY(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KD_Y") {
				KD_RPY(2) = atof(line_vec[2].c_str());
		    }
		    if (line_vec[0] == "KI_R") {
	            KI_RPY(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KI_P") {
				KI_RPY(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KI_Y") {
				KI_RPY(2) = atof(line_vec[2].c_str());
		    }
		    if (line_vec[0] == "maxInteg_R") {
	            maxInteg_RPY(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "maxInteg_P") {
				maxInteg_RPY(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "maxInteg_Y") {
				maxInteg_RPY(2) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KP_wx") {
				KP_w(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KP_wy") {
				KP_w(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KP_wz") {
		        KP_w(2)  = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KD_wx") {
				KD_w(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KD_wy") {
				KD_w(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KD_wz") {
		        KD_w(2) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KI_wx") {
				KI_w(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KI_wy") {
				KI_w(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KI_wz") {
		        KI_w(2) = atof(line_vec[2].c_str());
		    }
		    if (line_vec[0] == "maxInteg_wx") {
	            maxInteg_w(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "maxInteg_wy") {
				maxInteg_w(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "maxInteg_wz") {
				maxInteg_w(2) = atof(line_vec[2].c_str());
		    }
		}
		myfile.close();
		updateControlParamPID(PID_att, KP_RPY, KI_RPY, KD_RPY, maxInteg_RPY);
		updateControlParamPID(PID_angVel, KP_w, KI_w, KD_w, maxInteg_w);
    }
    else {
		printf("Unable to open file %s \n",AttParamPath);
    }

    //Get parameters for position controller
    ifstream myfile2 (PosParamPath);
    if (myfile2.is_open()) {
		while (getline (myfile2 ,line)) {
		    line_vec = split(line, ' ');
		    if (line_vec[0] == "KP_X") {
	            KP_Pos(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KP_Y") {
				KP_Pos(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KP_Z") {
				KP_Pos(2) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KD_X") {
	            KD_Pos(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KD_Y") {
				KD_Pos(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KD_Z") {
				KD_Pos(2) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KI_X") {
	            KI_Pos(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KI_Y") {
				KI_Pos(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "KI_Z") {
				KI_Pos(2) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "maxInteg_X") {
	            maxInteg_Pos(0) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "maxInteg_Y") {
				maxInteg_Pos(1) = atof(line_vec[2].c_str());
		    }
		    else if (line_vec[0] == "maxInteg_Z") {
				maxInteg_Pos(2) = atof(line_vec[2].c_str());
		    }
		}
		myfile2.close();
		updateControlParamPID(PID_pos, KP_Pos, KI_Pos, KD_Pos, maxInteg_Pos);
    }
    else {
		printf("Unable to open file %s \n", PosParamPath);
    }

	printf("Done updating control parameters\n");
}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This is an example illustrating the use of the Bayesian Network
    inference utilities found in the dlib C++ library.

    In this example all the nodes in the Bayesian network are
    boolean variables.  That is, they take on either the value
    0 or the value 1.

    The network contains 4 nodes and looks as follows:

        B     C
        \\   //
         \/ \/
           A
           ||
           \/
            D

    The probabilities of each node are summarized below.  (The probability
    of each node being 0 is not listed since it is just P(X=0) = 1-p(X=1) )

        p(B=1) = 0.01

        p(C=1) = 0.001

        p(A=1 | B=0, C=0) = 0.01
        p(A=1 | B=0, C=1) = 0.5
        p(A=1 | B=1, C=0) = 0.9
        p(A=1 | B=1, C=1) = 0.99

        p(D=1 | A=0) = 0.2
        p(D=1 | A=1) = 0.5

*/

#include <dlib/bayes_utils.h>
#include <dlib/graph_utils.h>
#include <dlib/graph.h>
#include <dlib/directed_graph.h>
#include <iostream>

using namespace dlib;
using namespace std;

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

int main()
{
    try
    {
        // There are many useful convenience functions in this namespace.  They all
        // perform simple access or modify operations on the nodes of a bayesian network.
        // You don't have to use them but they are convenient and they also will check for
        // various errors in your bayesian network when your application is built with
        // the DEBUG or ENABLE_ASSERTS preprocessor definitions defined.  So their use
        // is recommended.  In fact, most of the global functions used in this example
        // program are from this namespace.
        using namespace bayes_node_utils;

        // This statement declares a bayesian network called bn.  Note that a bayesian network
        // in the dlib world is just a directed_graph object that contains a special kind
        // of node called a bayes_node.
        directed_graph<bayes_node>::kernel_1a_c bn;

        // Use an enum to make some more readable names for our nodes.
        enum nodes
        {
            A = 0,
            B = 1,
            C = 2,
            D = 3
        };

        // The next few blocks of code setup our bayesian network.

        // The first thing we do is tell the bn object how many nodes it has
        // and also add the three edges.  Again, we are using the network
        // shown in ASCII art at the top of this file.
        bn.set_number_of_nodes(4);
        bn.add_edge(A, D);
        bn.add_edge(B, A);
        bn.add_edge(C, A);

        // Now we inform all the nodes in the network that they are binary
        // nodes.  That is, they only have two possible values.
        set_node_num_values(bn, A, 2);
        set_node_num_values(bn, B, 2);
        set_node_num_values(bn, C, 2);
        set_node_num_values(bn, D, 2);

        assignment parent_state;
        // Now we will enter all the conditional probability information for each node.
        // Each node's conditional probability is dependent on the state of its parents.
        // To specify this state we need to use the assignment object.  This assignment
        // object allows us to specify the state of each nodes parents.

        // Here we specify that p(B=1) = 0.01
        // parent_state is empty in this case since B is a root node.
        set_node_probability(bn, B, 1, parent_state, 0.01);
        // Here we specify that p(B=0) = 1-0.01
        set_node_probability(bn, B, 0, parent_state, 1-0.01);

        // Here we specify that p(C=1) = 0.001
        // parent_state is empty in this case since B is a root node.
        set_node_probability(bn, C, 1, parent_state, 0.001);
        // Here we specify that p(C=0) = 1-0.001
        set_node_probability(bn, C, 0, parent_state, 1-0.001);

        // This is our first node that has parents. So we set the parent_state
        // object to reflect that A has both B and C as parents.
        parent_state.add(B, 1);
        parent_state.add(C, 1);
        // Here we specify that p(A=1 | B=1, C=1) = 0.99
        set_node_probability(bn, A, 1, parent_state, 0.99);
        // Here we specify that p(A=0 | B=1, C=1) = 1-0.99
        set_node_probability(bn, A, 0, parent_state, 1-0.99);

        // Here we use the [] notation because B and C have already
        // been added into parent state.
        parent_state[B] = 1;
        parent_state[C] = 0;
        // Here we specify that p(A=1 | B=1, C=0) = 0.9
        set_node_probability(bn, A, 1, parent_state, 0.9);
        set_node_probability(bn, A, 0, parent_state, 1-0.9);

        parent_state[B] = 0;
        parent_state[C] = 1;
        // Here we specify that p(A=1 | B=0, C=1) = 0.5
        set_node_probability(bn, A, 1, parent_state, 0.5);
        set_node_probability(bn, A, 0, parent_state, 1-0.5);

        parent_state[B] = 0;
        parent_state[C] = 0;
        // Here we specify that p(A=1 | B=0, C=0) = 0.01
        set_node_probability(bn, A, 1, parent_state, 0.01);
        set_node_probability(bn, A, 0, parent_state, 1-0.01);

        // Here we set probabilities for node D.
        // First we clear out parent state so that it doesn't have any of
        // the assignments for the B and C nodes used above.
        parent_state.clear();
        parent_state.add(A,1);
        // Here we specify that p(D=1 | A=1) = 0.5
        set_node_probability(bn, D, 1, parent_state, 0.5);
        set_node_probability(bn, D, 0, parent_state, 1-0.5);

        parent_state[A] = 0;
        // Here we specify that p(D=1 | A=0) = 0.2
        set_node_probability(bn, D, 1, parent_state, 0.2);
        set_node_probability(bn, D, 0, parent_state, 1-0.2);

        // We have now finished setting up our bayesian network.  So lets compute some
        // probability values.  The first thing we will do is compute the prior probability
        // of each node in the network.  To do this we will use the join tree algorithm which
        // is an algorithm for performing exact inference in a bayesian network.

        // First we need to create an undirected graph which contains set objects at each node and
        // edge.  This long declaration does the trick.
        typedef dlib::set<unsigned long>::compare_1b_c set_type;
        typedef graph<set_type, set_type>::kernel_1a_c join_tree_type;
        join_tree_type join_tree;

        // Now we need to populate the join_tree with data from our bayesian network.  The next
        // function calls do this.  Explaining exactly what they do is outside the scope of this
        // example.  Just think of them as filling join_tree with information that is useful
        // later on for dealing with our bayesian network.
        create_moral_graph(bn, join_tree);
        create_join_tree(join_tree, join_tree);

        // Now that we have a proper join_tree we can use it to obtain a solution to our
        // bayesian network.  Doing this is as simple as declaring an instance of
        // the bayesian_network_join_tree object as follows:
        bayesian_network_join_tree solution(bn, join_tree);

        // now print out the probabilities for each node
        cout << "Using the join tree algorithm:\n";
        cout << "p(A=1) = " << solution.probability(A)(1) << endl;
        cout << "p(A=0) = " << solution.probability(A)(0) << endl;
        cout << "p(B=1) = " << solution.probability(B)(1) << endl;
        cout << "p(B=0) = " << solution.probability(B)(0) << endl;
        cout << "p(C=1) = " << solution.probability(C)(1) << endl;
        cout << "p(C=0) = " << solution.probability(C)(0) << endl;
        cout << "p(D=1) = " << solution.probability(D)(1) << endl;
        cout << "p(D=0) = " << solution.probability(D)(0) << endl;
        cout << "\n\n\n";

        // Now to make things more interesting lets say that we have discovered that the C
        // node really has a value of 1.  That is to say, we now have evidence that
        // C is 1.  We can represent this in the network using the following two function
        // calls.
        set_node_value(bn, C, 1);
        set_node_as_evidence(bn, C);

        // Now we want to compute the probabilities of all the nodes in the network again
        // given that we now know that C is 1.  We can do this as follows:
        bayesian_network_join_tree solution_with_evidence(bn, join_tree);

        // now print out the probabilities for each node
        cout << "Using the join tree algorithm:\n";
        cout << "p(A=1 | C=1) = " << solution_with_evidence.probability(A)(1) << endl;
        cout << "p(A=0 | C=1) = " << solution_with_evidence.probability(A)(0) << endl;
        cout << "p(B=1 | C=1) = " << solution_with_evidence.probability(B)(1) << endl;
        cout << "p(B=0 | C=1) = " << solution_with_evidence.probability(B)(0) << endl;
        cout << "p(C=1 | C=1) = " << solution_with_evidence.probability(C)(1) << endl;
        cout << "p(C=0 | C=1) = " << solution_with_evidence.probability(C)(0) << endl;
        cout << "p(D=1 | C=1) = " << solution_with_evidence.probability(D)(1) << endl;
        cout << "p(D=0 | C=1) = " << solution_with_evidence.probability(D)(0) << endl;
        cout << "\n\n\n";

        // Note that when we made our solution_with_evidence object we reused our join_tree object.
        // This saves us the time it takes to calculate the join_tree object from scratch.  But
        // it is important to note that we can only reuse the join_tree object if we haven't changed
        // the structure of our bayesian network.  That is, if we have added or removed nodes or
        // edges from our bayesian network then we must recompute our join_tree.  But in this example
        // all we did was change the value of a bayes_node object (we made node C be evidence)
        // so we are ok.

        // Next this example will show you how to use the bayesian_network_gibbs_sampler object
        // to perform approximate inference in a bayesian network.  This is an algorithm
        // that doesn't give you an exact solution but it may be necessary to use in some
        // instances.  For example, the join tree algorithm used above, while fast in many
        // instances, has exponential runtime in some cases.  Moreover, inference in bayesian
        // networks is NP-Hard for general networks so sometimes the best you can do is
        // find an approximation.
        // However, it should be noted that the gibbs sampler does not compute the correct
        // probabilities if the network contains a deterministic node.  That is, if any
        // of the conditional probability tables in the bayesian network have a probability
        // of 1.0 for something the gibbs sampler should not be used.

        // This Gibbs sampler algorithm works by randomly sampling possibles values of the
        // network.  So to use it we should set the network to some initial state.

        set_node_value(bn, A, 0);
        set_node_value(bn, B, 0);
        set_node_value(bn, D, 0);

        // We will leave the C node with a value of 1 and keep it as an evidence node.

        // First create an instance of the gibbs sampler object
        bayesian_network_gibbs_sampler sampler;

        // To use this algorithm all we do is go into a loop for a certain number of times
        // and each time through we sample the bayesian network.  Then we count how
        // many times a node has a certain state.  Then the probability of that node
        // having that state is just its count/total times through the loop.

        // The following code illustrates the general procedure.
        unsigned long A_count = 0;
        unsigned long B_count = 0;
        unsigned long C_count = 0;
        unsigned long D_count = 0;

        // The more times you let the loop run the more accurate the result will be.  Here we loop
        // 2000 times.
        const long rounds = 2000;
        for (long i = 0; i < rounds; ++i)
        {
            sampler.sample_graph(bn);

            if (node_value(bn, A) == 1)
                ++A_count;
            if (node_value(bn, B) == 1)
                ++B_count;
            if (node_value(bn, C) == 1)
                ++C_count;
            if (node_value(bn, D) == 1)
                ++D_count;
        }

        cout << "Using the approximate Gibbs Sampler algorithm:\n";
        cout << "p(A=1 | C=1) = " << (double)A_count/(double)rounds << endl;
        cout << "p(B=1 | C=1) = " << (double)B_count/(double)rounds << endl;
        cout << "p(C=1 | C=1) = " << (double)C_count/(double)rounds << endl;
        cout << "p(D=1 | C=1) = " << (double)D_count/(double)rounds << endl;
    }
    catch (std::exception& e)
    {
        cout << "exception thrown: " << endl;
        cout << e.what() << endl;
        cout << "hit enter to terminate" << endl;
        cin.get();
    }
}


#include <iostream>
#include <fstream>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/adapted/boost_array.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector.hpp>
BOOST_GEOMETRY_REGISTER_BOOST_ARRAY_CS(cs::cartesian)

#include <enne-stage/constants.hpp>

#include"/usr/local/pykep/src/lambert_problem.cpp"

int main()
{
double MU_EARTH = enne_stage::ASTRO_ENNE_GM_EARTH;// m^3/s^2

// 200 km, 30deg, quarter orbit
boost::array< double, 3> r1 = {6778136,0,0};
boost::array< double, 3> r2 = {0,6043243.047,3489068};
boost::array< double, 3> v0 = {0,7668.558733,0};
boost::array< double, 3> v3 = {-7557.86574,0,0};

// // Hohmann transfer initial conditions.
// boost::array< double, 3> r1 = {6778136,0,0};
// boost::array< double, 3> r2 = {-6978136,0.00007,0};
// boost::array< double, 3> v0 = {0,7668.558733,0};
// boost::array< double, 3> v3 = {0.000000008,-7557.86574,0};

double r1a = std::sqrt(std::pow(r1[0],2.0)+std::pow(r1[1],2.0)+std::pow(r1[2],2.0));
double dt_orbit =  2*enne_stage::kPI * std::sqrt(std::pow((r1a),3.0)/MU_EARTH);

double start = 500;
const int amount = 100000;

boost::array< double, amount> toff;

std::ofstream myfile;
myfile.open ("../data/i.txt");
for (int i=start; i<start+amount; i++) myfile << i / 10000.0 << std::endl;
myfile.close();

for (int j = 0; j < 5; ++j)
{
	boost::array< double, amount> dvtotarray;
	boost::array< double, amount> toff;

	for (int i = start; i < start + amount; ++i)
	{
		const double a = i / 10000.0;
		double dt = dt_orbit * a;
		kep_toolbox::lambert_problem test( r1,r2, dt, MU_EARTH,0,j);
		int	nmax = test.lambert_problem::get_Nmax();

		if (nmax > 0 && j>0)
		{
		std::vector<boost::array< double, 3> > v1;
		v1 = test.lambert_problem::get_v1();
	    double dv1 = boost::geometry::distance(v1[nmax*2-1], v0);

		std::vector<boost::array< double, 3> > v2;
		v2 = test.lambert_problem::get_v2();
	    double dv2 = boost::geometry::distance(v3, v2[nmax*2-1]);

	    double dvtot = dv1+dv2;

		toff[i-start] = a;
		dvtotarray[i-start] = dvtot;
		}

		if (nmax == 0 && j==0)
		{
		std::vector<boost::array< double, 3> > v1;
		v1 = test.lambert_problem::get_v1();
	    double dv1 = boost::geometry::distance(v1[0], v0);

		std::vector<boost::array< double, 3> > v2;
		v2 = test.lambert_problem::get_v2();
	    double dv2 = boost::geometry::distance(v3, v2[0]);

	    double dvtot = dv1+dv2;

		toff[i-start] = a;
		dvtotarray[i-start] = dvtot;
		}

		if (nmax == 0 && j>0)
		{
		toff[i-start] = a;
		dvtotarray[i-start] = 100000;
		}
		if (nmax == 1 && j>1)
		{
		toff[i-start] = a;
		dvtotarray[i-start] = 100000;
		}
		if (nmax == 2 && j>2)
		{
		toff[i-start] = a;
		dvtotarray[i-start] = 100000;
		}
		if (nmax == 3 && j>3)
		{
		toff[i-start] = a;
		dvtotarray[i-start] = 100000;
		}
	}
	std::string filename;
	std::ostringstream convert;
	convert << j;
	filename = "../data/dv_" + convert.str();
	filename += ".txt";
  	std::ofstream myfile1;
    myfile1.open (filename);
	for (int i=0; i<amount; i++) myfile1 << dvtotarray[i] << std::endl;
  	myfile1.close();

}

  return 0;
}


#include <hpp/fcl/math/sampling.h>
#include <boost/random/lagged_fibonacci.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/math/constants/constants.hpp>

namespace fcl
{

/// The seed the user asked for (cannot be 0)
static boost::uint32_t userSetSeed = 0;

/// Flag indicating whether the first seed has already been generated or not
static bool firstSeedGenerated = false;

/// The value of the first seed
static boost::uint32_t firstSeedValue = 0;

/// Compute the first seed to be used; this function should be called only once
static boost::uint32_t firstSeed()
{
  static boost::mutex fsLock;
  boost::mutex::scoped_lock slock(fsLock);

  if(firstSeedGenerated)
    return firstSeedValue;

  if(userSetSeed != 0)
    firstSeedValue = userSetSeed;
  else firstSeedValue = (boost::uint32_t)(boost::posix_time::microsec_clock::universal_time() - boost::posix_time::ptime(boost::date_time::min_date_time)).total_microseconds();
  firstSeedGenerated = true;

  return firstSeedValue;
}

/// We use a different random number generator for the seeds of the
/// Other random generators. The root seed is from the number of
/// nano-seconds in the current time.
static boost::uint32_t nextSeed()
{
  static boost::mutex rngMutex;
  boost::mutex::scoped_lock slock(rngMutex);
  static boost::lagged_fibonacci607 sGen(firstSeed());
  static boost::uniform_int<> sDist(1, 1000000000);
  static boost::variate_generator<boost::lagged_fibonacci607&, boost::uniform_int<> > s(sGen, sDist);
  return s();
}

boost::uint32_t RNG::getSeed()
{
  return firstSeed();
}

void RNG::setSeed(boost::uint32_t seed)
{
  if(firstSeedGenerated)
  {
    std::cerr << "Random number generation already started. Changing seed now will not lead to deterministic sampling." << std::endl;
  }
  if(seed == 0)
  {
    std::cerr << "Random generator seed cannot be 0. Using 1 instead." << std::endl;
    userSetSeed = 1;
  }
  else
    userSetSeed = seed;
}

RNG::RNG() : generator_(nextSeed()),
                 uniDist_(0, 1),
                 normalDist_(0, 1),
                 uni_(generator_, uniDist_),
                 normal_(generator_, normalDist_)
{
}

double RNG::halfNormalReal(double r_min, double r_max, double focus)
{
  assert(r_min <= r_max);

  const double mean = r_max - r_min;
  double v = gaussian(mean, mean / focus);

  if(v > mean) v = 2.0 * mean - v;
  double r = v >= 0.0 ? v + r_min : r_min;
  return r > r_max ? r_max : r;
}

int RNG::halfNormalInt(int r_min, int r_max, double focus)
{
  int r = (int)floor(halfNormalReal((double)r_min, (double)(r_max) + 1.0, focus));
  return (r > r_max) ? r_max : r;
}

// From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III,
//       pg. 124-132
void RNG::quaternion(double value[4])
{
  double x0 = uni_();
  double r1 = sqrt(1.0 - x0), r2 = sqrt(x0);
  double t1 = 2.0 * boost::math::constants::pi<double>() * uni_(), t2 = 2.0 * boost::math::constants::pi<double>() * uni_();
  double c1 = cos(t1), s1 = sin(t1);
  double c2 = cos(t2), s2 = sin(t2);
  value[0] = s1 * r1;
  value[1] = c1 * r1;
  value[2] = s2 * r2;
  value[3] = c2 * r2;
}

// From Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning, by James Kuffner, ICRA 2004
void RNG::eulerRPY(double value[3])
{
  value[0] = boost::math::constants::pi<double>() * (2.0 * uni_() - 1.0);
  value[1] = acos(1.0 - 2.0 * uni_()) - boost::math::constants::pi<double>() / 2.0;
  value[2] = boost::math::constants::pi<double>() * (2.0 * uni_() - 1.0);
}

void RNG::disk(double r_min, double r_max, double& x, double& y)
{
  double a = uniform01();
  double b = uniform01();
  double r = std::sqrt(a * r_max * r_max + (1 - a) * r_min * r_min);
  double theta = 2 * boost::math::constants::pi<double>() * b;
  x = r * std::cos(theta);
  y = r * std::sin(theta);
}

void RNG::ball(double r_min, double r_max, double& x, double& y, double& z)
{
  double a = uniform01();
  double b = uniform01();
  double c = uniform01();
  double r = std::pow(a * r_max * r_max * r_max + (1 - a) * r_min * r_min * r_min, 1 / 3.0);
  double theta = std::acos(1 - 2 * b);
  double phi = 2 * boost::math::constants::pi<double>() * c;

  double costheta = std::cos(theta);
  double sintheta = std::sin(theta);
  double cosphi = std::cos(phi);
  double sinphi = std::sin(phi);
  x = r * costheta;
  y = r * sintheta * cosphi;
  z = r * sintheta * sinphi;
}

}


#include <Eigen/Core>

USING_PART_OF_NAMESPACE_EIGEN

namespace Eigen {

/* Echelon a matrix in-place:
 *
 * Meta-Unrolled version, for small fixed-size matrices
 */
template<typename Derived, int Step>
struct unroll_echelon
{
  enum { k = Step - 1,
         Rows = Derived::RowsAtCompileTime,
         Cols = Derived::ColsAtCompileTime,
         CornerRows = Rows - k,
         CornerCols = Cols - k
  };
  static void run(MatrixBase<Derived>& m)
  {
    unroll_echelon<Derived, Step-1>::run(m);
    int rowOfBiggest, colOfBiggest;
    m.template corner<CornerRows, CornerCols>(BottomRight)
     .cwise().abs()
     .maxCoeff(&rowOfBiggest, &colOfBiggest);
    m.row(k).swap(m.row(k+rowOfBiggest));
    m.col(k).swap(m.col(k+colOfBiggest));
    m.template corner<CornerRows-1, CornerCols>(BottomRight)
      -= m.col(k).template end<CornerRows-1>()
       * (m.row(k).template end<CornerCols>() / m(k,k));
  }
};

template<typename Derived>
struct unroll_echelon<Derived, 0>
{
  static void run(MatrixBase<Derived>& m) {}
};

/* Echelon a matrix in-place:
 *
 * Non-unrolled version, for dynamic-size matrices.
 * (this version works for all matrices, but in the fixed-size case the other
 * version is faster).
 */
template<typename Derived>
struct unroll_echelon<Derived, Dynamic>
{
  static void run(MatrixBase<Derived>& m)
  {
    for(int k = 0; k < m.diagonal().size() - 1; k++)
    {
      int rowOfBiggest, colOfBiggest;
      int cornerRows = m.rows()-k, cornerCols = m.cols()-k;
      m.corner(BottomRight, cornerRows, cornerCols)
      .cwise().abs()
      .maxCoeff(&rowOfBiggest, &colOfBiggest);
      m.row(k).swap(m.row(k+rowOfBiggest));
      m.col(k).swap(m.col(k+colOfBiggest));
      m.corner(BottomRight, cornerRows-1, cornerCols)
        -= m.col(k).end(cornerRows-1) * (m.row(k).end(cornerCols) / m(k,k));
    }
  }
};

using namespace std;
template<typename Derived>
void echelon(MatrixBase<Derived>& m)
{
  const int size = DiagonalCoeffs<Derived>::SizeAtCompileTime;
  const bool unroll = size <= 4;
  unroll_echelon<Derived, unroll ? size-1 : Dynamic>::run(m);
}

template<typename Derived>
void doSomeRankPreservingOperations(MatrixBase<Derived>& m)
{
  for(int a = 0; a < 3*(m.rows()+m.cols()); a++)
  {
    double d = ei_random<double>(-1,1);
    int i = ei_random<int>(0,m.rows()-1); // i is a random row number
    int j;
    do {
      j = ei_random<int>(0,m.rows()-1);
    } while (i==j); // j is another one (must be different)
    m.row(i) += d * m.row(j);

    i = ei_random<int>(0,m.cols()-1); // i is a random column number
    do {
      j = ei_random<int>(0,m.cols()-1);
    } while (i==j); // j is another one (must be different)
    m.col(i) += d * m.col(j);
  }
}

} // namespace Eigen

using namespace std;

int main(int, char **)
{
  srand((unsigned int)time(0));
  const int Rows = 6, Cols = 4;
  typedef Matrix<double, Rows, Cols> Mat;
  const int N = Rows < Cols ? Rows : Cols;

  // start with a matrix m that's obviously of rank N-1
  Mat m = Mat::identity(Rows, Cols); // args just in case of dyn. size
  m.row(0) = m.row(1) = m.row(0) + m.row(1);

  doSomeRankPreservingOperations(m);

  // now m is still a matrix of rank N-1
  cout << "Here's the matrix m:" << endl << m << endl;

  cout << "Now let's echelon m (repeating many times for benchmarking purposes):" << endl;
  for(int i = 0; i < 1000000; i++) echelon(m);

  cout << "Now m is:" << endl << m << endl;
}