#include <gnuplot-iostream/gnuplot-iostream.h>
#include <boost/tuple/tuple.hpp>
#include <iostream>
#include <iomanip>
#include <functional>
#include <vector>
#include <cmath>
#include "Tools.hpp"
#define PI 3.14159265
using namespace std;

/***********************************************************************

Second Case: 2D drag! DragObject2D is a class for a point particle in
2D coordinates. Instead of individual accessor methods like Euler1D,
getAttrs returns a vector of the four instance variables. The Euler
step and updating code is all built in as well. A frequently used data
type in this program is vector<vector<double>>, usually called path.
The data stored is a vector of x values and y values in a projectile's
trajectory, ex. {{x1,x2,x3...},{y1,y2,y3...}}. Passing a data type like
this around is much easier than writing two methods or using evil
libraries to make functions return multiple values.

The getPath function takes an initial x and y velocity and performs the
Euler method on an instance of DragObject2D, saving the x and y values
in vectors. It simulates until a sign change is detected in y, meaning
the object has returned to the ground. For that reason,
making v_i negative is a bad idea. After simulation, the function
returns a path. plotPath just plots the path given with axes of path[0],
path[1].

RangeVAngle is more complicated, as it performs all calculation and
plots the range vs. angle graph. It iterates from -pi/2 to pi/2 with a
step size of dth. For each iteration, it gets the path with getPath. The
final x value is the range when the object returns to the ground, by
the definition of the getPath function.

***********************************************************************/

const double v_i = 10;		// Initial velocity for both the trajectory and the range v. angle plot
const double th = PI/6;		// Firing angle for the trajectory plot
const double y_0 = 1;		// Global starting height, Should be either zero or something positive, as the range vs. angle graph must be complete.

const double g = 9.8;		// Gravitational acceleration (negative)
const double b = 0;//.56832;	// Calculated drag constant for Syd Miyasaki
const double dt = 0.001;	// Timestep
const double dth = 0.001;	// Step in angle for the range v. angle plot

class DragObject2D{	// Problem-specific class
public:
	DragObject2D(double vx_0, double vy_0);	// Constructor
	vector<double> getAttrs() const;		// Accessor method for instance variables
	double getSpeed() const;				// Accessor / calculation method for speed
	void update();	// Euler step and updating
private:
	double x, y, vx, vy;
};

DragObject2D::DragObject2D(double vx_0, double vy_0){	// Constructor sets x to zero, y to starting height, initial velocities as arguments
	x = 0;
	y = y_0;
	vx = vx_0;
	vy = vy_0;
}

vector<double> DragObject2D::getAttrs() const {	// Usage of anonymously declared vectors to return all instance variables
	return vector<double>{x,y,vx,vy};
}

double DragObject2D::getSpeed() const {	// *trivial* mechanics (component velocity to speed)
	return sqrt(vx*vx + vy*vy);
}

void DragObject2D::update(){	// Update all the things!
	double v = this->getSpeed();// Get speed on the current object instance. "this" returns a pointer to the instance, and -> is an abbreviation of (*this).getSpeed()
	x += vx*dt;				// Update x and y
	y += vy*dt;

	vx += -(b*v*vx)*dt;		// Euler step for x and y
	vy += -(g+b*v*vy)*dt;
}

vector<vector<double>> getPath(double vx_0, double vy_0){	// Get the path thing
	vector<double> x_n;	// Vectors for the path
	vector<double> y_n;
	double yval = 0;	// The value of y

	DragObject2D object(vx_0, vy_0);// The object to track

	while(yval >= 0){				// Only track until the object hits the ground
		vector<double> attrs = object.getAttrs();	// Get attributes (x, y, vx, vy)
		x_n.push_back(attrs[0]);	// Add attributes to vectors, update y value
		y_n.push_back(attrs[1]);
		yval = attrs[1];

		object.update();			// Perform Euler step and updates
	}

	return vector<vector<double>>{x_n,y_n};	// Return the two vectors of x and y together
}

void plotPath(vector<vector<double>>& path){	// Plot the path; pretty basic stuff
	Gnuplot gp;

	gp << setprecision(3);
	gp << "set xrange [0:" << path[0].back() << "]\n";
	gp << "set yrange [0:" << getMaxVal(path[1])*1.1 << "]\n";
	gp << "set format y \"%.1f\"\n";
	gp << "set term png size 720,480 font \"FreeSerif,12\"\n";
	gp << "set xlabel \"x (m)\"\n";
	gp << "set ylabel \"y (m)\"\n";
	gp << "set title \"Trajectory of Syd Under Rayleigh's Drag Equation\\nb = " << b;
	gp << ", v_i = " << v_i << ", {/Symbol q}_i = " << th << "\"\n";
	gp << "set output \"Trajectory.png\"\n";
	gp << "plot '-' with dots lc rgb \"black\" notitle\n";
	gp.send1d(boost::make_tuple(path[0],path[1]));
}

void rangeVAngle(){			// Calculation and plotting of the range vs. angle plot
	vector<double> ranges;	// Declare vectors
	vector<double> angles;

	for(double ang = -PI/2; ang < PI/2; ang += dth){	// From -pi/2 to pi/2, steps of dth

		double vx_0 = v_i*cos(ang);	// From v_i and ang, determine the initial velocities
		double vy_0 = v_i*sin(ang);
		vector<vector<double>> path = getPath(vx_0, vy_0);	// Generate path
		double range = path[0].back();	// The last x value is the range

		angles.push_back(ang);	// Save angle and range
		ranges.push_back(range);
	}

	Gnuplot gp;		// Now plot!

	gp << setprecision(3);
	gp << "set xrange [" << -PI/2 << ":" << PI/2 << "]\n";
	gp << "set yrange [0:" << getMaxVal(ranges)*1.1 << "]\n";
	gp << "set format y \"%.1f\"\n";
	gp << "set term png size 720,480 font \"FreeSerif,12\"\n";
	gp << "set xlabel \"angle (rad)\"\n";
	gp << "set ylabel \"range (m)\"\n";
	gp << "set title \"Range vs. Firing Angle of Syd Under Rayleigh's Drag Equation, b = " << b << "\"\n";
	gp << "set output \"RangeAngle.png\"\n";
	gp << "plot '-' with dots lc rgb \"black\" notitle\n";
	gp.send1d(boost::make_tuple(angles,ranges));
}

int main(){		// Main function
	double vx_0 = v_i*cos(th);	// Use v_i and the launch angle th to determine initial velocities by components
	double vy_0 = v_i*sin(th);
	vector<vector<double>> path = getPath(vx_0, vy_0);	// Generate the path
	plotPath(path);	// Plot it

	rangeVAngle();	// And make the range vs. angle plot!

	return 0;	// If it all worked, return 0
}


/*
 * CurvatureFilter.cpp
 *
 *  Created on: Sep 23, 2017
 *      Author: Peter Fankhauser
 *   Institute: ETH Zurich, ANYbotics
 */

#include <grid_map_filters/CurvatureFilter.hpp>

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

#include <Eigen/Dense>

using namespace filters;

namespace grid_map {

template<typename T>
CurvatureFilter<T>::CurvatureFilter()
{
}

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

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

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

  return true;
}

template<typename T>
bool CurvatureFilter<T>::update(const T& mapIn, T& mapOut)
{
  if (!mapIn.isDefaultStartIndex()) throw std::runtime_error(
      "CurvatureFilter cannot be used with grid maps that don't have a default buffer start index.");

  mapOut = mapIn;
  mapOut.add(outputLayer_);
  auto& input = mapOut[inputLayer_];
  auto& curvature = mapOut[outputLayer_];
  const float L2 = mapOut.getResolution() * mapOut.getResolution();

  for (Eigen::Index j{0}; j < input.cols(); ++j) {
    for (Eigen::Index i{0}; i < input.rows(); ++i) {
      // http://help.arcgis.com/en/arcgisdesktop/10.0/help/index.html#/How_Curvature_works/00q90000000t000000/
      if (!std::isfinite(input(i, j))) continue;
      float D = ((input(i, j==0 ? j : j-1) + input(i, j==input.cols()-1 ? j : j + 1)) / 2.0 - input(i, j)) / L2;
      float E = ((input(i==0 ? i : i-1, j) + input(i==input.rows()-1 ? i : i + 1, j)) / 2.0 - input(i, j)) / L2;
      if (!std::isfinite(D)) D = 0.0;
      if (!std::isfinite(E)) E = 0.0;
      curvature(i, j) = -2.0 * (D + E);
    }
  }

  return true;
}

} /* namespace */

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


/* Boost example/filter.cpp
 * two examples of filters for computing the sign of a determinant
 * the second filter is based on an idea presented in
 * "Interval arithmetic yields efficient dynamic filters for computational
 * geometry" by Brönnimann, Burnikel and Pion, 2001
 *
 * Copyright 2003 Guillaume Melquiond
 *
 * 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)
 */

#include <boost/numeric/interval.hpp>
#include <cstring>
#include <iostream>

namespace dummy {
  using namespace boost;
  using namespace numeric;
  using namespace interval_lib;
  typedef save_state<rounded_arith_opp<double> > R;
  typedef checking_no_nan<double, checking_no_empty<double> > P;
  typedef interval<double, policies<R, P> > I;
}

template<class T>
class vector {
  T* ptr;
public:
  vector(int d) { ptr = (T*)malloc(sizeof(T) * d); }
  ~vector() { free(ptr); }
  const T& operator[](int i) const { return ptr[i]; }
  T& operator[](int i) { return ptr[i]; }
};

template<class T>
class matrix {
  int dim;
  T* ptr;
public:
  matrix(int d): dim(d) { ptr = (T*)malloc(sizeof(T) * dim * dim); }
  ~matrix() { free(ptr); }
  int get_dim() const { return dim; }
  void assign(const matrix<T> &a) { memcpy(ptr, a.ptr, sizeof(T) * dim * dim); }
  const T* operator[](int i) const { return &(ptr[i * dim]); }
  T* operator[](int i) { return &(ptr[i * dim]); }
};

typedef dummy::I I_dbl;

/* compute the sign of a determinant using an interval LU-decomposition; the
   function answers 1 or -1 if the determinant is positive or negative (and
   more importantly, the result must be provable), or 0 if the algorithm was
   unable to get a correct sign */
int det_sign_algo1(const matrix<double> &a) {
  int dim = a.get_dim();
  vector<int> p(dim);
  for(int i = 0; i < dim; i++) p[i] = i;
  int sig = 1;
  I_dbl::traits_type::rounding rnd;
  typedef boost::numeric::interval_lib::unprotect<I_dbl>::type I;
  matrix<I> u(dim);
  for(int i = 0; i < dim; i++) {
    const double* line1 = a[i];
    I* line2 = u[i];
    for(int j = 0; j < dim; j++)
      line2[j] = line1[j];
  }
  // computation of L and U
  for(int i = 0; i < dim; i++) {
    // partial pivoting
    {
      int pivot = i;
      double max = 0;
      for(int j = i; j < dim; j++) {
        const I &v = u[p[j]][i];
        if (zero_in(v)) continue;
        double m = norm(v);
        if (m > max) { max = m; pivot = j; }
      }
      if (max == 0) return 0;
      if (pivot != i) {
        sig = -sig;
        int tmp = p[i];
        p[i] = p[pivot];
        p[pivot] = tmp;
      }
    }
    // U[i,?]
    {
      I *line1 = u[p[i]];
      const I &pivot = line1[i];
      if (boost::numeric::interval_lib::cerlt(pivot, 0.)) sig = -sig;
      for(int k = i + 1; k < dim; k++) {
        I *line2 = u[p[k]];
        I fact = line2[i] / pivot;
        for(int j = i + 1; j < dim; j++) line2[j] -= fact * line1[j];
      }
    }
  }
  return sig;
}

/* compute the sign of a determinant using a floating-point LU-decomposition
   and an a posteriori interval validation; the meaning of the answer is the
   same as previously */
int det_sign_algo2(const matrix<double> &a) {
  int dim = a.get_dim();
  vector<int> p(dim);
  for(int i = 0; i < dim; i++) p[i] = i;
  int sig = 1;
  matrix<double> lui(dim);
  {
    // computation of L and U
    matrix<double> lu(dim);
    lu.assign(a);
    for(int i = 0; i < dim; i++) {
      // partial pivoting
      {
        int pivot = i;
        double max = std::abs(lu[p[i]][i]);
        for(int j = i + 1; j < dim; j++) {
          double m = std::abs(lu[p[j]][i]);
          if (m > max) { max = m; pivot = j; }
        }
        if (max == 0) return 0;
        if (pivot != i) {
          sig = -sig;
          int tmp = p[i];
          p[i] = p[pivot];
          p[pivot] = tmp;
        }
      }
      // L[?,i] and U[i,?]
      {
        double *line1 = lu[p[i]];
        double pivot = line1[i];
        if (pivot < 0) sig = -sig;
        for(int k = i + 1; k < dim; k++) {
          double *line2 = lu[p[k]];
          double fact = line2[i] / pivot;
          line2[i] = fact;
          for(int j = i + 1; j < dim; j++) line2[j] -= line1[j] * fact;
        }
      }
    }

    // computation of approximate inverses: Li and Ui
    for(int j = 0; j < dim; j++) {
      for(int i = j + 1; i < dim; i++) {
        double *line = lu[p[i]];
        double s = - line[j];
        for(int k = j + 1; k < i; k++) s -= line[k] * lui[k][j];
        lui[i][j] = s;
      }
      lui[j][j] = 1 / lu[p[j]][j];
      for(int i = j - 1; i >= 0; i--) {
        double *line = lu[p[i]];
        double s = 0;
        for(int k = i + 1; k <= j; k++) s -= line[k] * lui[k][j];
        lui[i][j] = s / line[i];
      }
    }
  }

  // norm of PAUiLi-I computed with intervals
  {
    I_dbl::traits_type::rounding rnd;
    typedef boost::numeric::interval_lib::unprotect<I_dbl>::type I;
    vector<I> m1(dim);
    vector<I> m2(dim);
    for(int i = 0; i < dim; i++) {
      for(int j = 0; j < dim; j++) m1[j] = 0;
      const double *l1 = a[p[i]];
      for(int j = 0; j < dim; j++) {
        double v = l1[j];    // PA[i,j]
        double *l2 = lui[j]; // Ui[j,?]
        for(int k = j; k < dim; k++) {
          using boost::numeric::interval_lib::mul;
          m1[k] += mul<I>(v, l2[k]); // PAUi[i,k]
        }
      }
      for(int j = 0; j < dim; j++) m2[j] = m1[j]; // PAUi[i,j] * Li[j,j]
      for(int j = 1; j < dim; j++) {
        const I &v = m1[j];  // PAUi[i,j]
        double *l2 = lui[j]; // Li[j,?]
        for(int k = 0; k < j; k++)
          m2[k] += v * l2[k]; // PAUiLi[i,k]
      }
      m2[i] -= 1; // PAUiLi-I
      double ss = 0;
      for(int i = 0; i < dim; i++) ss = rnd.add_up(ss, norm(m2[i]));
      if (ss >= 1) return 0;
    }
  }
  return sig;
}

int main() {
  int dim = 20;
  matrix<double> m(dim);
  for(int i = 0; i < dim; i++) for(int j = 0; j < dim; j++)
    m[i][j] = /*1 / (i-j-0.001)*/ cos(1+i*sin(1 + j)) /*1./(1+i+j)*/;

  // compute the sign of the determinant of a "strange" matrix with the two
  // algorithms, the first should fail and the second succeed
  std::cout << det_sign_algo1(m) << " " << det_sign_algo2(m) << std::endl;
}


/*
 * Copyright 2020 Netherlands eScience Center
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <Eigen/Core>
#include <Eigen/Dense>
#include <utility>

#include <Spectra/GenEigsComplexShiftSolver.h>
#include <Spectra/GenEigsRealShiftSolver.h>
#include <Spectra/GenEigsSolver.h>
#include <Spectra/MatOp/DenseGenComplexShiftSolve.h>
#include <Spectra/MatOp/DenseGenMatProd.h>
#include <Spectra/MatOp/DenseGenRealShiftSolve.h>
#include <Spectra/MatOp/DenseSymMatProd.h>
#include <Spectra/MatOp/DenseSymShiftSolve.h>
#include <Spectra/MatOp/SymShiftInvert.h>
#include <Spectra/SymEigsShiftSolver.h>
#include <Spectra/SymEigsSolver.h>
#include <Spectra/SymGEigsShiftSolver.h>

#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>

namespace py = pybind11;

using ComplexMatrix = Eigen::MatrixXcd;
using ComplexVector = Eigen::VectorXcd;
using Matrix = Eigen::MatrixXd;
using Vector = Eigen::VectorXd;
using Eigen::Index;

Spectra::SortRule string_to_sortrule(const std::string& name)
{
    std::unordered_map<std::string, Spectra::SortRule> rules = {
        {"LargestMagn", Spectra::SortRule::LargestMagn},
        {"LargestReal", Spectra::SortRule::LargestReal},
        {"LargestImag", Spectra::SortRule::LargestImag},
        {"LargestAlge", Spectra::SortRule::LargestAlge},
        {"SmallestMagn", Spectra::SortRule::SmallestMagn},
        {"SmallestReal", Spectra::SortRule::SmallestReal},
        {"SmallestImag", Spectra::SortRule::SmallestImag},
        {"SmallestAlge", Spectra::SortRule::SmallestAlge},
        {"BothEnds", Spectra::SortRule::BothEnds}};
    auto it = rules.find(name);
    if (it != rules.cend())
    {
        return it->second;
    }
    else
    {
        std::ostringstream oss;
        oss << "There is no selection rule named: " << name << "\n"
            << "Available selection rules:\n";
        for (const auto& pair : rules)
        {
            oss << pair.first << "\n";
        }
        throw std::runtime_error(oss.str());
    }
}

/// \brief Run the computation and throw and error if it fails
template <typename ResultVector, typename ResultMatrix, typename Solver>
std::pair<ResultVector, ResultMatrix> compute_and_check(
    Solver& eigs, const std::string& selection)
{
    // Initialize and compute
    eigs.init();
    // Compute using the user provided selection rule
    eigs.compute(string_to_sortrule(selection));

    // Retrieve results
    if (eigs.info() == Spectra::CompInfo::Successful)
    {
        return std::make_pair(eigs.eigenvalues(), eigs.eigenvectors());
    }
    else
    {
        throw std::runtime_error(
            "The Spectra SymEigsSolver calculation has failed!");
    }
}

/// \brief Call the Spectra::GenEigsSolver eigensolver
std::pair<ComplexVector, ComplexMatrix> geneigssolver(
    const Matrix& mat, Index nvalues, Index nvectors,
    const std::string& selection)
{
    using DenseOp = Spectra::DenseGenMatProd<double>;

    // Construct matrix operation object using the wrapper class DenseSymMatProd
    Spectra::DenseGenMatProd<double> op(mat);
    Spectra::GenEigsSolver<double, DenseOp> eigs(op, nvalues, nvectors);
    return compute_and_check<ComplexVector, ComplexMatrix>(eigs, selection);
}

/// \brief Call the Spectra::GenEigsRealShiftSolver eigensolver
std::pair<ComplexVector, ComplexMatrix> geneigsrealshiftsolver(
    const Matrix& mat, Index nvalues, Index nvectors, double sigma,
    const std::string& selection)
{
    using DenseOp = Spectra::DenseGenRealShiftSolve<double>;
    DenseOp op(mat);
    Spectra::GenEigsRealShiftSolver<double, DenseOp> eigs(op, nvalues, nvectors,
                                                          sigma);
    return compute_and_check<ComplexVector, ComplexMatrix>(eigs, selection);
}

/// \brief Call the Spectra::GenEigsComplexShiftSolver eigensolver
std::pair<ComplexVector, ComplexMatrix> geneigscomplexshiftsolver(
    const Matrix& mat, Index nvalues, Index nvectors, double sigmar,
    double sigmai, const std::string& selection)
{
    using DenseOp = Spectra::DenseGenComplexShiftSolve<double>;
    DenseOp op(mat);
    Spectra::GenEigsComplexShiftSolver<double, DenseOp> eigs(
        op, nvalues, nvectors, sigmar, sigmai);
    return compute_and_check<ComplexVector, ComplexMatrix>(eigs, selection);
}

/// \brief Call the Spectra::DenseSymMatProd eigensolver
std::pair<Vector, Matrix> symeigssolver(const Matrix& mat, Index nvalues,
                                        Index nvectors,
                                        const std::string& selection)
{
    using DenseSym = Spectra::DenseSymMatProd<double>;
    // Construct matrix operation object using the wrapper class DenseSymMatProd
    DenseSym op(mat);
    Spectra::SymEigsSolver<double, DenseSym> eigs(op, nvalues, nvectors);

    return compute_and_check<Vector, Matrix>(eigs, selection);
}

/// \brief Call the Spectra::SymEigsShiftSolver eigensolver
std::pair<Vector, Matrix> symeigsshiftsolver(const Matrix& mat, Index nvalues,
                                             Index nvectors, double sigma,
                                             const std::string& selection)
{
    using DenseSymShift = Spectra::DenseSymShiftSolve<double>;
    // Construct matrix operation object using the wrapper class DenseSymMatProd
    DenseSymShift op(mat);
    Spectra::SymEigsShiftSolver<double, DenseSymShift> eigs(op, nvalues, nvectors,
                                                            sigma);

    return compute_and_check<Vector, Matrix>(eigs, selection);
}

/// \brief Call the Spectra::SymGEigsShiftSolver eigensolver
std::pair<Vector, Matrix> symgeneigsshiftsolver(const Matrix& mat_A,
                                                const Matrix& mat_B, Index nvalues,
                                                Index nvectors, double sigma,
                                                const std::string& selection)
{
    using SymShiftInvert =
        Spectra::SymShiftInvert<double, Eigen::Dense, Eigen::Dense>;
    using DenseSym = Spectra::DenseSymMatProd<double>;

    // Construct matrix operation object using the wrapper class DenseSymMatProd
    SymShiftInvert op_A(mat_A, mat_B);
    DenseSym op_B(mat_B);
    Spectra::SymGEigsShiftSolver<double, SymShiftInvert, DenseSym, Spectra::GEigsMode::ShiftInvert>
        eigs(op_A, op_B, nvalues, nvectors, sigma);

    return compute_and_check<Vector, Matrix>(eigs, selection);
}

PYBIND11_MODULE(spectra_dense_interface, m)
{
    m.doc() =
        "Interface to the C++ spectra library, see: "
        "https://github.com/yixuan/spectra";

    m.def("general_eigensolver", &geneigssolver,
          py::return_value_policy::reference_internal);

    m.def("general_real_shift_eigensolver", &geneigsrealshiftsolver,
          py::return_value_policy::reference_internal);

    m.def("general_complex_shift_eigensolver", &geneigscomplexshiftsolver,
          py::return_value_policy::reference_internal);

    m.def("symmetric_eigensolver", &symeigssolver,
          py::return_value_policy::reference_internal);

    m.def("symmetric_shift_eigensolver", &symeigsshiftsolver,
          py::return_value_policy::reference_internal);

    m.def("symmetric_generalized_shift_eigensolver", &symgeneigsshiftsolver,
          py::return_value_policy::reference_internal);
}


/** View a conflict graph as an implication graph and run Tarjan's strongly connected components algorithm */

#include "cliquetable_scc.hpp"
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/strong_components.hpp>

using namespace boost;

void make_domain_consistent(CliqueTableInstance* inst, IntSet* state_intset)
{
	typedef adjacency_list<vecS, vecS, bidirectionalS,
	        property<vertex_color_t, default_color_type, property<vertex_degree_t,int>>> ImplGraph;
	typedef graph_traits<ImplGraph>::vertex_descriptor Vertex;

	ImplGraph graph(2 * inst->nvars);

	// cout << "Clique table:" << endl;
	// inst->print();

	// For every edge (i,j), we have two arcs (i, not j) and (j, not i)
	for (int i = 0; i < inst->nvars; ++i) {
		for (int j = inst->adj[i].get_first(); j != inst->adj[i].get_end(); j = inst->adj[i].get_next(j)) {
			if (i < j && i != inst->get_complement(j)) {
				add_edge(i, inst->get_complement(j), graph);
				add_edge(j, inst->get_complement(i), graph);
			}
		}
	}

	typedef graph_traits<ImplGraph>::edge_iterator edge_iterator;
	pair<edge_iterator, edge_iterator> ei = edges(graph);

	// Debugging info
	// cout << endl << "Implication graph:" << endl;
	// for (edge_iterator it = ei.first; it != ei.second; ++it) {
	// 	cout << "(" << source(*it, graph) << ", " << target(*it, graph) << ")" << " ";
	// }
	// cout << endl;

	// Find strongly connected components
	int nv = num_vertices(graph);
	vector<int> component(nv), discover_time(nv);
	vector<default_color_type> color(nv);
	vector<Vertex> root(nv);
	int num = strong_components(graph, make_iterator_property_map(component.begin(), get(vertex_index, graph)),
	                            root_map(make_iterator_property_map(root.begin(), get(vertex_index, graph))).
	                            color_map(make_iterator_property_map(color.begin(), get(vertex_index, graph))).
	                            discover_time_map(make_iterator_property_map(discover_time.begin(), get(vertex_index, graph))));

	// cout << num << " strong components" << endl;

	// for (int j = num - 1; j >= 0; --j) {
	// 	cout << "[ ";
	// 	for (int i = 0; i < component.size(); ++i) {
	// 		if (j == component[i]) {
	// 			cout << i << " ";
	// 		}
	// 	}
	// 	cout << "] ";
	// }

	// Assert reverse topological ordering
	for (edge_iterator it = ei.first; it != ei.second; ++it) {
		assert(component[source(*it, graph)] >= component[target(*it, graph)]);
	}

	vector<int> topological_order;
	for (int j = num - 1; j >= 0; --j) {
		for (int i = 0; i < (int) component.size(); ++i) {
			if (j == component[i]) {
				topological_order.push_back(i);
			}
		}
	}

	// cout << "Topological order:  ";
	// for (int i : topological_order) {
	// 	cout << i << " ";
	// }
	// cout << endl;

	vector<vector<int>> vertices_in_component(component.size());
	for (int i = 0; i < nv; ++i) {
		vertices_in_component[component[i]].push_back(i);
	}

	// // Debugging info
	// for (int i = 0; i < component.size(); ++i) {
	// 	cout << "Component " << i << ":  ";
	// 	for (int j : vertices_in_component[i]) {
	// 		cout << j << " ";
	// 	}
	// 	cout << endl;
	// }

	// Traverse components in topological order
	vector<set<int>> ancestors(component.size());
	for (int c = nv - 1; c >= 0; --c) {
		for (int v : vertices_in_component[c]) {
			typename graph_traits<ImplGraph>::adjacency_iterator u, u_end;
			for (tie(u, u_end) = adjacent_vertices(v, graph); u != u_end; ++u) {
				// Add current component and its own ancestors to ancestor list of adjacent component
				ancestors[component[*u]].insert(c);
				for (int ancestor : ancestors[c]) {
					ancestors[component[*u]].insert(ancestor);
				}
			}
		}
	}

	// // Debugging info
	// for (int i = 0; i < ancestors.size(); ++i) {
	// 	cout << "Component " << i << " - ancestors:  ";
	// 	for (int j : ancestors[i]) {
	// 		cout << j << " ";
	// 	}
	// 	cout << endl;
	// }

	// If the same component contains both complement nodes, remove them
	for (int v = 0; v < inst->nvars; ++v) {
		if (component[v] == component[inst->get_complement(v)]) {
			state_intset->remove(v);
			state_intset->remove(inst->get_complement(v));
		}
	}

	// For each vertex, check if it precedes its component; if so, remove the vertex from the domain
	for (int v = 0; v < nv; ++v) {
		int complement = component[inst->get_complement(v)];
		if (ancestors[complement].find(component[v]) != ancestors[complement].end()) {
			// There exists a path from v to complement of v
			state_intset->remove(v);
		}
	}
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "all_pairs_distances.h"
#include <Eigen/Dense>

template <typename Mat>
IGL_INLINE void igl::all_pairs_distances(
  const Mat & V,
  const Mat & U,
  const bool squared,
  Mat & D)
{
  // dimension should be the same
  assert(V.cols() == U.cols());
  // resize output
  D.resize(V.rows(),U.rows());
  for(int i = 0;i<V.rows();i++)
  {
    for(int j=0;j<U.rows();j++)
    {
      D(i,j) = (V.row(i)-U.row(j)).squaredNorm();
      if(!squared)
      {
        D(i,j) = sqrt(D(i,j));
      }
    }
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
// generated by autoexplicit.sh
template void igl::all_pairs_distances<Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, bool, Eigen::Matrix<double, -1, -1, 0, -1, -1>&);
#endif


////////////////////////////////////////////////////////////////////////////////
#include "vertex_degree.h"
#include <igl/edges.h>
#include <Eigen/Dense>
#include <iostream>
////////////////////////////////////////////////////////////////////////////////

namespace cellogram {

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

void vertex_degree(const Eigen::MatrixXi &F, Eigen::VectorXi &degree)
{
	degree = Eigen::VectorXi::Zero(F.maxCoeff()+1);
	/*for (int i = 0; i < F.rows(); i++)
	{
		for (int j = 0; j < 3; j++)
		{
			degree(F(i, j))++;
			degree(F(i, (j + 1) % 3))++;
		}
	}*/

	Eigen::MatrixXi E;
	igl::edges(F, E);

	//std::cout << E.transpose() << std::endl;

	for (int i = 0; i < E.rows(); i++)
	{
		for (int j = 0; j < 2; j++)
		{
			degree(E(i, j))++;
			degree(E(i, (j + 1) % 2))++;
		}

	}
	degree /= 2;
}

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

} // namespace cellogram


#include "cutting_plane.hpp"
#include "ell.hpp"
#include "profit_oracle.hpp"
#include "ldlt_ext.hpp"
//#include <boost/numeric/ublas/symmetric.hpp>
#include <xtensor/xarray.hpp>
#include <xtensor-blas/xlinalg.hpp>
//#include <fmt/format.h>
#include <iostream>

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

// Versions: (latest first)
// http://melpon.org/wandbox/permlink/CcvL0BHfVJhHZH4M
// http://melpon.org/wandbox/permlink/ExQoromITFQ7WOxO
// http://melpon.org/wandbox/permlink/YiLtKIriWtkigZs8

int test1() {
  //namespace bnu = boost::numeric::ublas;
  //using Vec = bnu::vector<double>;
  //using Mat = xt::xarray<double, xt::layout_type::row_major>;
  using Vec = xt::xarray<double, xt::layout_type::row_major>;

  double p = 20, A = 40, alpha = 0.1, beta = 0.4;
  double v1 = 10, v2 = 35, k = 30.5;
  double fb;
  int niter, status;
  bool feasible;

  {
    ell E(100., Vec{0., 0.});
    profit_oracle P(p, A, alpha, beta, v1, v2, k);
    std::tie(std::ignore, fb, niter, feasible, status) =
        cutting_plane_dc(P, E, 0.);
    // fmt::print("{:f} {} {} {} \n", fb, niter, feasible, status);
    std::cout << fb << ", " << niter << ", " << feasible << ", " << status << "\n";
  }

  double ui = 1., e1 = 0.003, e2 = 0.007, e3 = 1.;

  {
    ell E1(100., Vec{0., 0.});
    profit_rb_oracle P1(p, A, alpha, beta, v1, v2, k, ui, e1, e2, e3);
    std::tie(std::ignore, fb, niter, feasible, status) =
        cutting_plane_dc(P1, E1, 0.);
    // fmt::print("{:f} {} {} {} \n", fb, niter, feasible, status);
    std::cout << fb << ", " << niter << ", " << feasible << ", " << status
              << "\n";
  }

  {
    ell E2(100., Vec{2., 0.});
    profit_q_oracle P2(p, A, alpha, beta, v1, v2, k);
    std::tie(std::ignore, fb, niter, feasible, status) =
        cutting_plane_q(P2, E2, 0.);
    // fmt::print("{:f} {} {} {} \n", fb, niter, feasible, status);
    std::cout << fb << ", " << niter << ", " << feasible << ", " << status
             << "\n";
  }

  return 0;
}

int main() {
  using Arr = xt::xarray<double, xt::layout_type::row_major>;
  using xt::placeholders::_;
  using xt::linalg::dot;

  auto m1 = Arr({{25., 15., -5.},
                {15., 18.,  0.},
                {-5.,  0., 11.}});
  std::cout << m1.shape()[0] << "\n";
  auto Q1 = ldlt_ext(m1.shape()[0]);
  Q1.factorize(m1);
  if (!Q1.is_sd()) {
    auto v = Q1.witness();
    auto p = v.size();
    auto sub = xt::range(0, p);
    Arr App = xt::view(m1, sub, sub);
    Arr Appv = dot(App, v);
    auto fj = -dot(v, Appv)();
    std::cout << fj << std::endl;
  }

  auto m2 = Arr({{18., 22.,  54.,  42.},
                {22., -70.,  86.,  62.},
                {54., 86., -174., 134.},
                {42., 62., 134., -106.}});
  std::cout << m2.shape()[0] << "\n";
  auto Q2 = ldlt_ext(m2.shape()[0]);
  Q2.factorize(m2);
  if (!Q2.is_sd()) {
    auto v = Q2.witness();
    auto p = v.size();
    auto sub = xt::range(0, p);
    Arr App = xt::view(m2, sub, sub);
    Arr Appv = dot(App, v);
    auto fj = -dot(v, Appv)();
    std::cout << fj << std::endl;
  }

  return 0;
}


/*
   Copyright (C) 2015-2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>

   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.
*/

// Critical temperature of hexagonal lattice Ising model

#pragma once

#include <cmath>
#include <stdexcept>
#include <boost/math/differentiation/autodiff.hpp>
#include <standards/newton.hpp>

namespace {

template<typename T>
struct func {
  func(T Ja, T Jb, T Jc) : Ja_(Ja), Jb_(Jb), Jc_(Jc) {}
  auto operator()(T beta) const -> decltype(boost::math::differentiation::make_fvar<T, 2>(beta)) {
    using std::tanh;
    auto beta_fvar = boost::math::differentiation::make_fvar<T, 2>(beta);
    auto za = tanh(beta_fvar * Ja_);
    auto zb = tanh(beta_fvar * Jb_);
    auto zc = tanh(beta_fvar * Jc_);
    return za * zb + zb * zc + zc * za - 1;
  }
  T Ja_, Jb_, Jc_;
};

}

namespace ising {
namespace tc {

template<typename T>
inline T hexagonal(T Ja, T Jb, T Jc) {
  Ja = abs(Ja);
  Jb = abs(Jb);
  Jc = abs(Jc);
  if (Ja * Jb * Jc <= 0) throw(std::invalid_argument("Ja * Jb * Jc should be non-zero"));
  auto result = standards::newton_1d(func<T>(Ja, Jb, Jc), 1 / (Ja + Jb + Jc));
  if (!result.second) throw(std::runtime_error("convergence error"));
  return 1 / result.first;
}

} // end namespace tc
} // end namespace ising


#include <boost/numeric/odeint.hpp>
#include "stochastic_euler.hpp"
#include "cahnhilliard.h"
#include "cahnhilliard_thermal.h"
#include "cahnhilliard_thermal_nodiffusion.h"
#include "run_ch_solver.h"

void run_ch_solver_non_thermal( CHparamsVector& chparams , SimInfo& info )
{

  // Instantiate rhs
  CahnHilliard2DRHS rhs = CahnHilliard2DRHS( chparams , info );

  std::vector<double> x;
  if (info.t0 == 0) {
    rhs.setInitialConditions(x);
    int iter = 0;
  }
  else {
    x        = info.x;
    int iter = info.iter;
  }

  // define adaptive stepper
  typedef boost::numeric::odeint::runge_kutta_cash_karp54<std::vector<double>> error_stepper_type;

  // define runge kutta
  typedef boost::numeric::odeint::controlled_runge_kutta<error_stepper_type> controlled_stepper_type;

  controlled_stepper_type controlled_stepper;

  const double stability_limit = chparams.compute_stability_limit(info.dx , info.dy); // just an estimate
  const double res0            = rhs.l2residual(x);

  std::cout << "residual at initial condition: " << res0 << std::endl;
  if (info.iter == 0)
    rhs.write_state(x,0,info.nx,info.ny,info.outdir);

  if (chparams.sigma_noise < 1e-2) {
    std::cout << "Solving deterministic (noise-free) CH" << std::endl;
    integrate_adaptive(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
    //boost::numeric::odeint::integrate_const(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
  }
  else {
    std::cout << "Solving stochastic CH" << std::endl;
    boost::mt19937 rng;
    boost::numeric::odeint::integrate_const( stochastic_euler() ,
		     std::make_pair( rhs , ornstein_stoch( rng , chparams.sigma_noise ) ),
		     x , info.t0 , info.tf , stability_limit/40. );
  }
  info.iter += 1;
  std::cout << "iter: " << info.iter << " , t = " << info.tf << ", relative residual: " << rhs.l2residual(x) / res0 << std::endl;
  rhs.write_state(x,info.iter,info.nx,info.ny,info.outdir);
  info.x = x;

};

void run_ch_solver_thermal_no_diffusion( CHparamsVector& chparams , SimInfo& info )
{

  // Instantiate rhs
  CahnHilliard2DRHS_thermal_nodiffusion rhs = CahnHilliard2DRHS_thermal_nodiffusion( chparams , info );

  std::vector<double> x;
  if (info.t0 == 0) {
    rhs.setInitialConditions(x);
    int iter = 0;
  }
  else {
    x        = info.x;
    int iter = info.iter;
  }

  // define adaptive stepper
  typedef boost::numeric::odeint::runge_kutta_cash_karp54<std::vector<double>> error_stepper_type;

  // define runge kutta
  typedef boost::numeric::odeint::controlled_runge_kutta<error_stepper_type> controlled_stepper_type;

  controlled_stepper_type controlled_stepper;

  const double stability_limit = chparams.compute_stability_limit(info.dx , info.dy); // just an estimate
  const double res0            = rhs.l2residual(x);

  std::cout << "residual at initial condition: " << res0 << std::endl;
  if (info.iter == 0)
    rhs.write_state(x,0,info.nx,info.ny,info.outdir);

  if (chparams.sigma_noise < 1e-2) {
    std::cout << "Solving deterministic (noise-free) CH" << std::endl;
    integrate_adaptive(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
    //boost::numeric::odeint::integrate_const(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
  }
  else {
    std::cout << "Solving stochastic CH" << std::endl;
    boost::mt19937 rng;
    boost::numeric::odeint::integrate_const( stochastic_euler() ,
		     std::make_pair( rhs , ornstein_stoch( rng , chparams.sigma_noise ) ),
		     x , info.t0 , info.tf , stability_limit/40. );
  }
  info.iter += 1;
  std::cout << "iter: " << info.iter << " , t = " << info.tf << ", relative residual: " << rhs.l2residual(x) / res0 << std::endl;
  rhs.write_state(x,info.iter,info.nx,info.ny,info.outdir);
  info.x = x;

};

void run_ch_solver_thermal_with_diffusion( CHparamsVector& chparams , SimInfo& info )
{

  // Instantiate rhs
  CahnHilliard2DRHS_thermal rhs = CahnHilliard2DRHS_thermal( chparams , info );

  std::vector<double> x;
  if (info.t0 == 0) {
    rhs.setInitialConditions(x);
    int iter = 0;
  }
  else {
    x        = info.x;
    int iter = info.iter;
  }

  // define adaptive stepper
  typedef boost::numeric::odeint::runge_kutta_cash_karp54<std::vector<double>> error_stepper_type;

  // define runge kutta
  typedef boost::numeric::odeint::controlled_runge_kutta<error_stepper_type> controlled_stepper_type;

  controlled_stepper_type controlled_stepper;

  const double stability_limit = chparams.compute_stability_limit(info.dx , info.dy); // just an estimate
  const double res0            = rhs.l2residual(x);

  std::cout << "residual at initial condition: " << res0 << std::endl;
  if (info.iter == 0)
    rhs.write_state(x,0,info.nx,info.ny,info.outdir);

  if (chparams.sigma_noise < 1e-2) {
    std::cout << "Solving deterministic (noise-free) CH" << std::endl;
    integrate_adaptive(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
    //boost::numeric::odeint::integrate_const(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
  }
  else {
    std::cout << "Solving stochastic CH" << std::endl;
    boost::mt19937 rng;
    boost::numeric::odeint::integrate_const( stochastic_euler() ,
		     std::make_pair( rhs , ornstein_stoch( rng , chparams.sigma_noise ) ),
		     x , info.t0 , info.tf , stability_limit/40. );
  }
  info.iter += 1;
  std::cout << "iter: " << info.iter << " , t = " << info.tf << ", relative residual: " << rhs.l2residual(x) / res0 << std::endl;
  rhs.write_state(x,info.iter,info.nx,info.ny,info.outdir);
  info.x = x;

};

void run_ch_solver_non_thermal( CHparamsScalar& chparams , SimInfo& info )
{

  // Instantiate rhs
  CahnHilliard2DRHS rhs = CahnHilliard2DRHS( chparams , info );

  std::vector<double> x;
  if (info.t0 == 0) {
    rhs.setInitialConditions(x);
    int iter = 0;
  }
  else {
    x        = info.x;
    int iter = info.iter;
  }

  // define adaptive stepper
  typedef boost::numeric::odeint::runge_kutta_cash_karp54<std::vector<double>> error_stepper_type;

  // define runge kutta
  typedef boost::numeric::odeint::controlled_runge_kutta<error_stepper_type> controlled_stepper_type;

  controlled_stepper_type controlled_stepper;

  const double stability_limit = chparams.compute_stability_limit(info.dx , info.dy); // just an estimate
  const double res0            = rhs.l2residual(x);

  std::cout << "residual at initial condition: " << res0 << std::endl;
  if (info.iter == 0)
    rhs.write_state(x,0,info.nx,info.ny,info.outdir);

  if (chparams.sigma_noise < 1e-2) {
    std::cout << "Solving deterministic (noise-free) CH" << std::endl;
    integrate_adaptive(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
    //boost::numeric::odeint::integrate_const(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
  }
  else {
    std::cout << "Solving stochastic CH" << std::endl;
    boost::mt19937 rng;
    boost::numeric::odeint::integrate_const( stochastic_euler() ,
		     std::make_pair( rhs , ornstein_stoch( rng , chparams.sigma_noise ) ),
		     x , info.t0 , info.tf , stability_limit/40. );
  }
  info.iter += 1;
  std::cout << "iter: " << info.iter << " , t = " << info.tf << ", relative residual: " << rhs.l2residual(x) / res0 << std::endl;
  rhs.write_state(x,info.iter,info.nx,info.ny,info.outdir);
  info.x = x;

};

void run_ch_solver_thermal_no_diffusion( CHparamsScalar& chparams , SimInfo& info )
{

  // Instantiate rhs
  CahnHilliard2DRHS_thermal_nodiffusion rhs = CahnHilliard2DRHS_thermal_nodiffusion( chparams , info );

  std::vector<double> x;
  if (info.t0 == 0) {
    rhs.setInitialConditions(x);
    int iter = 0;
  }
  else {
    x        = info.x;
    int iter = info.iter;
  }

  // define adaptive stepper
  typedef boost::numeric::odeint::runge_kutta_cash_karp54<std::vector<double>> error_stepper_type;

  // define runge kutta
  typedef boost::numeric::odeint::controlled_runge_kutta<error_stepper_type> controlled_stepper_type;

  controlled_stepper_type controlled_stepper;

  const double stability_limit = chparams.compute_stability_limit(info.dx , info.dy); // just an estimate
  const double res0            = rhs.l2residual(x);

  std::cout << "residual at initial condition: " << res0 << std::endl;
  if (info.iter == 0)
    rhs.write_state(x,0,info.nx,info.ny,info.outdir);

  if (chparams.sigma_noise < 1e-2) {
    std::cout << "Solving deterministic (noise-free) CH" << std::endl;
    integrate_adaptive(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
    //boost::numeric::odeint::integrate_const(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
  }
  else {
    std::cout << "Solving stochastic CH" << std::endl;
    boost::mt19937 rng;
    boost::numeric::odeint::integrate_const( stochastic_euler() ,
		     std::make_pair( rhs , ornstein_stoch( rng , chparams.sigma_noise ) ),
		     x , info.t0 , info.tf , stability_limit/40. );
  }
  info.iter += 1;
  std::cout << "iter: " << info.iter << " , t = " << info.tf << ", relative residual: " << rhs.l2residual(x) / res0 << std::endl;
  rhs.write_state(x,info.iter,info.nx,info.ny,info.outdir);
  info.x = x;

};

void run_ch_solver_thermal_with_diffusion( CHparamsScalar& chparams , SimInfo& info )
{

  // Instantiate rhs
  CahnHilliard2DRHS_thermal rhs = CahnHilliard2DRHS_thermal( chparams , info );

  std::vector<double> x;
  if (info.t0 == 0) {
    rhs.setInitialConditions(x);
    int iter = 0;
  }
  else {
    x        = info.x;
    int iter = info.iter;
  }

  // define adaptive stepper
  typedef boost::numeric::odeint::runge_kutta_cash_karp54<std::vector<double>> error_stepper_type;

  // define runge kutta
  typedef boost::numeric::odeint::controlled_runge_kutta<error_stepper_type> controlled_stepper_type;

  controlled_stepper_type controlled_stepper;

  const double stability_limit = chparams.compute_stability_limit(info.dx , info.dy); // just an estimate
  const double res0            = rhs.l2residual(x);

  std::cout << "residual at initial condition: " << res0 << std::endl;
  if (info.iter == 0)
    rhs.write_state(x,0,info.nx,info.ny,info.outdir);

  if (chparams.sigma_noise < 1e-2) {
    std::cout << "Solving deterministic (noise-free) CH" << std::endl;
    integrate_adaptive(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
    //boost::numeric::odeint::integrate_const(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
  }
  else {
    std::cout << "Solving stochastic CH" << std::endl;
    boost::mt19937 rng;
    boost::numeric::odeint::integrate_const( stochastic_euler() ,
		     std::make_pair( rhs , ornstein_stoch( rng , chparams.sigma_noise ) ),
		     x , info.t0 , info.tf , stability_limit/40. );
  }
  info.iter += 1;
  std::cout << "iter: " << info.iter << " , t = " << info.tf << ", relative residual: " << rhs.l2residual(x) / res0 << std::endl;
  rhs.write_state(x,info.iter,info.nx,info.ny,info.outdir);
  info.x = x;

};

void run_ch_solver( CHparamsVector& chparams , SimInfo& info )
{

  if      (info.rhs_type.compare("ch_non_thermal") == 0) {
    run_ch_solver_non_thermal( chparams , info );
  }
  else if (info.rhs_type.compare("ch_thermal_no_diffusion") == 0) {
    run_ch_solver_thermal_no_diffusion( chparams , info );
  }
  else if (info.rhs_type.compare("ch_thermal_with_diffusion") == 0) {
    run_ch_solver_thermal_with_diffusion( chparams , info );
  }

};

void run_ch_solver( CHparamsScalar& chparams , SimInfo& info )
{

  if      (info.rhs_type.compare("ch_non_thermal") == 0) {
    run_ch_solver_non_thermal( chparams , info );
  }
  else if (info.rhs_type.compare("ch_thermal_no_diffusion") == 0) {
    run_ch_solver_thermal_no_diffusion( chparams , info );
  }
  else if (info.rhs_type.compare("ch_thermal_with_diffusion") == 0) {
    run_ch_solver_thermal_with_diffusion( chparams , info );
  }

};


/**
 * \file boost/numeric/ublasx/operation/rcond.hpp
 *
 * \brief Matrix reciprocal condition number estimate.
 *
 * The condition number of a regular (square) matrix is the product
 * of the \e norm of the matrix and the norm of its inverse (or
 * pseudo-inverse), and hence depends on the kind of matrix-norm.
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * 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)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_RCOND_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_RCOND_HPP

#include <algorithm>
#include <boost/numeric/bindings/lapack/auxiliary/lange.hpp>
#include <boost/numeric/bindings/lapack/computational/gbcon.hpp>
#include <boost/numeric/bindings/lapack/computational/gbtrf.hpp>
#include <boost/numeric/bindings/lapack/computational/gecon.hpp>
#include <boost/numeric/bindings/lapack/computational/getrf.hpp>
#include <boost/numeric/bindings/lapack/computational/hecon.hpp>
#include <boost/numeric/bindings/lapack/computational/hetrf.hpp>
#include <boost/numeric/bindings/lapack/computational/sycon.hpp>
#include <boost/numeric/bindings/lapack/computational/sytrf.hpp>
#include <boost/numeric/bindings/lapack/computational/trcon.hpp>
#include <boost/numeric/bindings/ublas.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/exception.hpp>
#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/hermitian.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/qr.hpp>
#include <stdexcept>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail {

enum matrix_norm_category
{
	matrix_norm_1,
	matrix_norm_2,
	matrix_norm_frobenius,
	matrix_norm_inf
};

//template <
//	typename ValueT,
//	typename LayoutT,
//	typename StorageT
//>
//ValueT* band_mat_to_lapack_vec(banded_matrix<ValueT,LayoutT,StorageT> const& A, ::fortran_int_t& n)
//{
//	typedef banded_matrix<ValueT,LayoutT,StorageT> matrix_type;
//	typedef typename matrix_traits<matrix_type>::size_type size_type;
//
//	const size_type kl = A.lower();
//	const size_type ku = A.upper();
//	const size_type nr = num_rows(A);
//	const size_type nc = num_columns(A);
//
//	n = (2*kl+ku+1)*nc;
//
//	ValueT* v = new ValueT[n];
//
//	for (size_type r = 0; r < nr; ++r)
//	{
//		for (size_type c = 0; c < nc; ++c)
//		{
//			size_type rr = kl+ku+r-c;
//			size_type k = (2*kl+ku+1)*c+rr;
//
//			if ((r == c) || (c <= (ku+r) && r <= (c+kl)))
//			{
//				// In band
//				v[k] = A(r,c);
//			}
//			else
//			{
//				// Out of band
//				v[k] = ValueT();
//			}
//		}
//	}
//
//	return v;
//}

//template <typename T>
//void print_vector(std::string const& desc, T const* a, std::size_t n)
//{
//	std::cout << std::endl << desc << std::endl << "  [";
//	for(std::size_t i = 0; i < n; ++i)
//	{
//		std::cout << " " << std::fixed << a[i];
//	}
//	std::cout << "]" << std::endl;
//}

template <typename MatrixT>
typename type_traits<
	typename matrix_traits<MatrixT>::value_type
>::real_type rcond_impl(MatrixT const& A, matrix_norm_category norm_category, column_major_tag)
{
    typedef typename matrix_traits<MatrixT>::value_type value_type;
	typedef typename type_traits<value_type>::real_type result_type;
	typedef typename matrix_traits<MatrixT>::size_type size_type;
	typedef matrix<value_type, column_major> work_matrix_type;

	size_type nr = num_rows(A);
	size_type nc = num_columns(A);
	size_type k = ::std::min(nr,nc);

	// Check if A is a square matrix
	if (nr != nc)
	{
		// Non-square matrix -> Use QR decomposition
		if (nr < nc)
		{
			return rcond_impl(qr_decompose(trans(A)).R(false), norm_category, column_major_tag());
		}
		else
		{
			return rcond_impl(qr_decompose(A).R(false), norm_category, column_major_tag());
		}
	}

	char what_norm;
	result_type norm;
	result_type res;

	//FIXME: actually, in bindings this function is broken
//	switch (norm_category)
//	{
//		case matrix_norm_1:
//			what_norm = 'O';
//			break;
//		case matrix_norm_inf:
//			what_norm = 'I';
//			break;
////		case matrix_norm_frobenius:
////			what_norm = 'F';
////			break;
//		default:
//			throw std::runtime_error("[rcond::detail::rcond_impl] Unsupported norm category.");
//	}

	// Compute the norm of A
	//FIXME: actually, in bindings this function is broken
//	::boost::numeric::bindings::lapack::lange(
//		what_norm,
//		A
//	);
	switch (norm_category)
	{
		case matrix_norm_1:
			what_norm = 'O';
			norm = norm_1(A);
			break;
		case matrix_norm_inf:
			what_norm = 'I';
			norm = norm_inf(A);
			break;
		default:
			throw std::runtime_error("[rcond::detail::rcond_impl] Unsupported norm category.");
	}

	// Compute the LUP factorization of A
	work_matrix_type tmp_LU(A);
	vector< ::fortran_int_t > dummy_ipiv(k);
	::boost::numeric::bindings::lapack::getrf(
		tmp_LU,
		dummy_ipiv
	);
	dummy_ipiv.resize(0, false); // free memory

	// Finally, compute the reciprocal condition number
	::boost::numeric::bindings::lapack::gecon(
		what_norm,
		tmp_LU,
		norm,
		res
	);

	return res;
}

template <typename MatrixT>
typename type_traits<
	typename matrix_traits<MatrixT>::value_type
>::real_type rcond_impl(MatrixT const& A, matrix_norm_category norm_category, row_major_tag)
{
	typedef matrix<
				typename matrix_traits<MatrixT>::value_type,
				column_major
			> work_matrix_type;

	work_matrix_type tmp_A(A);

	return rcond_impl(tmp_A, norm_category, column_major_tag());
}

template <
	typename ValueT,
	typename TriangularT,
	typename StorageT
>
typename type_traits<ValueT>::real_type rcond_impl(triangular_matrix<ValueT,TriangularT,column_major,StorageT> const& A, matrix_norm_category norm_category, column_major_tag)
{
	typedef triangular_matrix<ValueT,TriangularT,column_major,StorageT> matrix_type;
    typedef typename matrix_traits<matrix_type>::value_type value_type;
	typedef typename matrix_traits<matrix_type>::size_type size_type;
	typedef typename type_traits<value_type>::real_type result_type;
	typedef matrix<value_type,column_major> auxiliary_matrix_type;
	typedef triangular_adaptor<auxiliary_matrix_type, TriangularT> work_matrix_type;

	size_type nr = num_rows(A);
	size_type nc = num_columns(A);

	// Check if A is a square matrix
	if (nr != nc)
	{
		// Non-square matrix -> Use QR decomposition
		if (nr < nc)
		{
			return rcond_impl(qr_decompose(trans(A)).R(), norm_category, column_major_tag());
		}
		else
		{
			return rcond_impl(qr_decompose(A).R(), norm_category, column_major_tag());
		}
	}

	char what_norm;
	result_type res;

	switch (norm_category)
	{
		case matrix_norm_1:
			what_norm = 'O';
			break;
		case matrix_norm_inf:
			what_norm = 'I';
			break;
		default:
			throw std::runtime_error("[rcond::detail::rcond_impl] Unsupported norm category.");
	}

	auxiliary_matrix_type aux_A(A);
	work_matrix_type tmp_A(aux_A);

	// Finally, compute the reciprocal condition number
	::boost::numeric::bindings::lapack::trcon(
		what_norm,
		tmp_A,
		res
	);

	return res;
}

template <
	typename ValueT,
	typename TriangularT,
	typename StorageT
>
typename type_traits<ValueT>::real_type rcond_impl(triangular_matrix<ValueT,TriangularT,row_major,StorageT> const& A, matrix_norm_category norm_category, row_major_tag)
{
	typedef triangular_matrix<ValueT,TriangularT,column_major,StorageT> work_matrix_type;

	work_matrix_type tmp_A(A);

	return rcond_impl(tmp_A, norm_category, column_major_tag());
}

/*
template <
	typename ValueT,
	typename StorageT
>
typename type_traits<ValueT>::real_type rcond_impl(banded_matrix<ValueT,column_major,StorageT> const& A, matrix_norm_category norm_category, column_major_tag)
{
	typedef banded_matrix<ValueT,column_major,StorageT> matrix_type;
    typedef typename matrix_traits<matrix_type>::value_type value_type;
	typedef typename matrix_traits<matrix_type>::size_type size_type;
	typedef typename type_traits<value_type>::real_type result_type;
	//typedef matrix<value_type,LayoutT> auxiliary_matrix_type;
	//typedef banded_adaptor<auxiliary_matrix_type> work_matrix_type;
	typedef banded_matrix<ValueT,row_major,StorageT> work_matrix_type;
	typedef vector< ::fortran_int_t > vector_type;

	size_type nr = num_rows(A);
	size_type nc = num_columns(A);

	// Check if A is a square matrix
	if (nr != nc)
	{
		// Non-square matrix -> Use QR decomposition
		if (nr < nc)
		{
			return rcond_impl(qr_decompose(trans(A)).R(), norm_category, column_major_tag());
		}
		else
		{
			return rcond_impl(qr_decompose(A).R(), norm_category, column_major_tag());
		}
	}

	char what_norm;
	result_type norm;
	result_type res;
	size_type k = ::std::min(nr,nc);
	size_type kl = A.lower();
	size_type ku = A.upper();
//	size_type ldab = 2*kl+ku+1;

	switch (norm_category)
	{
		case matrix_norm_1:
			what_norm = 'O';
			break;
		case matrix_norm_inf:
			what_norm = 'I';
			break;
//		case matrix_norm_frobenius:
//			what_norm = 'F';
//			break;
		default:
			throw std::runtime_error("[rcond::detail::rcond_impl] Unsupported norm category.");
	}

	// Compute the norm of A
	//FIXME: actually, in bindings this function is broken
//	::boost::numeric::bindings::lapack::lange(
//		what_norm,
//		A
//	);
	switch (norm_category)
	{
		case matrix_norm_1:
			norm = norm_1(A);
			break;
		case matrix_norm_inf:
			norm = norm_inf(A);
			break;
		default:
			throw std::runtime_error("[rcond::detail::rcond_impl] Unsupported norm category.");
	}

	// Compute the LUP factorization of A
//	::fortran_int_t* aux_ab = band_mat_to_lapack_vec(A, n);
//	vector< ::fortran_int_t> tmp_ipiv(k);
//	::std::ptrdiff_t info;
//	info = ::boost::numeric::bindings::lapack::detail::gbtrf(
//		nr,
//		nc,
//		kl,
//		ku,
//		aux_ab,
//		ldab,
//		tmp_ipiv.data().begin()
//	);
	work_matrix_type AB(A, kl, kl+ku); //NOTE: "kl+ku" is not a typo
	vector_type ipiv(k);
	::boost::numeric::bindings::lapack::gbtrf(AB, ipiv);

	// Finally, compute the reciprocal condition number
	::boost::numeric::bindings::lapack::gbcon(
		what_norm,
		AB,
		ipiv,
		norm,
		res
	);

	return res;
}
*/

template <
	typename ValueT,
	typename StorageT
>
typename type_traits<ValueT>::real_type rcond_impl(banded_matrix<ValueT,column_major,StorageT> const& A, matrix_norm_category norm_category, column_major_tag)
{
	typedef banded_matrix<ValueT,row_major,StorageT> work_matrix_type;

	work_matrix_type tmp_A(A, A.lower(), A.upper());

	return rcond_impl(tmp_A, norm_category, row_major_tag());
}

template <
	typename ValueT,
	typename StorageT
>
typename type_traits<ValueT>::real_type rcond_impl(banded_matrix<ValueT,row_major,StorageT> const& A, matrix_norm_category norm_category, row_major_tag)
{
	typedef banded_matrix<ValueT,row_major,StorageT> matrix_type;
    typedef typename matrix_traits<matrix_type>::value_type value_type;
	typedef typename matrix_traits<matrix_type>::size_type size_type;
	typedef typename type_traits<value_type>::real_type result_type;
	typedef banded_matrix<ValueT,row_major,StorageT> work_matrix_type;
	typedef vector< ::fortran_int_t > vector_type;

	size_type nr = num_rows(A);
	size_type nc = num_columns(A);

	// Check if A is a square matrix
	if (nr != nc)
	{
		// Non-square matrix -> Use QR decomposition
		if (nr < nc)
		{
			return rcond_impl(qr_decompose(trans(A)).R(), norm_category, column_major_tag());
		}
		else
		{
			return rcond_impl(qr_decompose(A).R(), norm_category, column_major_tag());
		}
	}

	char what_norm;
	result_type norm;
	result_type res;
	size_type k = ::std::min(nr,nc);
	size_type kl = A.lower();
	size_type ku = A.upper();
//	size_type ldab = 2*kl+ku+1;

	switch (norm_category)
	{
		case matrix_norm_1:
			what_norm = 'O';
			break;
		case matrix_norm_inf:
			what_norm = 'I';
			break;
//		case matrix_norm_frobenius:
//			what_norm = 'F';
//			break;
		default:
			throw std::runtime_error("[rcond::detail::rcond_impl] Unsupported norm category.");
	}

	// Compute the norm of A
	//FIXME: actually, in bindings this function is broken
//	::boost::numeric::bindings::lapack::lange(
//		what_norm,
//		A
//	);
	switch (norm_category)
	{
		case matrix_norm_1:
			norm = norm_1(A);
			break;
		case matrix_norm_inf:
			norm = norm_inf(A);
			break;
		default:
			throw std::runtime_error("[rcond::detail::rcond_impl] Unsupported norm category.");
	}

	// Compute the LUP factorization of A
//	::fortran_int_t* aux_ab = band_mat_to_lapack_vec(A, n);
//	vector< ::fortran_int_t> tmp_ipiv(k);
//	::std::ptrdiff_t info;
//	info = ::boost::numeric::bindings::lapack::detail::gbtrf(
//		nr,
//		nc,
//		kl,
//		ku,
//		aux_ab,
//		ldab,
//		tmp_ipiv.data().begin()
//	);
	work_matrix_type AB(A, kl, kl+ku); //NOTE: "kl+ku" is not a typo
	vector_type ipiv(k);
	::boost::numeric::bindings::lapack::gbtrf(AB, ipiv);

	// Finally, compute the reciprocal condition number
	::boost::numeric::bindings::lapack::gbcon(
		what_norm,
		AB,
		ipiv,
		norm,
		res
	);

	return res;
}

template <
	typename ValueT,
	typename TriangularT,
	typename StorageT
>
typename type_traits<ValueT>::real_type rcond_impl(symmetric_matrix<ValueT,TriangularT,column_major,StorageT> const& A, matrix_norm_category norm_category, column_major_tag)
{
	typedef symmetric_matrix<ValueT,TriangularT,column_major,StorageT> matrix_type;
    typedef typename matrix_traits<matrix_type>::value_type value_type;
	typedef typename matrix_traits<matrix_type>::size_type size_type;
	typedef typename type_traits<value_type>::real_type result_type;
	typedef matrix<ValueT,column_major> auxiliary_matrix_type;
	typedef symmetric_adaptor<auxiliary_matrix_type,TriangularT> work_matrix_type;
	typedef vector< ::fortran_int_t > vector_type;

	size_type nr = num_rows(A);
	size_type nc = num_columns(A);
	size_type n = ::std::min(nr, nc);

	// Check if A is a square matrix
	if (nr != nc)
	{
		// Non-square matrix -> Use QR decomposition
		if (nr < nc)
		{
			return rcond_impl(qr_decompose(trans(A)).R(), norm_category, column_major_tag());
		}
		else
		{
			return rcond_impl(qr_decompose(A).R(), norm_category, column_major_tag());
		}
	}

	result_type norm;
	result_type res;

	// Compute the norm of A
	//FIXME: actually, in bindings this function is broken
//	::boost::numeric::bindings::lapack::lange(
//		what_norm,
//		A
//	);
	switch (norm_category)
	{
		case matrix_norm_1:
			norm = norm_1(A);
			break;
		case matrix_norm_inf:
			norm = norm_inf(A);
			break;
		default:
			throw std::runtime_error("[rcond::detail::rcond_impl] Unsupported norm category.");
	}

	// Compute the LUP factorization of A
	//work_matrix_type AB(A);
	auxiliary_matrix_type aux_A(A);
	work_matrix_type AB(aux_A);
	vector_type ipiv(n);
	::boost::numeric::bindings::lapack::sytrf(AB, ipiv);

	// Finally, compute the reciprocal condition number
	::boost::numeric::bindings::lapack::sycon(
		AB,
		ipiv,
		norm,
		res
	);

	return res;
}

template <
	typename ValueT,
	typename TriangularT,
	typename StorageT
>
typename type_traits<ValueT>::real_type rcond_impl(symmetric_matrix<ValueT,TriangularT,row_major,StorageT> const& A, matrix_norm_category norm_category, row_major_tag)
{
	typedef symmetric_matrix<ValueT,TriangularT,column_major,StorageT> work_matrix_type;

	work_matrix_type tmp_A(A);

	return rcond_impl(tmp_A, norm_category, column_major_tag());
}

template <
	typename ValueT,
	typename TriangularT,
	typename StorageT
>
typename type_traits<ValueT>::real_type rcond_impl(hermitian_matrix<ValueT,TriangularT,column_major,StorageT> const& A, matrix_norm_category norm_category, column_major_tag)
{
	typedef hermitian_matrix<ValueT,TriangularT,column_major,StorageT> matrix_type;
    typedef typename matrix_traits<matrix_type>::value_type value_type;
	typedef typename matrix_traits<matrix_type>::size_type size_type;
	typedef typename type_traits<value_type>::real_type result_type;
	typedef matrix<ValueT,column_major> auxiliary_matrix_type;
	typedef hermitian_adaptor<auxiliary_matrix_type,TriangularT> work_matrix_type;
	typedef vector< ::fortran_int_t > vector_type;

	size_type nr = num_rows(A);
	size_type nc = num_columns(A);
	size_type n = ::std::min(nr, nc);

	// Check if A is a square matrix
	if (nr != nc)
	{
		// Non-square matrix -> Use QR decomposition
		if (nr < nc)
		{
			return rcond_impl(qr_decompose(trans(A)).R(), norm_category, column_major_tag());
		}
		else
		{
			return rcond_impl(qr_decompose(A).R(), norm_category, column_major_tag());
		}
	}

	result_type norm;
	result_type res;

	// Compute the norm of A
	//FIXME: actually, in bindings this function is broken
//	::boost::numeric::bindings::lapack::lange(
//		what_norm,
//		A
//	);
	switch (norm_category)
	{
		case matrix_norm_1:
			norm = norm_1(A);
			break;
		case matrix_norm_inf:
			norm = norm_inf(A);
			break;
		default:
			throw std::runtime_error("[rcond::detail::rcond_impl] Unsupported norm category.");
	}

	// Compute the LUP factorization of A
	auxiliary_matrix_type aux_A(A);
	work_matrix_type AB(aux_A);
	vector_type ipiv(n);
	::boost::numeric::bindings::lapack::hetrf(AB, ipiv);

	// Finally, compute the reciprocal condition number
	::boost::numeric::bindings::lapack::hecon(
		AB,
		ipiv,
		norm,
		res
	);

	return res;
}

template <
	typename ValueT,
	typename TriangularT,
	typename StorageT
>
typename type_traits<ValueT>::real_type rcond_impl(hermitian_matrix<ValueT,TriangularT,row_major,StorageT> const& A, matrix_norm_category norm_category, row_major_tag)
{
	typedef hermitian_matrix<ValueT,TriangularT,column_major,StorageT> work_matrix_type;

	work_matrix_type tmp_A(A);

	return rcond_impl(tmp_A, norm_category, column_major_tag());
}

} // Namespace detail

/**
 * \brief Matrix reciprocal condition number estimate based on 1-norm.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param A The input \e square matrix expression.
 * \return The estimate of the reciprocal condition number of \a A.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT>
typename type_traits<
	typename matrix_traits<MatrixExprT>::value_type
>::real_type rcond(matrix_expression<MatrixExprT> const& A)
{
	typedef typename matrix_traits<MatrixExprT>::orientation_category orientation_category;

	return detail::rcond_impl(A(), detail::matrix_norm_1, orientation_category());
}

//FIXME: Does we also need this?
///**
// * \brief Matrix reciprocal condition number estimate based on the matrix norm
// *  defined by parameter \a what_norm.
// *
// * \tparam MatrixExprT The type of the input matrix expression.
// * \tparam NormTagT The type of the matrix norm category.
// *
// * \param A The input \e square matrix expression.
// * \param what_norm The matrix norm category.
// * \return The estimate of the reciprocal condition number of \a A.
// *
// * \author Marco Guazzone, marco.guazzone@gmail.com
// */
//template <typename MatrixExprT, typename NormTagT>
//typename type_traits<
//	typename matrix_traits<MatrixExprT>::value_type
//>::real_type rcond(matrix_expression<MatrixExprT> const& A, NormTagT what_norm)
//{
//	typedef typename matrix_traits<MatrixExprT>::orientation_category orientation_category;
//
//	return detail::rcond_impl(A(), detail::matrix_norm_1, orientation_category());
//}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_RCOND_HPP


#pragma once

#include <boost/gil/gil_all.hpp>

template<typename Pixel, typename View>
void clip_pixel(std::int64_t const& x, std::int64_t const& y, Pixel const& pixel, View& view)
{
	if(x < 0 || y < 0)
	{
		return;
	}
	else if(x < view.width() && y < view.height())
	{
		view(x, y) = pixel;
	}
}

template<typename Pixel, typename View>
void bresenham_line(std::int64_t const& x1, std::int64_t const& y1, std::int64_t const& x2, std::int64_t const& y2, Pixel const& pixel, View& view)
{
	// Adapted from http://www.etechplanet.com/codesnippets/computer-graphics-draw-a-line-using-bresenham-algorithm.aspx

	std::int64_t const dx(x2 - x1);
	std::int64_t const dy(y2 - y1);
	std::int64_t const adx(std::abs(dx));
	std::int64_t const ady(std::abs(dy));
	std::int64_t px(2 * ady - adx);
	std::int64_t py(2 * adx - ady);
	std::int64_t x, y, xe, ye;
	if(adx < ady)
	{
		if(dy < 0)
		{
			x = x2;
			y = y2;
			ye = y1;
		}
		else
		{
			x = x1;
			y = y1;
			ye = y2;
		}
		clip_pixel(x, y, pixel, view);

		for(std::int64_t i(0); y < ye; ++ i)
		{
			y = y + 1;
			if(py > 0)
			{
				if((dx < 0 && dy < 0) || (dx > 0 && dy > 0))
				{
					x = x + 1;
				}
				else
				{
					x = x - 1;
				}
				py = py + 2 * (adx - ady);
			}
			else
			{
				py = py + 2 * adx;
			}
			clip_pixel(x, y, pixel, view);
		}
	}
	else
	{
		if(dx < 0)
		{
			x = x2;
			y = y2;
			xe = x1;
		}
		else
		{
			x = x1;
			y = y1;
			xe = x2;
		}
		clip_pixel(x, y, pixel, view);

		for(std::int64_t i(0); x < xe; ++ i)
		{
			x = x + 1;
			if(px < 0)
			{
				px = px + 2 * ady;
			}
			else
			{
				if((dx < 0 && dy < 0) || (dx > 0 && dy > 0))
				{
					y = y + 1;
				}
				else
				{
					y = y - 1;
				}
				px = px + 2 * (ady - adx);
			}
			clip_pixel(x, y, pixel, view);
		}
	}
}


// Copyright 2017 Nest Labs, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "graph.hpp"
#include "detector.hpp"
#include "graphanalyzer.hpp"
#include "processorcontainer.hpp"
#include "lag.hpp"

#include <iostream>
#include <cmath>
#include <algorithm>
#include <sstream>
#include <string>
#include <Eigen/Dense>

using namespace std;
using namespace DetectorGraph;
using namespace Eigen;

/**
 * @file robotlocalization.cpp
 * @brief A localization system for a mobile robot with Lag-based feedback loop.
 *
 * @section ex-rl-intro Introduction
 * Back in the 2000s all [_citation needed_] robots were _differential wheeled
 * robots_ [1] and localization with sensor fusion was a popular problem.
 * This example shows a simple solution to that problem using DetectorGraph.
 * It uses an Extended Kalman Filter [2] to continuously predict and correct
 * the robot's Pose (and associated uncertainty) from two information sources:
 * the input to the wheels and a GPS-like system.
 *
 * @section ex-rl-fpvslag FuturePublisher vs. Lag\<T\>
 * This localization algorithm depends on a feedback-loop in that the output of
 * one graph evaluation (i.e. `LocalizationBelief`) is used as an input for the
 * next one (i.e. `Lagged<LocalizationBelief>`):
 @snippetlineno robotlocalization.cpp KalmanPoseCorrector Feedback Loop
 * Note that in this graph the TopicState where the feedback loop closes is a
 * legitimate output in itself and it's very likely that new Detectors in the
 * graph would subscribe to it. In cases like this the use of
 * DetectorGraph::Lag is preferred as it is more extensible and it preserves
 * the normal TopicState guarantee for the TopicState in question as well as it
 * makes the Lagged version of the TopicState clearly documented and also
 * available. Lag allows even for detectors that subscribes to both the immediate
 * and the Lagged version of a TopicState - this could be useful for things like
 * differentiation etc.
 *
 * @section ex-rl-conf-topicstates Configuration TopicStates
 * It also shows how TopicStates can be used for static/configuration data
 * (e.g. `RobotConfig`). This allows for easy dependency tracking, visualization
 * and testing at pretty much no runtime cost.
 *
 * @section ex-rl-localization Localization Algorithm
 * Regarding the Localization algorithm itself, it's mostly taken from [3]
 * except for the correction model where this example uses a different input
 * (i.e. `GPSPosition`) with much simpler transfer function to the Pose vector.
 *
 * @section ex-rl-arch Architecture
 * The graph below shows the relationships between the topics (rectangles) and
 * detectors (ellipses). Note that this graph can be automatically generated
 * for any instance of DetectorGraph::Graph using DetectorGraph::GraphAnalyzer.
 *  @dot "RobotBrain"
digraph GraphAnalyzer {
    rankdir = "LR";
    node[fontname=Helvetica];
    size="12,5";

    "RobotConfig" [label="0:RobotConfig",style=filled, shape=box, color=lightblue];
        "RobotConfig" -> "ExtendedKalmanPosePredictor";
    "InitialPose" [label="1:InitialPose",style=filled, shape=box, color=lightblue];
        "InitialPose" -> "ExtendedKalmanPosePredictor";
    "WheelSpeeds" [label="2:WheelSpeeds",style=filled, shape=box, color=lightblue];
        "WheelSpeeds" -> "ExtendedKalmanPosePredictor";
    "GPSPosition" [label="3:GPSPosition",style=filled, shape=box, color=lightblue];
        "GPSPosition" -> "KalmanPoseCorrector";
    "LaggedLocalizationBelief" [label="4:Lagged<LocalizationBelief>",style=filled, shape=box, color=lightblue];
        "LaggedLocalizationBelief" -> "ExtendedKalmanPosePredictor";
        "LaggedLocalizationBelief" -> "KalmanPoseCorrector";
    "KalmanPoseCorrector" [label="6:KalmanPoseCorrector", color=blue];
        "KalmanPoseCorrector" -> "LocalizationBelief";
    "ExtendedKalmanPosePredictor" [label="5:ExtendedKalmanPosePredictor", color=blue];
        "ExtendedKalmanPosePredictor" -> "LocalizationBelief";
    "LocalizationBelief" [label="7:LocalizationBelief",style=filled, shape=box, color=red];
        "LocalizationBelief" -> "LagLocalizationBelief";
    "LagLocalizationBelief" [label="8:Lag<LocalizationBelief>", color=blue];
        "LagLocalizationBelief" -> "LaggedLocalizationBelief" [style=dotted, color=red, constraint=false];
}
 *  @enddot
 *
 *
 * @section ex-rl-other-notes Other Notes
 * This example also uses the Eigen library [4] for matrix/vector arithmetic
 * and linear-algebraic operations.
 *
 * Note that this entire algorithm is contained in a single file for the sake
 * of unity as an example. In real-world scenarios the suggested pattern is to
 * split the code into:
 *
 @verbatim
   detectorgraph/
        include/
            robotbrain.hpp (RobotBrain header)
        src/
            robotbrain.hpp (RobotBrain implementation)
        detectors/
            include/
                ExtendedKalmanPosePredictor.hpp
                KalmanPoseCorrector.hpp
            src/
                ExtendedKalmanPosePredictor.cpp
                KalmanPoseCorrector.cpp
        topicstates/
            include/
                RobotConfig.hpp
                InitialPose.hpp
                WheelSpeeds.hpp
                LocalizationBelief.hpp
                GPSPosition.hpp
@endverbatim
 *
 * @section ex-rl-refs References
 *  - [1] Differential Wheeled Robots - https://en.wikipedia.org/wiki/Differential_wheeled_robot
 *  - [2] Extended Kalman Filter - https://en.wikipedia.org/wiki/Extended_Kalman_filter
 *  - [3] EKF applied to Mobile Robot’s Localization (Section 2.4.4, page 17) - http://cpscotti.com/pdf/pfc_scotti.pdf
 *  - [4] Eigen - http://eigen.tuxfamily.org/dox/index.html
 */

/// @cond DO_NOT_DOCUMENT

// Contains the configuration parameters of this differential mobile Robot
// or vehicle.
struct RobotConfig : public TopicState
{
    RobotConfig() : r(), b(), e() {}
    RobotConfig(double aR, double aB, double aE)
        : r(aR), b(aB), e(aE) {}
    double r; // wheels r
    double b; // displacement between wheels 2*l (l=d(wheel,center))

    double e; // relative wheel slippage/error
};

struct InitialPose : public TopicState
{
    InitialPose() : timestampMs(0), pose(Vector3d::Zero()) {}
    InitialPose(Vector3d aPose) : timestampMs(0), pose(aPose) {}
    InitialPose(uint64_t aTimestampMs, Vector3d aPose) : timestampMs(aTimestampMs), pose(aPose) {}
    uint64_t timestampMs;
    Vector3d pose;
};

// The measured (via encoders or other odometry method) wheel speeds
struct WheelSpeeds : public TopicState
{
    WheelSpeeds() : timestampMs(), phi(Vector2d::Zero()) {}
    WheelSpeeds(uint64_t ts, Vector2d aPhi) : timestampMs(ts), phi(aPhi) {}
    WheelSpeeds(uint64_t ts, double r, double l) : timestampMs(ts), phi(r, l) {}
    uint64_t timestampMs;

    // phi_r, phi_l in radians
    Vector2d phi;
};

//! [TopicStates Composition Example]
struct KalmanState
{
    KalmanState() : pose(Vector3d::Zero()), error(Matrix3d::Zero()) {}
    KalmanState(Vector3d aPose, Matrix3d aError) : pose(aPose), error(aError) {}
    Vector3d pose;
    Matrix3d error;
};

//! [Mutually Atomic Variables]
struct LocalizationBelief : public TopicState
{
    LocalizationBelief() : timestampMs(), state() {}
    LocalizationBelief(uint64_t ts, const KalmanState& aState) : timestampMs(ts), state(aState) {}
    LocalizationBelief(uint64_t ts, const Vector3d& aPose, const Matrix3d& aError) : timestampMs(ts), state(aPose, aError) {}
    uint64_t timestampMs;
    KalmanState state;
};
//! [Mutually Atomic Variables]

struct GPSPosition : public TopicState
{
    GPSPosition() : timestampMs(), state() {}
    GPSPosition(uint64_t ts, const KalmanState& aState) : timestampMs(ts), state(aState) {}
    uint64_t timestampMs;
    KalmanState state;
};
//! [TopicStates Composition Example]

double WrapAngle(double th)
{
    // remainder requires C++11
    return std::remainder(th, 2. * M_PI);
}

class ExtendedKalmanPosePredictor : public Detector,
    public SubscriberInterface<RobotConfig>,
    public SubscriberInterface<InitialPose>,
    public SubscriberInterface< Lagged<LocalizationBelief> >,
    public SubscriberInterface<WheelSpeeds>,
    public Publisher<LocalizationBelief>
{
public:
    ExtendedKalmanPosePredictor(Graph* graph) : Detector(graph), mCurrentBelief(), mConfig()
    {
        Subscribe<RobotConfig>(this);
        Subscribe<InitialPose>(this);
        Subscribe< Lagged<LocalizationBelief> >(this);
        Subscribe<WheelSpeeds>(this);
        SetupPublishing<LocalizationBelief>(this);
    }

    virtual void Evaluate(const RobotConfig& aConfig)
    {
        mConfig = aConfig;
    }

    virtual void Evaluate(const InitialPose& aInitialPose)
    {
        mCurrentBelief.state = KalmanState(aInitialPose.pose, Matrix3d::Zero());
        mCurrentBelief.timestampMs = aInitialPose.timestampMs;
    }

    virtual void Evaluate(const Lagged<LocalizationBelief>& aLaggedBelief)
    {
        mCurrentBelief.state = aLaggedBelief.data.state;
        mCurrentBelief.timestampMs = aLaggedBelief.data.timestampMs;
    }

    virtual void Evaluate(const WheelSpeeds& aWheelSpeeds)
    {
        // [\Delta S_{r}, \Delta S_{l}] from phi
        Vector2d wheelLinearSpeed = aWheelSpeeds.phi * mConfig.r;

        // Eq. 2.36
        double d_th = (wheelLinearSpeed.x() - wheelLinearSpeed.y()) / mConfig.b;
        // Eq. 2.37
        double d_travel = (wheelLinearSpeed.x() + wheelLinearSpeed.y()) / 2.;

        double theta = mCurrentBelief.state.pose.z();

        // Used on most equations below
        double th_dth2 = theta + d_th / 2.;

        // Eq. 2.33 on [3]
        Vector3d cartesianSpeed;
        cartesianSpeed << d_travel * cos(th_dth2),
                          d_travel * sin(th_dth2),
                                             d_th;

        // Compute Time Delta
        double tDelta = (aWheelSpeeds.timestampMs - mCurrentBelief.timestampMs) * 1e-3;

        // Eq. 2.33 (i.e. 2.23) on [3]
        mCurrentBelief.state.pose = mCurrentBelief.state.pose + (cartesianSpeed * tDelta);
        mCurrentBelief.state.pose.z() = WrapAngle(mCurrentBelief.state.pose.z());

        // Eq. 2.34 on [3]
        Matrix3d G;
        G << 1, 0, -d_travel * sin(th_dth2),
             0, 1,  d_travel * cos(th_dth2),
             0, 0,                        1;

        // Eq. 2.35 on [3]
        Matrix<double, 3, 2> V;
        V << 0.5 * cos(th_dth2) - (d_travel / (2. * mConfig.b)) * sin(th_dth2), 0.5 * cos(th_dth2) + (d_travel / (2. * mConfig.b)) * sin(th_dth2),
             0.5 * sin(th_dth2) + (d_travel / (2. * mConfig.b)) * cos(th_dth2), 0.5 * sin(th_dth2) - (d_travel / (2. * mConfig.b)) * cos(th_dth2),
                                                                  1./mConfig.b,                                                     -1./mConfig.b;

        // Eq. 2.38 on [3]
        Matrix2d M;
        M << mConfig.e * abs(wheelLinearSpeed.x()),                                     0,
                                                 0, mConfig.e * abs(wheelLinearSpeed.y());

        // Eq. 2.24 on [3]
        mCurrentBelief.state.error = G * mCurrentBelief.state.error * G.transpose() + V * M * V.transpose();

        mCurrentBelief.timestampMs = aWheelSpeeds.timestampMs;
        Publish(mCurrentBelief);
    }

    LocalizationBelief mCurrentBelief;
    RobotConfig mConfig;
};

///! [KalmanPoseCorrector Feedback Loop]
class KalmanPoseCorrector : public Detector,
    public SubscriberInterface< Lagged<LocalizationBelief> >,
    public SubscriberInterface<GPSPosition>,
    public Publisher<LocalizationBelief>
{
public:
    KalmanPoseCorrector(Graph* graph) : Detector(graph), mCurrentBelief()
    {
        Subscribe< Lagged<LocalizationBelief> >(this);
        Subscribe<GPSPosition>(this);
        SetupPublishing<LocalizationBelief>(this);
    }
///! [KalmanPoseCorrector Feedback Loop]

    virtual void Evaluate(const Lagged<LocalizationBelief>& aLaggedBelief)
    {
        mCurrentBelief.state = aLaggedBelief.data.state;
        mCurrentBelief.timestampMs = aLaggedBelief.data.timestampMs;
    }

    virtual void Evaluate(const GPSPosition& aCorrectionInput)
    {
        // C = Identity(3)

        // Eq. 2.15 on [3]
        Matrix3d K = mCurrentBelief.state.error * (mCurrentBelief.state.error + aCorrectionInput.state.error).inverse();

        // Eq. 2.13 on [3]
        mCurrentBelief.state.pose = mCurrentBelief.state.pose + K * (aCorrectionInput.state.pose - mCurrentBelief.state.pose);

        // Eq. 2.14 on [3]
        mCurrentBelief.state.error = (Matrix3d::Identity() - K) * mCurrentBelief.state.error;

        Publish(mCurrentBelief);
    }

    LocalizationBelief mCurrentBelief;
};

//![RobotBrain Static]
class RobotBrain : public ProcessorContainer
{
public:
    RobotBrain()
    : mPosePredictor(&mGraph)
    , mKalmanPoseCorrector(&mGraph)
    , mBeliefFeedback(&mGraph)
    {
    }

    ExtendedKalmanPosePredictor mPosePredictor;
    KalmanPoseCorrector mKalmanPoseCorrector;
    Lag<LocalizationBelief> mBeliefFeedback;
    //![RobotBrain Static]

    virtual void ProcessOutput()
    {
        Topic<LocalizationBelief>* correctedStateTopic = mGraph.ResolveTopic<LocalizationBelief>();
        if (correctedStateTopic->HasNewValue())
        {
            const LocalizationBelief& belief = correctedStateTopic->GetNewValue();
            const auto& pose = belief.state.pose;
            cout << "Pose = [" << pose.x() << ", " << pose.y() << ", " << pose.z() << "], ";
            const auto& error = belief.state.error.diagonal();
            cout << "Error = [" << error.x() << ", " << error.y() << ", " << error.z() << "]";
            cout << endl;
        }
    }
};

int main()
{
    RobotBrain robot;

    uint64_t kSimStep = 1000;
    uint64_t dataIndex = 0;

    robot.ProcessData(RobotConfig(1, 1, 0.1));
    robot.ProcessData(InitialPose(Vector3d(10.0, 10.0, 0.0)));

    // Circular Movement
    // for (int i=0;i<100;i++) robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1., 0.5));

    // Fwd, Rotate ~180, Fwd, GPS Update
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, -1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, -1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(GPSPosition((dataIndex++)*kSimStep,
        KalmanState(Vector3d(10., 10., 4),  // Position
                    Vector3d(0.1,0.1,300000).asDiagonal()  // Error
            )));

    cout << "---- Done ----" << endl;

    GraphAnalyzer analyzer(robot.mGraph);
    analyzer.GenerateDotFile("robot_localization.dot");

    return 0;
}

/// @endcond DO_NOT_DOCUMENT


/**
 * @file utils.cpp
 * @brief Usefult utilities
 * @author Parker Lusk <plusk@mit.edu>
 * @date 12 October 2020
 */

#include <functional>
#include <queue>
#include <random>
#include <utility>
#include <vector>

#include <Eigen/Dense>

#include "clipper/utils.h"

namespace clipper {
namespace utils {

Eigen::VectorXd randvec(size_t n)
{
  std::random_device rd;
  std::mt19937 gen(rd());
  std::uniform_real_distribution<double> dis(0, 1);

  return Eigen::VectorXd::NullaryExpr(n, 1, [&](){ return dis(gen); });
}

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

std::vector<int> findIndicesOfkLargest(const Eigen::VectorXd& x, int k)
{
  using T = std::pair<double, int>; // pair value to be compared and index
  if (k < 1) return {}; // invalid input
  // n.b., the top of this queue is smallest element
  std::priority_queue<T, std::vector<T>, std::greater<T>> q;
  for (size_t i=0; i<x.rows(); ++i) {
    if (q.size() < k) {
      q.push({x(i), i});
    } else if (q.top().first < x(i)) {
      q.pop();
      q.push({x(i), i});
    }
  }

  std::vector<int> indices(k);
  for (size_t i=0; i<k; ++i) {
    indices[k - i - 1] = q.top().second;
    q.pop();
  }

  return indices;
}

} // ns utils
} // ns clipper


#include "problemes.h"
#include "arithmetique.h"

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

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;

namespace {
    bool intersection(const std::set<nombre> &a, const std::set<nombre> &b) {
        std::set<nombre> i;
        std::set_intersection(a.begin(), a.end(), b.begin(), b.end(), std::inserter(i, i.begin()));
        return i.empty();
    }
}

ENREGISTRER_PROBLEME(215, "Crack-free Walls") {
    // Consider the problem of building a wall out of 2×1 and 3×1 bricks (horizontal×vertical dimensions) such that, for
    // extra strength, the gaps between horizontally-adjacent bricks never line up in consecutive layers, i.e. never
    // form a "running crack".
    //
    // For example, the following 9×3 wall is not acceptable due to the running crack shown in red:
    //
    // There are eight ways of forming a crack-free 9×3 wall, written W(9,3) = 8.
    //
    // Calculate W(32,10).
    nombre taille = 32;
    nombre hauteur = 10;
    std::vector<std::set<nombre>> murs;
    for (nombre n = 0; 3 * n <= taille; ++n) {
        if ((taille - 3 * n) % 2 == 0) {
            vecteur mur((taille - 3 * n) / 2, 2);
            mur.insert(mur.end(), n, 3);
            do {
                std::set<nombre> set_mur;
                nombre l = 0;
                for (auto m: mur) {
                    set_mur.insert(l);
                    l += m;
                }
                set_mur.erase(set_mur.begin());
                murs.push_back(set_mur);
            } while (std::next_permutation(mur.begin(), mur.end()));
        }
    }

    std::map<std::set<nombre>, std::vector<std::set<nombre>>> I;
    for (const auto &m1: murs)
        for (const auto &m2: murs) {
            if (intersection(m1, m2))
                I[m1].push_back(m2);
        }

    std::map<std::set<nombre>, nombre> dp;
    for (const auto&[k, v]: I)
        dp[k] = v.size();

    for (nombre h = 2; h < hauteur; ++h) {
        std::map<std::set<nombre>, nombre> suite_dp;
        for (const auto&[k, v]: dp) {
            for (const auto &i: I[k])
                suite_dp[i] += v;
        }

        std::swap(suite_dp, dp);
    }

    nombre resultat = 0;
    for (const auto&[k, v]: dp)
        resultat += v;

    return std::to_string(resultat);
}


// =============================================================================
//  BoostIncludes.hpp
//
//  MIT License
//
//  Copyright (c) 2007-2018 Dairoku Sekiguchi
//
//  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.
// =============================================================================
/*!
	\file		BoostIncludes.hpp
	\author		Dairoku Sekiguchi
	\version	1.0
	\date		2008/02/22
	\brief
*/
#ifndef __BOOST_INCLUDES_H
#define __BOOST_INCLUDES_H

#pragma warning(disable:4996)		// to suppress std::uninitialized_copy warning

// -----------------------------------------------------------------------------
// 	include files
// -----------------------------------------------------------------------------
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/bindings/lapack/gesv.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;
namespace traits = boost::numeric::bindings::traits;

#endif	// #ifdef __BOOST_INCLUDES_H


#include <Eigen/Eigenvalues>
#include <cmath>
#include <iostream>
#include <solvers/bfgs.hpp>
#include <solvers/sqp.hpp>

#ifndef SOLVER_ASSERT
#define SOLVER_ASSERT(x) eigen_assert(x)
#endif

namespace sqp {

template <typename T>
SQP<T>::SQP() {
    // TODO(mi): Performance strongly depends on QP solver settings, which is bad.
    qp_solver_.settings().warm_start = true;
    qp_solver_.settings().check_termination = 10;
    qp_solver_.settings().eps_abs = 1e-4;
    qp_solver_.settings().eps_rel = 1e-4;
    qp_solver_.settings().max_iter = 100;
    qp_solver_.settings().adaptive_rho = true;
    qp_solver_.settings().adaptive_rho_interval = 50;
    qp_solver_.settings().alpha = 1.6;
}

template <typename T>
void SQP<T>::solve(Problem& prob, const Vector& x0, const Vector& lambda0) {
    x_ = x0;
    lambda_ = lambda0;
    run_solve(prob);
}

template <typename T>
void SQP<T>::solve(Problem& prob) {
    const int nx = prob.num_var;
    const int nc = prob.num_constr;

    x_.setZero(nx);
    lambda_.setZero(nc);
    run_solve(prob);
}

template <typename T>
void SQP<T>::run_solve(Problem& prob) {
    Vector p;         // search direction
    Vector p_lambda;  // dual search direction
    Scalar alpha;     // step size

    const int nx = prob.num_var;
    const int nc = prob.num_constr;

    p.resize(nx);
    p_lambda.resize(nc);

    step_prev_.resize(nx);
    grad_L_.resize(nx);
    delta_grad_L_.resize(nx);

    Hess_.resize(nx, nx);
    grad_obj_.resize(nx);
    Jac_constr_.resize(nc, nx);
    constr_.resize(nc);
    l_.resize(nc);
    u_.resize(nc);

    info_.qp_solver_iter = 0;

    if (settings_.iteration_callback) {
        settings_.iteration_callback(*this);
    }

    int& iter = info_.iter;
    for (iter = 1; iter <= settings_.max_iter; iter++) {
        // Solve QP
        solve_qp(prob, p, p_lambda);
        p_lambda -= lambda_;

        alpha = line_search(prob, p);

        // take step
        x_ = x_ + alpha * p;
        lambda_ = lambda_ + alpha * p_lambda;

        // update step info
        step_prev_ = alpha * p;
        primal_step_norm_ = alpha * p.template lpNorm<Eigen::Infinity>();
        dual_step_norm_ = alpha * p_lambda.template lpNorm<Eigen::Infinity>();

        if (settings_.iteration_callback) {
            settings_.iteration_callback(*this);
        }

        if (termination_criteria(x_, prob)) {
            info_.status = SOLVED;
            break;
        }
    }
    if (iter > settings_.max_iter) {
        info_.status = MAX_ITER_EXCEEDED;
    }
}

template <typename Matrix>
bool is_posdef_eigen(Matrix H) {
    Eigen::EigenSolver<Matrix> eigensolver(H);
    for (int i = 0; i < eigensolver.eigenvalues().rows(); i++) {
        double v = eigensolver.eigenvalues()(i).real();
        if (v <= 0) {
            return false;
        }
    }
    return true;
}

template <typename Matrix>
bool is_posdef(Matrix H) {
    Eigen::LLT<Matrix> llt(H);
    if (llt.info() == Eigen::NumericalIssue) {
        return false;
    }
    return true;
}

template <typename T>
bool SQP<T>::termination_criteria(const Vector& x, Problem& prob) {
    if (primal_step_norm_ <= settings_.eps_prim && dual_step_norm_ <= settings_.eps_dual &&
        max_constraint_violation(x, prob) <= settings_.eps_prim) {
        return true;
    }
    return false;
}

template <typename Derived>
inline bool is_nan(const Eigen::MatrixBase<Derived>& x) {
    // return ((x.array() == x.array())).all();
    return x.array().isNaN().any();
}

template <typename T>
void SQP<T>::solve_qp(Problem& prob, Vector& step, Vector& lambda) {
    /* QP from linearized NLP:
     * minimize     0.5 x'.P.x + q'.x
     * subject to   l <= A.x + b <= u
     *
     * with:
     *   P      Hessian of Lagrangian
     *   q      objective gradient
     *   A,b    linearized constraint at current iterate
     *   l,u    constraint bounds
     *
     * transform to:
     * minimize     0.5 x'.P.x + q'.x
     * subject to   l <= A.x <= u
     *
     * Where the constraint bounds l,u set to l=u for equality constraints or
     * set to +/-INFINITY if unbounded.
     */
    prob.objective_linearized(x_, grad_obj_, obj_);
    prob.constraint_linearized(x_, Jac_constr_, constr_, l_, u_);

    delta_grad_L_ = -grad_L_;
    grad_L_ = grad_obj_ + Jac_constr_.transpose() * lambda_;

    // BFGS update
    if (info_.iter == 1) {
        Hess_.setIdentity();
    } else {
        delta_grad_L_ += grad_L_;  // delta_grad_L_ = grad_L_prev - grad_L
        BFGS_update(Hess_, step_prev_, delta_grad_L_);
    }

    if (!is_posdef(Hess_)) {
        std::cout << "Hessian not positive definite\n";
        Scalar tau = 1e-3;
        Vector v = Vector(prob.num_var);
        while (!is_posdef(Hess_)) {
            v.setConstant(tau);
            Hess_ += v.asDiagonal();
            tau *= 10;
        }
    }
    if (is_nan(Hess_)) {
        std::cout << "Hessian is NaN\n";
    }

    SOLVER_ASSERT(is_posdef(Hess_));
    SOLVER_ASSERT(!is_nan(Hess_));

    // Constraints
    // from   l <= A.x + b <= u
    // to   l-b <= A.x     <= u-b
    Vector l = l_ - constr_;
    Vector u = u_ - constr_;
    Matrix& A = Jac_constr_;
    Matrix& P = Hess_;
    Vector& q = grad_obj_;

    // solve the QP
    run_solve_qp(P, q, A, l, u, step, lambda);

    if (settings_.second_order_correction) {
        second_order_correction(prob, step, lambda);
    }

    // TODO:
    // B is not convex then use grad_L as step direction
    // i.e. fallback to steepest descent of Lagrangian
}

template <typename T>
bool SQP<T>::run_solve_qp(const Matrix& P, const Vector& q, const Matrix& A, const Vector& l,
                          const Vector& u, Vector& prim, Vector& dual) {
    qp_solver::QuadraticProblem<Scalar> qp_;

    qp_.P = &P;
    qp_.q = &q;
    qp_.A = &A;
    qp_.l = &l;
    qp_.u = &u;

    qp_solver_.setup(qp_);
    qp_solver_.solve(qp_);

    info_.qp_solver_iter += qp_solver_.info().iter;

    if (qp_solver_.info().status == qp_solver::NUMERICAL_ISSUES) {
        std::cout << "QPSolver NUMERICAL_ISSUES\n";
        return false;
    }
    // if (qp_solver_.info().status == qp_solver::MAX_ITER_EXCEEDED) {
    //     std::cout << "QPSolver MAX_ITER_EXCEEDED\n";
    //     return false;
    // }

    prim = qp_solver_.primal_solution();
    dual = qp_solver_.dual_solution();

    SOLVER_ASSERT(!is_nan(prim));
    SOLVER_ASSERT(!is_nan(dual));

    return true;
}

template <typename T>
void SQP<T>::second_order_correction(Problem& prob, Vector& p, Vector& lambda) {
    // Scalar mu, constr_l1, phi_l1;
    // constr_l1 = constraint_norm(constr_, l_, u_);
    // mu = (grad_obj_.dot(p) + 0.5 * p.dot(Hess_ * p)) / ((1 - settings_.rho) * constr_l1);
    // phi_l1 = obj_ + mu * constr_l1;

    // Scalar obj_step, constr_l1_step, phi_l1_step;
    // Vector x_step = x_ + p;
    // prob.objective(x_step, obj_step);
    // constr_l1_step = constraint_norm(x_step, prob);
    // phi_l1_step = obj_step + mu * constr_l1_step;

    // printf("phi_l1_step %f  phi_l1 %f  constr_l1_step %f  constr_l1 %f\n", phi_l1_step, phi_l1,
    //        constr_l1_step, constr_l1);
    // if (phi_l1_step >= phi_l1 && constr_l1_step >= constr_l1) {
    {
        Vector x_step = x_ + p;
        Vector constr_step(constr_.rows());
        prob.constraint(x_step, constr_step, l_, u_);

        Matrix& A = Jac_constr_;
        Matrix& P = Hess_;
        Vector& q = grad_obj_;

        Vector d = constr_step - A * p;
        Vector l = l_ - d;
        Vector u = u_ - d;

        // TODO: only l and u change, possible to update QP solver more efficiently
        run_solve_qp(P, q, A, l, u, p, lambda);
    }
}
template <typename T>
typename SQP<T>::Scalar SQP<T>::line_search(Problem& prob, const Vector& p) {
    // Note: using members obj_ and grad_obj_, which are updated in solve_qp().

    Scalar mu, phi_l1, Dp_phi_l1;
    const Scalar tau = settings_.tau;  // line search step decrease, 0 < tau < settings.tau

    Scalar constr_l1 = constraint_norm(constr_, l_, u_);

    // get mu from merit function model using hessian of Lagrangian instead
    mu = (grad_obj_.dot(p) + 0.5 * p.dot(Hess_ * p)) / ((1 - settings_.rho) * constr_l1);

    phi_l1 = obj_ + mu * constr_l1;
    Dp_phi_l1 = grad_obj_.dot(p) - mu * constr_l1;

    Scalar alpha = 1.0;
    int i;
    for (i = 1; i < settings_.line_search_max_iter; i++) {
        Scalar obj_step;
        Vector x_step = x_ + alpha * p;
        prob.objective(x_step, obj_step);

        Scalar phi_l1_step = obj_step + mu * constraint_norm(x_step, prob);
        if (phi_l1_step <= phi_l1 + alpha * settings_.eta * Dp_phi_l1) {
            // accept step
            break;
        } else {
            alpha = tau * alpha;
        }
    }
    return alpha;
}

template <typename T>
typename SQP<T>::Scalar SQP<T>::constraint_norm(const Vector &constr, const Vector &l, const Vector &u) const {
    Scalar c_l1 = DIV_BY_ZERO_REGUL;

    // l <= c(x) <= u
    c_l1 += (l - constr).cwiseMax(0.0).sum();
    c_l1 += (constr - u).cwiseMax(0.0).sum();

    return c_l1;
}

template <typename T>
typename SQP<T>::Scalar SQP<T>::constraint_norm(const Vector& x, Problem& prob) {
    // Note: uses members constr_, l_ and u_ as temporary
    prob.constraint(x, constr_, l_, u_);

    return constraint_norm(constr_, l_, u_);
}

template <typename T>
typename SQP<T>::Scalar SQP<T>::max_constraint_violation(const Vector& x, Problem& prob) {
    // Note: uses members constr_, l_ and u_ as temporary

    Scalar c_max = 0;
    prob.constraint(x, constr_, l_, u_);

    // l <= c(x) <= u
    if (prob.num_constr > 0) {
        c_max = fmax(c_max, (l_ - constr_).maxCoeff());
        c_max = fmax(c_max, (constr_ - u_).maxCoeff());
    }

    return c_max;
}

template class SQP<double>;
template class SQP<float>;

}  // namespace sqp