// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// Polygon Example

#include <algorithm> // for reverse, unique
#include <iostream>
#include <string>

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)

std::string boolstr(bool v)
{
    return v ? "true" : "false";
}

int main(void)
{
    using namespace boost::geometry;

    typedef model::d2::point_xy<double> point_2d;
    typedef model::polygon<point_2d> polygon_2d;
    typedef model::box<point_2d> box_2d;

    // Define a polygon and fill the outer ring.
    // In most cases you will read it from a file or database
    polygon_2d poly;
    {
        const double coor[][2] = {
            {2.0, 1.3}, {2.4, 1.7}, {2.8, 1.8}, {3.4, 1.2}, {3.7, 1.6},
            {3.4, 2.0}, {4.1, 3.0}, {5.3, 2.6}, {5.4, 1.2}, {4.9, 0.8}, {2.9, 0.7},
            {2.0, 1.3} // closing point is opening point
            };
        assign_points(poly, coor);
    }

    // Polygons should be closed, and directed clockwise. If you're not sure if that is the case,
    // call the correct algorithm
    correct(poly);

    // Polygons can be streamed as text
    // (or more precisely: as DSV (delimiter separated values))
    std::cout << dsv(poly) << std::endl;

    // As with lines, bounding box of polygons can be calculated
    box_2d b;
    envelope(poly, b);
    std::cout << dsv(b) << std::endl;

    // The area of the polygon can be calulated
    std::cout << "area: " << area(poly) << std::endl;

    // And the centroid, which is the center of gravity
    point_2d cent;
    centroid(poly, cent);
    std::cout << "centroid: " << dsv(cent) << std::endl;

    // The number of points can be requested per ring (using .size())
    // or per polygon (using num_points)
    std::cout << "number of points in outer ring: " << poly.outer().size() << std::endl;

    // Polygons can have one or more inner rings, also called holes, islands, interior rings.
    // Let's add one
    {
        poly.inners().resize(1);
        model::ring<point_2d>& inner = poly.inners().back();

        const double coor[][2] = { {4.0, 2.0}, {4.2, 1.4}, {4.8, 1.9}, {4.4, 2.2}, {4.0, 2.0} };
        assign_points(inner, coor);
    }

    correct(poly);

    std::cout << "with inner ring:" << dsv(poly) << std::endl;
    // The area of the polygon is changed of course
    std::cout << "new area of polygon: " << area(poly) << std::endl;
    centroid(poly, cent);
    std::cout << "new centroid: " << dsv(cent) << std::endl;

    // You can test whether points are within a polygon
    std::cout << "point in polygon:"
        << " p1: "  << boolstr(within(make<point_2d>(3.0, 2.0), poly))
        << " p2: "  << boolstr(within(make<point_2d>(3.7, 2.0), poly))
        << " p3: "  << boolstr(within(make<point_2d>(4.4, 2.0), poly))
        << std::endl;

    // As with linestrings and points, you can derive from polygon to add, for example,
    // fill color and stroke color. Or SRID (spatial reference ID). Or Z-value. Or a property map.
    // We don't show this here.

    // Clip the polygon using a box
    box_2d cb(make<point_2d>(1.5, 1.5), make<point_2d>(4.5, 2.5));
    typedef std::vector<polygon_2d> polygon_list;
    polygon_list v;

    intersection(cb, poly, v);
    std::cout << "Clipped output polygons" << std::endl;
    for (polygon_list::const_iterator it = v.begin(); it != v.end(); ++it)
    {
        std::cout << dsv(*it) << std::endl;
    }

    typedef model::multi_polygon<polygon_2d> polygon_set;
    polygon_set ps;
    union_(cb, poly, ps);

    polygon_2d hull;
    convex_hull(poly, hull);
    std::cout << "Convex hull:" << dsv(hull) << std::endl;

    // If you really want:
    //   You don't have to use a vector, you can define a polygon with a deque
    //   You can specify the container for the points and for the inner rings independantly

    typedef model::polygon<point_2d, true, true, std::deque, std::deque> deque_polygon;
    deque_polygon poly2;
    ring_type<deque_polygon>::type& ring = exterior_ring(poly2);
    append(ring, make<point_2d>(2.8, 1.9));
    append(ring, make<point_2d>(2.9, 2.4));
    append(ring, make<point_2d>(3.3, 2.2));
    append(ring, make<point_2d>(3.2, 1.8));
    append(ring, make<point_2d>(2.8, 1.9));
    std::cout << dsv(poly2) << std::endl;

    return 0;
}


/*
 * AttitudeESKF.cpp
 *
 *  Copyright (c) 2013 Gareth Cross. Apache 2 License.
 *
 *  This file is part of kr_attitude_eskf.
 *
 *	Created on: 12/24/2013
 *		  Author: gareth
 */

#ifndef NDEBUG
#define NDEBUG
#endif

#include "AttitudeESKF.hpp"
#include <Eigen/LU>
#include <Eigen/Cholesky>
#include <iostream>
#include <cmath>

using namespace Eigen;

namespace kr {

//	skew symmetric matrix
template <typename T>
static inline Matrix<T, 3, 3> crossSkew(const Matrix<T, 3, 1> &w) {
  Matrix<T, 3, 3> W;

  W(0, 0) = 0;
  W(0, 1) = -w(2);
  W(0, 2) = w(1);

  W(1, 0) = w(2);
  W(1, 1) = 0;
  W(1, 2) = -w(0);

  W(2, 0) = -w(1);
  W(2, 1) = w(0);
  W(2, 2) = 0;

  return W;
}

//	hardcoded 3x3 invert (unchecked)
template <typename T>
static inline Matrix<T, 3, 3> invert(const Matrix<T, 3, 3> &A, T det) {
  Matrix<T, 3, 3> C;
  det = 1 / det;

  C(0, 0) = (-A(2, 1) * A(1, 2) + A(1, 1) * A(2, 2)) * det;
  C(0, 1) = (-A(0, 1) * A(2, 2) + A(0, 2) * A(2, 1)) * det;
  C(0, 2) = (A(0, 1) * A(1, 2) - A(0, 2) * A(1, 1)) * det;

  C(1, 0) = (A(2, 0) * A(1, 2) - A(1, 0) * A(2, 2)) * det;
  C(1, 1) = (-A(2, 0) * A(0, 2) + A(0, 0) * A(2, 2)) * det;
  C(1, 2) = (A(1, 0) * A(0, 2) - A(0, 0) * A(1, 2)) * det;

  C(2, 0) = (-A(2, 0) * A(1, 1) + A(1, 0) * A(2, 1)) * det;
  C(2, 1) = (A(2, 0) * A(0, 1) - A(0, 0) * A(2, 1)) * det;
  C(2, 2) = (-A(1, 0) * A(0, 1) + A(0, 0) * A(1, 1)) * det;

  return C;
}

//	hardcoded determinant
template <typename T> static inline T determinant(const Matrix<T, 3, 3> &A) {
  return A(0, 0) * (A(1, 1) * A(2, 2) - A(1, 2) * A(2, 1)) -
         A(0, 1) * (A(1, 0) * A(2, 2) - A(1, 2) * A(2, 0)) +
         A(0, 2) * (A(1, 0) * A(2, 1) - A(1, 1) * A(2, 0));
}

//  Eigen does not define these operators, which we use for integration
template <typename Scalar>
static inline Eigen::Quaternion<Scalar> operator + (const Eigen::Quaternion<Scalar>& a,
                                      const Eigen::Quaternion<Scalar>& b) {
  return Eigen::Quaternion<Scalar>(a.w()+b.w(),
                                   a.x()+b.x(),
                                   a.y()+b.y(),
                                   a.z()+b.z());
}

template <typename Scalar>
static inline Eigen::Quaternion<Scalar> operator * (const Eigen::Quaternion<Scalar>& q,
                                      Scalar s) {
  return Eigen::Quaternion<Scalar>(q.w() * s,
                                   q.x() * s,
                                   q.y() * s,
                                   q.z() * s);
}

/**
 *  @brief Integrate a rotation quaterion using Euler integration
 *  @param q Quaternion to integrate
 *  @param w Angular velocity (body frame), stored in 3 complex terms
 *  @param dt Time interval in seconds
 *  @param normalize If True, quaternion is normalized after integration
 */
template <typename Scalar>
static inline void integrateEuler(Eigen::Quaternion<Scalar> &q, Eigen::Quaternion<Scalar> &w, Scalar dt,
                    bool normalize = true) {
  q = q + (q * w * static_cast<Scalar>(0.5)) * dt;

  if (normalize) {
    q.normalize();
  }
}

/**
 *  @brief Integrate a rotation quaternion using 4th order Runge Kutta
 *  @param q Quaternion to integrate
 *  @param w Angular velocity (body frame), stored in 3 complex terms
 *  @param dt Time interval in seconds
 *  @param normalize If true, quaternion is normalized after integration
 */
template <typename Scalar>
static inline void integrateRungeKutta4(Eigen::Quaternion<Scalar> &q, const Eigen::Quaternion<Scalar> &w, Scalar dt,
                          bool normalize = true) {
  const static Scalar half = static_cast<Scalar>(0.5);
  const static Scalar two = static_cast<Scalar>(2);

  Eigen::Quaternion<Scalar> qw = q * w * half;
  Eigen::Quaternion<Scalar> k2 = (q + qw * dt * half) * w * half;
  Eigen::Quaternion<Scalar> k3 = (q + k2 * dt * half) * w * half;
  Eigen::Quaternion<Scalar> k4 = (q + k3 * dt) * w * half;

  q = q + (qw + k2 * two + k3 * two + k4) * (dt / 6);

  if (normalize) {
    q.normalize();
  }
}

template <typename Scalar>
static inline Eigen::Matrix<Scalar,3,3>
rodrigues(const Eigen::Matrix<Scalar,3,1>& w) {
  const auto norm = w.norm();
  if (norm < std::numeric_limits<Scalar>::epsilon()*10) {
    return Eigen::Matrix<Scalar,3,3>::Identity() + crossSkew(w);
  }
  return Eigen::AngleAxis<Scalar>(norm, w / norm).matrix();
}

AttitudeESKF::AttitudeESKF()
    : q_(1,0,0,0), steadyCount_(0), biasThresh_(0), isStable_(true) {
  P_.setZero();
  b_.setZero();
  w_.setZero();
  dx_.setZero();

  magRef_.setZero();
  predMag_.setZero();

  estBias_ = false;
  ignoreZ_ = false;
  useMag_ = false;
}

void AttitudeESKF::predict(const AttitudeESKF::vec3 &wb,
                           AttitudeESKF::scalar_t dt,
                           const AttitudeESKF::mat3 &cov,
                           bool useRK4) {
  static const Matrix<scalar_t, 3, 3> I3 =
      Matrix<scalar_t, 3, 3>::Identity(); //  identity R3

  scalar_t wb2 = wb[0] * wb[0] + wb[1] * wb[1] + wb[2] * wb[2];
  if (wb2 < biasThresh_ * biasThresh_) {
    steadyCount_++; //  not rotating, update moving average

    if (estBias_ && steadyCount_ > 20) {
      b_ = (b_ * (steadyCount_ - 1) + wb) / steadyCount_;
    }
  } else {
    steadyCount_ = 0;
  }

  w_ = (wb - b_); //	true gyro reading

  //	error-state jacobian
  const Matrix<scalar_t, 3, 3> F = I3 - crossSkew<scalar_t>(w_ * dt);

  //  integrate state and covariance
  Eigen::Quaternion<scalar_t> wQuat(0, w_[0], w_[1], w_[2]);
  if (!useRK4) {
    integrateEuler(q_, wQuat, dt, true);
  } else {
    integrateRungeKutta4(q_, wQuat, dt, true);
  }

  //  noise jacobian
  const Matrix <scalar_t,3,3> G = -I3 * dt;
  P_ = F*P_*F.transpose() + G*cov*G.transpose();
}

void AttitudeESKF::update(const AttitudeESKF::vec3 &ab,
                          const mat3 &aCov,
                          const AttitudeESKF::vec3 &mb,
                          const mat3 &mCov) {
  Matrix<scalar_t, 3, 3> A;

  //  rotation matrix: world -> body
  const Matrix<scalar_t, 3, 3> bRw = q_.conjugate().matrix();

  vec3 gravity;
  gravity[0] = 0.0;
  gravity[1] = 0.0;
  gravity[2] = kOneG;

  //  predicted gravity vector
  const vec3 aPred = bRw * gravity;

  if (!useMag_) {
    //  calculate jacobian
    Matrix<scalar_t, 3, 3> H = crossSkew(aPred);
    Matrix<scalar_t, 3, 1> r = ab - aPred;

    //  solve for the kalman gain
    const Matrix<scalar_t, 3, 3> S = H * P_ * H.transpose() + aCov;
    Matrix<scalar_t, 3, 3> Sinv;

    const scalar_t det = determinant(S);
    if (std::abs(det) < static_cast<scalar_t>(1e-5)) {
      isStable_ = false;
      return;
    } else {
      isStable_ = true;
    }
    Sinv = invert(S, det);

    const Matrix<scalar_t, 3, 3> K = P_ * H.transpose() * Sinv;

    A = K * H;
    dx_ = K * r;
  }
  else {
#ifdef ATTITUDE_ESKF_BUILD_MAG  //  stop compilation of FullPivLU
    //  m-field prediction
    vec3 field = bRw * magRef_;
    predMag_ = field;

    Matrix<scalar_t, 6, 1> r;
    r.block<3, 1>(0, 0) = ab - aPred;
    r.block<3, 1>(3, 0) = mb - field;

    Matrix<scalar_t, 6, 3> H;
    H.setZero();

    //  jacobians for gravity and magnetic field
    H.block<3, 3>(0, 0) = crossSkew(aPred);
    H.block<3, 3>(3, 0) = crossSkew(field);

    //  covariance for both sensors
    Matrix<scalar_t, 6, 6> covR;
    covR.setZero();
    covR.block<3,3>(0,0) = aCov;
    covR.block<3,3>(3,3) = mCov;

    const Matrix<scalar_t, 6, 6> S = H * P_ * H.transpose() + covR;
    Matrix<scalar_t, 6, 6> Sinv;

    Eigen::FullPivLU<Matrix<scalar_t,6,6>> LU(S);
    isStable_ = LU.isInvertible();

    if (!isStable_) {
      return;
    }
    Sinv = LU.inverse();

    //  generate update
    const Matrix<scalar_t, 3, 6> K = P_ * H.transpose() * Sinv;
    dx_ = K * r;
    A = K * H;
#else
    dx_.setZero();
    A.setZero();
#endif
  }

  if (ignoreZ_) {
    //  cancel body-frame z update
    dx_[2] = 0;
  }

  //  perform state update
  P_ = (Matrix<scalar_t, 3, 3>::Identity() - A) * P_;

  q_ = q_ * quat(1, dx_[0]/2, dx_[1]/2, dx_[2]/2);
  q_.normalize();
}

void AttitudeESKF::externalYawUpdate(scalar_t yaw, scalar_t alpha) {
  //  check if we are near the hover state
  const Matrix<scalar_t,3,3> wRb = q_.matrix();
  Matrix<scalar_t,3,1> g;
  g[0] = 0;
  g[1] = 0;
  g[2] = 1;

  g = wRb.transpose() * g;
  if (g[2] > 0.85) {
    //  break into roll pitch yaw
    Matrix<scalar_t,3,1> rpy = getRPY(wRb);
    //  interpolate between prediction and estimate
    rpy[2] = rpy[2]*(1-alpha) + yaw*alpha;
    q_ = Eigen::AngleAxis<scalar_t>(rpy[2],vec3(0,0,1)) *
    Eigen::AngleAxis<scalar_t>(rpy[1],vec3(0,1,0)) *
    Eigen::AngleAxis<scalar_t>(rpy[0],vec3(1,0,0));
  }
}

bool AttitudeESKF::initialize(const vec3 &ab,
                              const vec3 &aCov,
                              const vec3 &mb,
                              const vec3 &mCov) {
  if (!useMag_) {
    //  determine attitude angles
    scalar_t ay = ab[1];
    if (ay > kOneG) { ay = kOneG; }
    else if (ay < -kOneG) { ay = -kOneG; }
    const scalar_t& ax = ab[0];
    const scalar_t& az = ab[2];

    const scalar_t phi = std::asin(-ay / kOneG);  //  roll
    const scalar_t theta = std::atan2(ax, az);    //  pitch

    q_ = Eigen::AngleAxis<scalar_t>(theta, vec3(0,1,0)) *
         Eigen::AngleAxis<scalar_t>(phi, vec3(1,0,0));
  }
  else {
    ///  @todo: This is kind of ugly, find some simpler mechanism to do this.

#ifdef ATTITUDE_ESKF_BUILD_MAG
    const static scalar_t eps(1e-6);
    for (int i=0; i < 3; i++) {
      if (aCov[i] < eps || mCov[i] < eps) {
        return false;
      }
    }
    //  jacobian
    Eigen::Matrix <scalar_t,6,3> J;
    J.block<3,3>(0,0) = crossSkew(ab);
    J.block<3,3>(3,0) = crossSkew(mb);

    //  weight matrix
    Eigen::Matrix <scalar_t,6,6> S;
    S.setZero();
    for (int i=0; i < 3; i++) {
      S(i,i) = 1 / aCov[i];
      S(i+3,i+3) = 1 / mCov[i];
    }

    //  hessian
    const mat3 H = J.transpose() * S * J;
    const Eigen::LDLT<mat3> ldlt(H);

    //  optimize
    vec3 w(0,0,0);
    Matrix<scalar_t,6,1> r;
    for (unsigned int iter=0; iter < 5; iter++) {
      const mat3 W = rodrigues(w);
      //  residuals
      r.block<3,1>(0,0) = (W * vec3(0,0,kOneG)) - ab;
      r.block<3,1>(3,0) = (W * magRef_) - mb;
      //  step
      w.noalias() += ldlt.solve(J.transpose() * S * r);
    }
    q_ = quat(rodrigues(w).transpose());
#endif
  }
  //  start w/ a large uncertainty
  P_.setIdentity();
  P_ *= M_PI*M_PI;

  return true;
}

AttitudeESKF::vec3 AttitudeESKF::getRPY(const mat3& R) {
  vec3 rpy;
  scalar_t sth = -R(2, 0);
  if (sth > 1) {
    sth = 1;
  } else if (sth < -1) {
    sth = -1;
  }

  const scalar_t theta = std::asin(sth);
  const scalar_t cth = std::sqrt(1 - sth*sth);

  scalar_t phi, psi;
  if (cth < static_cast<scalar_t>(1.0e-6)) {
    phi = std::atan2(R(0, 1), R(1, 1));
    psi = 0;
  } else {
    phi = std::atan2(R(2, 1), R(2, 2));
    psi = std::atan2(R(1, 0), R(0, 0));
  }

  rpy[0] = phi;    //  x, [-pi,pi]
  rpy[1] = theta;  //  y, [-pi/2,pi/2]
  rpy[2] = psi;    //  z, [-pi,pi]
  return rpy;
}

} //  namespace kr


#include <iostream>
#include <functional>
#include <numeric>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <map>
#include <Eigen\dense>

#include "markov.h"
#include "TransitionMatrix.h"

// Hint: Set N - number of simulations low until you have it working
//       Then set it much much higher, and run in release mode so its faster

int main() {

	SetTransitionMatrix();
	setGameMatrix();
	// Print Results to File
	std::ofstream myfile;
	myfile.open("dtmc_results.txt");

	int start = 0;

	//simulate discrete time Markov Chain
	unsigned int N = 100;
	std::map<int, int> hist;

	std::vector<int> discreteMC;
	//std::vector< std::vector<double> > matrix(3, std::vector<double>(3)); //initializes a 3x3 matrix with zeros
	for (unsigned int i = 0; i < N; ++i) {

		//TODO (add DTMC, and histogram lines.)
		discreteMC = DTMC(TransitionMatrix,ROLLS,start);
		++hist[std::round(discreteMC.back())];
		int counter = 0;
		// Code if you wanted to print out results at each step
		for (auto elem : discreteMC) {
			if (elem < 100) counter++;
			std::cout << elem << std::endl;
			std::cout << counter << std::endl;
			myfile << elem << std::endl;
			myfile << counter << std::endl;

		}
		std::cout << "****New Game*****" << i << std::endl;
		myfile << "******New Game*******" << std::endl;
	}
	//Returns an array discreteMC with the states at each step of the discrete-time Markov Chain
	//The number of transitions is given by steps. The initial state is given by start
	//(the states are indexed from 0 to n-1 where n is the number of arrays in transMatrix).
	//hist is the histogram

	// (double)p.second / N    - (decimal) percentage.
	for (auto p : hist) {
		std::cout << p.first << "\t" << (double)p.second / N << std::endl;
	}

	myfile.close();

	return 1;
}


/***************************************************************************
 *   Copyright (C) 2016 by Саша Миленковић                                 *
 *   sasa.milenkovic.xyz@gmail.com                                         *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *   ( http://www.gnu.org/licenses/gpl-3.0.en.html )                       *
 *                                     *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

#include <Eigen/Dense>
#include <cmath>
#include "quartic.hpp"
const double PI = 3.141592653589793238463L;
const double M_2PI = 2*PI;
const double eps=1e-12;

//---------------------------------------------------------------------------
// solve cubic equation x^3 + a*x^2 + b*x + c
// x - array of size 3
// In case 3 real roots: => x[0], x[1], x[2], return 3
//         2 real roots: x[0], x[1],          return 2
//         1 real root : x[0], x[1] ± i*x[2], return 1
inline unsigned int solveP3(double *x,double a,double b,double c) {

    double a2 = a * a;
    double q = (a2 - 3 * b) / 9;
    double r = (a * ( 2 * a2 - 9 * b) + 27 * c) / 54;
    double r2 = r*r;
    double q3 = q*q*q;
    double A,B;
        if(r2<q3)
        {
            double t=r/sqrt(q3);
            if( t<-1) t=-1;
            if( t> 1) t= 1;
            t=acos(t);
            a/=3; q=-2*sqrt(q);
            x[0]=q*cos(t/3)-a;
            x[1]=q*cos((t+M_2PI)/3)-a;
            x[2]=q*cos((t-M_2PI)/3)-a;
            return 3;
        }
        else
        {
            A =-pow(fabs(r)+sqrt(r2-q3),1./3);
            if( r<0 ) A=-A;
            B = (0==A ? 0 : q/A);

        a/=3;
        x[0] =(A+B)-a;
        x[1] =-0.5*(A+B)-a;
        x[2] = 0.5*sqrt(3.)*(A-B);
        if(fabs(x[2])<eps) { x[2]=x[1]; return 2; }

        return 1;
        }
}

//---------------------------------------------------------------------------
// solve quartic equation x^4 + a*x^3 + b*x^2 + c*x + d
Eigen::Vector4cd solve_quartic(double a, double b, double c, double d)
{
    double a3 = -b;
    double b3 =  a*c -4.*d;
    double c3 = -a*a*d - c*c + 4.*b*d;

    // cubic resolvent
    // y^3 − b*y^2 + (ac−4d)*y − a^2*d−c^2+4*b*d = 0

    double x3[3];
    unsigned int iZeroes = solveP3(x3, a3, b3, c3);

    double q1, q2, p1, p2, D, sqD, y;

    y = x3[0];
    // THE ESSENCE - choosing Y with maximal absolute value !
    if(iZeroes != 1)
    {
        if(fabs(x3[1]) > fabs(y)) y = x3[1];
        if(fabs(x3[2]) > fabs(y)) y = x3[2];
    }

    // h1+h2 = y && h1*h2 = d  <=>  h^2 -y*h + d = 0    (h === q)

    D = y*y - 4*d;
    if(fabs(D) < eps) //in other words - D==0
    {
        q1 = q2 = y * 0.5;
        // g1+g2 = a && g1+g2 = b-y   <=>   g^2 - a*g + b-y = 0    (p === g)
        D = a*a - 4*(b-y);
        if(fabs(D) < eps) //in other words - D==0
            p1 = p2 = a * 0.5;

        else
        {
            sqD = sqrt(D);
            p1 = (a + sqD) * 0.5;
            p2 = (a - sqD) * 0.5;
        }
    }
    else
    {
        sqD = sqrt(D);
        q1 = (y + sqD) * 0.5;
        q2 = (y - sqD) * 0.5;
        // g1+g2 = a && g1*h2 + g2*h1 = c       ( && g === p )  Krammer
        p1 = (a*q1-c)/(q1-q2);
        p2 = (c-a*q2)/(q1-q2);
    }

    Eigen::Vector4cd retval;
    std::complex<double> tmp;
    // solving quadratic eq. - x^2 + p1*x + q1 = 0
    D = p1*p1 - 4*q1;
    if(D < 0.0)
    {
        tmp.real( -p1 * 0.5 );
        tmp.imag( sqrt(-D) * 0.5 );
        retval[0] = tmp;
        tmp = std::conj(tmp);
        retval[1] = tmp;
    }
    else
    {
        tmp.imag(0);
        sqD = sqrt(D);
        tmp.real( (-p1 + sqD) * 0.5 );
        retval[0] = tmp;
        tmp.real( (-p1 - sqD) * 0.5 );
        retval[1] = tmp;
    }

    // solving quadratic eq. - x^2 + p2*x + q2 = 0
    D = p2*p2 - 4*q2;
    if(D < 0.0)
    {
        tmp.real( -p2 * 0.5 );
        tmp.imag( sqrt(-D) * 0.5 );
        retval[2] = tmp;
        tmp = std::conj(tmp);
        retval[3] = tmp;
    }
    else
    {
        tmp.imag(0);
        sqD = sqrt(D);
        tmp.real( (-p2 + sqD) * 0.5 );
        retval[2] = tmp;
        tmp.real( (-p2 - sqD) * 0.5 );
        retval[3] = tmp;
    }

    return retval;
}


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

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [remquo]
#include <boost/simd/arithmetic.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <iostream>

namespace bs =  boost::simd;
using pack_ft =  bs::pack <float, 4>;
using pack_it =  bs::pack <std::int32_t, 4>;

int main()
{
  pack_ft xf = { 3.0f, -2.0f, -3.0f, 1.0f };
  pack_ft yf = { 4.0f, -1.0f, -3.0f, 2.0f };
  pack_ft rf;
  pack_it qi;
  std::tie(rf, qi) = bs::remquo(xf, yf);

  std::cout
    <<  "---- simd:  std::tie(xf, yf) = bs::remquo(xf, yf)\n"
    << " <- xf = " << xf << '\n'
    << " <- yf = " << yf << '\n'
    << " -> rf = " << rf << '\n'
    << " -> qi = " << qi << '\n';

  float sxf = 3.0f, syf = 4.0f;
  float srf;
  std::int32_t sqi;
  std::tie(srf, sqi) = bs::remquo(sxf, syf);

  std::cout
    << "---- scalar: std::tie(srf, sqi) = bs::remquo(sxf, syf)\n"
    << " <- sxf =  " << sxf << '\n'
    << " <- syf =  " << syf << '\n'
    << " -> srf = " << srf << '\n'
    << " -> sqi = " << sqi << '\n';
  return 0;
}
//! [remquo]


/* CHOMP class implementation
 *
 * Copyright (C) 2016 Rafael Valencia. All rights reserved.
 * License (3-Cluase BSD): https://github.com/rafaelvalencia
 *
 * This code uses and is based on code from:
 *   Project: trychomp https://github.com/poftwaresatent/trychomp
 *   Copyright (C) 2014 Roland Philippsen. All rights reserved.
 *   License (3-Clause BSD) : https://github.com/poftwaresatent/trychomp
 * **
 * \file chomp.cpp
 *
 * CHOMP for point vehicles (x,y) moving holonomously in the plane. It will
 * plan a trajectory (xi) connecting start point (qs_) to end point (qe) while
 * avoiding obstacles (obs)
 */
#include "path_adaptor.hpp"
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <stdlib.h>
#include <sys/time.h>
#include <err.h>

typedef Eigen::VectorXd Vector;
typedef Eigen::MatrixXd Matrix;
typedef Eigen::Isometry3d Transform;
static size_t const obs_dim(3); 	// obstacle dimensions (x,y,Radius)

using namespace std;
#define PI (3.141592653589793)

////////////////////////////////////////////////////////////////////////////
// Auxiliar functions

void pi2pi(double &angle)
{
	double ang;
    /* Process angle*/
    if( (angle < -2*PI) ||  (angle > 2* PI) )
		ang = fmod(angle, 2*PI);
    else
        ang = angle;
    if (ang > PI) ang = ang - 2*PI;
    if (ang < -PI) ang = ang + 2*PI;
    angle = ang;
}

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

CHOMP_SE2::CHOMP_SE2(double dt_input, double eta_input, double lambda_input, size_t nq_input, size_t numIt_input, double gain) :
CHOMP(dt_input, eta_input, lambda_input, nq_input, 3, numIt_input, gain)
{
	cout << "-----------------------------------------------------:"<< endl;
	cout << "CHOMP SE2 started "<< endl;
	cout << "-----------------------------------------------------:"<< endl;
}

void CHOMP_SE2::boundTrajectoryAngles (void)
{
	for (size_t iq (0); iq < nq_; ++iq)
	{
		//Makes the angles to be between Pi to -Pi
		Vector q (xi_.block  (iq * cdim_, 0, cdim_, 1) );
		pi2pi ( q(2) );
		xi_.block  (iq * cdim_, 0, cdim_, 1) = q;
	}
}

void CHOMP_SE2::boundVectorAngles (Vector& VV)
{
    static size_t const VVsize = 	(VV.size()/cdim_) - 1;
	for (size_t iq (0); iq < VVsize; ++iq)
	{
		//Makes the angles to be between Pi to -Pi
		Vector q (VV.block  (iq * cdim_, 0, cdim_, 1) );
		pi2pi ( q(2));
		VV.block  (iq * cdim_, 0, cdim_, 1) = q;
	}
}

double CHOMP_SE2::chompIteration(Vector  &xi)
{

	// Before performing the iteration check if a path has been given
	if (PATH_INIT_==false)
	{
		cout << "A path was not initialized. Leaving CHOMP iteration! " << endl;
		return NAN;
	}

	//////////////////////////////////////////////////
	// beginning of "the" constrained CHOMP iteration

	Vector nabla_smooth (AA_ * xi_ + bb_);
	Vector const & xidd (nabla_smooth); // indeed, it is the same in this formulation...

	// Constrained CHOMP.
	// Impose nonholonmic (NH) restrictions with the rolling constraint.
	// Next we evaluate the constraint functional
	// and its Jacobian b and C, respectively,
	// as it appears in CHOMP's IJRR paper.
	//
	Matrix CC ( Matrix::Zero (xidim_, 1) );
	double b = 0;
	Vector c1 (Vector::Zero (3));
	Vector c2 (Vector::Zero (3));
	Vector cf1 (Vector::Zero (3));
	Vector cf2 (Vector::Zero (3));

	for (size_t iq (0); iq < (nq_-1); ++iq)
	{

		//Evaluate the constraint functional. It is defined by a sum of auxiliar functions
		//that depend on only two consecutive robot poses.
		Vector const q1 (xi_.block  (iq * cdim_, 0, cdim_, 1) );
		Vector const q2 (xi_.block  ((iq+1) * cdim_, 0, cdim_, 1) );

		double nhc =  ( q2(0)  -  q1(0) )* sin(q1(2)) - ( q2(1)  -  q1(1) )* cos(q1(2)); //nonholonomic constraint
		double fmc =   cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1)) + sqrt(pow(cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1)),2)); //forward motion constraint

		b +=  nhc * nhc + fmc; //square of the rolling constraint + forward motion constrain

		//Computation of the Jacobian of the NH constraint.
		//  Jacobian of the auxiliar functions
		c1(0) = -2*sin(q1(2))*(cos(q1(2))*( q1(1)  -  q2(1) ) - sin(q1(2))*( q1(0)  -  q2(0) ));
		c1(1) =  2*cos(q1(2))*(cos(q1(2))*( q1(1)  -  q2(1) ) - sin(q1(2))*( q1(0)  -  q2(0) ));
		c1(2) =  -2*(cos(q1(2))*( q1(0)  -  q2(0) ) + sin(q1(2))*( q1(1)  -  q2(1) ))*(cos(q1(2))*( q1(1)  -  q2(1) ) - sin(q1(2))*( q1(0)  -  q2(0) ));
		c2(0) =  2*sin(q1(2))*(cos(q1(2))*( q1(1)  -  q2(1) ) - sin(q1(2))*( q1(0)  -  q2(0) ));
		c2(1) = -2*cos(q1(2))*(cos(q1(2))*( q1(1)  -  q2(1) ) - sin(q1(2))*( q1(0)  -  q2(0) ));
		c2(2) =  0;
		//end of NHC Jacobian

		//Computation of the Jacobian of the NH constraint.
		//  Jacobian of the auxiliar functions
		cf1(0)=     cos(q1(2)) + (cos(q1(2))*(cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1))))/sqrt(pow(cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1)),2));
		cf1(1)=     sin(q1(2)) + (sin(q1(2))*(cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1))))/sqrt(pow(cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1)),2));
		cf1(2)=     cos(q1(2))*(q1(1) - q2(1)) - sin(q1(2))*(q1(0) - q2(0)) + ((cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1)))*(cos(q1(2))*(q1(1) - q2(1)) - sin(q1(2))*(q1(0) - q2(0))))/sqrt(pow(cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1)),2));
		cf2(0)=     -cos(q1(2)) - (cos(q1(2))*(cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1))))/sqrt(pow(cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1)),2));
		cf2(1) =    - sin(q1(2)) - (sin(q1(2))*(cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1))))/sqrt(pow(cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1)),2));
		cf2(2)=     0;
		//end of FMC Jacobian

		if (iq == 0 )
		{
			  c2(0) +=  2*sin(qs_(2))*(cos(qs_(2))*( qs_(1)  -  q1(1) ) - sin(qs_(2))*( qs_(0)  -  q1(0) ));
			  c2(1) += -2*cos(qs_(2))*(cos(qs_(2))*( qs_(1)  -  q1(1) ) - sin(qs_(2))*( qs_(0)  -  q1(0) ));
			  c2(2) +=  0;

			  cf2(0) +=     -cos(qs_(2)) - (cos(qs_(2))*(cos(qs_(2))*(qs_(0) - q1(0)) + sin(qs_(2))*(qs_(1) - q1(1))))/sqrt(pow(cos(qs_(2))*(qs_(0) - q1(0)) + sin(qs_(2))*(qs_(1) - q1(1)),2));
			  cf2(1) +=    - sin(qs_(2)) - (sin(qs_(2))*(cos(qs_(2))*(qs_(0) - q1(0)) + sin(qs_(2))*(qs_(1) - q1(1))))/sqrt(pow(cos(qs_(2))*(qs_(0) - q1(0)) + sin(qs_(2))*(qs_(1) - q1(1)),2));
			  cf2(2) +=     0;

			  double nhstart  =  ( q1(0)  -  qs_(0) )* sin(qs_(2)) - ( q1(1)  -  qs_(0) )* cos(qs_(2));
			  double fmstart =   cos(qs_(2))*(qs_(0) - q1(0)) + sin(qs_(2))*(qs_(1) - q1(1)) + sqrt(pow(cos(qs_(2))*(qs_(0) - q1(0)) + sin(qs_(2))*(qs_(1) - q1(1)),2)); //forward motion constraint
			  b +=  nhstart * nhstart + fmstart; //square of the rolling constraint
		}

		//Update Jacobian with the contributions from the Jacobian of the auxiliar functions
		CC.block  (iq * cdim_, 0, cdim_, 1) = CC.block  (iq * cdim_, 0, cdim_, 1) + c1 + cf1;
		CC.block  ((iq+1) * cdim_, 0, cdim_, 1) = CC.block  ((iq+1) * cdim_, 0, cdim_, 1) + c2 + cf2;
	}

	CC /=  dt_ * dt_ * (nq_ + 1) ;
	b /=  dt_ * dt_ * (nq_ + 1);

	Matrix CCtrans = CC.transpose();
    // end of the computation of C and b

	Vector nabla_obs (Vector::Zero (xidim_));

	for (size_t iq (0); iq < nq_; ++iq)
	{
		Vector const qq (xi_.block (iq * cdim_, 0, cdim_, 1));
		Vector qd;
		if (0 == iq)
		{
			qd = 0.5 * (xi_.block ((iq+1) * cdim_, 0, cdim_, 1) - qs_);
		}
		else if (iq == nq_ - 1)
		{
			qd = 0.5 * (qe_ - xi_.block ((iq-1) * cdim_, 0, cdim_, 1));
		}
		else
		{
			qd = 0.5 * (xi_.block ((iq+1) * cdim_, 0, cdim_, 1) - xi_.block ((iq-1) * cdim_, 0, cdim_, 1));;
		}

		Vector const & xx (qq.block (0,0,2,1));
		Vector const & xd (qd.block (0,0,2,1));

		// In this case, C and W are NOT the same
		Matrix JJ(Matrix::Zero (2, 3));
		JJ(0,0)=1;
		JJ(1,1)=1;

		double const vel (xd.norm());
		if (vel < 1.0e-3)
		{
			// avoid div by zero further down
			continue;
		}
		Vector const xdn (xd / vel);

		Vector const xdd (JJ * xidd.block (iq * cdim_, 0, cdim_ , 1));

		Matrix const prj (Matrix::Identity (2, 2) - xdn * xdn.transpose()); // hardcoded planar case
		Vector const kappa (prj * xdd / pow (vel, 2.0));

		//Add obstacles
		for (int ii = 0; ii < OBS_.cols(); ii++)
		{
			Vector delta(xx - OBS_.block(0, ii, 2, 1));
			double const dist(delta.norm());
			if ((dist >= OBS_(2, ii)) || (dist < 1e-9))
				continue;
			double const cost(costGain_ * OBS_(2, ii) * pow(1.0 - dist / OBS_(2, ii), 3.0) / 3.0);
			delta *= - costGain_ *pow(1.0 - dist / OBS_(2, ii), 2.0) / dist;
			nabla_obs.block(iq * cdim_, 0, cdim_, 1) += JJ.transpose() * vel * (prj * delta - cost * kappa);
		}

	}
    double residual;

	Vector dxi (Ainv_ * (nabla_obs + lambda_ * nabla_smooth)); //unconstrained step

	if (b < 1.0e-10)
	{
		//unconstrained update to initialize trajectory (it starts with b aprox to zero)
		//cout << " One unconstrained update to initialize trajectory  " << endl;
		xi_ -= dxi / eta_;

		Vector dxi (Ainv_ * (nabla_obs + lambda_ * nabla_smooth));
		residual =  dxi.norm() / eta_;
	}
	else
	{
		// Constrained optimization update
		Vector CAC (CCtrans * Ainv_ * CC);

		Matrix CACinv (CAC.inverse());

		Vector Proj (Ainv_ * CC * CACinv); //auxiliar matrix for the update equation

		//cout << "b =" << b << "\n";
		Vector cdxi ( - dxi/eta_  +  Proj*CCtrans*dxi / eta_ -  Proj * b  );

		xi_ += cdxi; //constrained update

		residual = cdxi.norm() / eta_;
	}
	// end of "the" constrainedCHOMP iteration
	//////////////////////////////////////////////////
	boundTrajectoryAngles();

	res_ = residual;
	xi = xi_; //updated path

	return res_;

}


#include "Cady/Cady.h"
#include "Cady/Frontend.h"
#include "Cady/CodeGen.h"

#include <map>
#include <iomanip>

using namespace Cady;
using namespace Cady::CodeGen;

void example_0(){

        Function f("f");
        f.AddArgument("x");
        f.AddArgument("y");

        //auto expr_0 = BinaryOperator::Mul(Log::Make(BinaryOperator::Mul(ExogenousSymbol::Make("x"),ExogenousSymbol::Make("x"))),  Exp::Make(ExogenousSymbol::Make("y")));
        //auto expr_0 = BinaryOperator::Pow(ExogenousSymbol::Make("x"), Constant::Make(2));
        auto expr_0 = Phi::Make(BinaryOperator::Pow(ExogenousSymbol::Make("x"), Constant::Make(3)));

        auto stmt_0 = std::make_shared<EndgenousSymbol>("stmt0", expr_0);

        f.AddStatement(stmt_0);

        std::ofstream fstr("prog.cxx");
        fstr << R"(
#include <cstdio>
#include <cmath>
)";

        StringCodeGenerator cg;
        cg.Emit(fstr, f);
        fstr << R"(

int main(){
        double x_min = 0.1;
        double x_max = +2.0;
        double y_min = -2.0;
        double y_max = +2.0;

        double epsilon = 1e-10;
        double increment = 0.05;

        for(double x =x_min; x <= x_max + increment /2; x += increment ){
                for(double y =y_min; y <= y_max + increment /2; y += increment ){
                        double d_x = 0.0;
                        double d_y = 0.0;

                        double value = f(x, &d_x, y, &d_y);

                        double dummy;
                        double x_lower = f(x - epsilon /2 , &dummy, y, &dummy);
                        double x_upper = f(x + epsilon /2 , &dummy, y, &dummy);
                        double x_finite_diff = ( x_upper - x_lower ) / epsilon;
                        double x_residue = d_x - x_finite_diff;

                        double y_lower = f(x, &dummy, y - epsilon /2 , &dummy);
                        double y_upper = f(x, &dummy, y + epsilon /2 , &dummy);
                        double y_finite_diff = ( y_upper - y_lower ) / epsilon;
                        double y_residue = d_y - y_finite_diff;

                        //printf("%f,%f,%f,%f,%f,%f\n", x, y, d_x, d_y, x_finite_diff, x_residue);
                        printf("%f,%f,%f => %f,%f,%f => %f,%f,%f\n", x, y,value, d_x, x_finite_diff,x_residue, d_y, y_finite_diff,y_residue);
                }

        }

}
)";
}

void example_1(){

        Function f("f");
        f.AddArgument("a");
        f.AddArgument("b");
        f.AddArgument("x");

        auto expr_0 = BinaryOperator::Mul( ExogenousSymbol::Make("a"), BinaryOperator::Mul(ExogenousSymbol::Make("x"),  ExogenousSymbol::Make("x")));

        auto stmt_0 = std::make_shared<EndgenousSymbol>("stmt0", expr_0);

        auto expr_1 = BinaryOperator::Add( ExogenousSymbol::Make(stmt_0->Name()), ExogenousSymbol::Make("b"));

        auto stmt_1 = std::make_shared<EndgenousSymbol>("stmt1", expr_1);
        f.AddStatement(stmt_0);
        f.AddStatement(stmt_1);

        std::ofstream fstr("prog.c");
        StringCodeGenerator cg;
        cg.Emit(fstr, f);
        fstr << R"(
#include <stdio.h>
int main(){
        double a = 2.0;
        double b = 3.0;

        double epsilon = 1e-10;
        double increment = 0.05;

        for(double x =0.0; x <= 2.0 + increment /2; x += increment ){
                double d_a = 0.0;
                double d_b = 0.0;
                double d_x = 0.0;

                double y = f(a, &d_a, b, &d_b, x, &d_x);

                double dummy;
                double lower = f(a, &dummy, b, &dummy, x - epsilon/2, &dummy);
                double upper = f(a, &dummy, b, &dummy, x + epsilon/2, &dummy);
                double finite_diff = ( upper - lower ) / epsilon;
                double residue = d_x - finite_diff;

                printf("%f,%f,%f,%f,%f,%f,%f\n", x, y, d_a, d_b, d_x, finite_diff, residue);

        }

}
)";
}

void black_scholes(){

        Function f("black");
        f.AddArgument("t");
        f.AddArgument("T");
        f.AddArgument("r");
        f.AddArgument("S");
        f.AddArgument("K");
        f.AddArgument("vol");

        auto time_to_expiry = BinaryOperator::Sub(
                ExogenousSymbol::Make("T"),
                ExogenousSymbol::Make("t")
        );

        auto deno = BinaryOperator::Div(
                Constant::Make(1.0),
                BinaryOperator::Mul(
                        ExogenousSymbol::Make("vol"),
                        BinaryOperator::Pow(
                                time_to_expiry,
                                Constant::Make(0.5)
                        )
                )
        );

        auto d1 = BinaryOperator::Mul(
                deno,
                BinaryOperator::Add(
                        Log::Make(
                                BinaryOperator::Div(
                                        ExogenousSymbol::Make("S"),
                                        ExogenousSymbol::Make("K")
                                )
                        ),
                        BinaryOperator::Mul(
                                BinaryOperator::Add(
                                        ExogenousSymbol::Make("r"),
                                        BinaryOperator::Div(
                                                BinaryOperator::Pow(
                                                        ExogenousSymbol::Make("vol"),
                                                        Constant::Make(2.0)
                                                ),
                                                Constant::Make(2.0)
                                        )
                                ),
                                time_to_expiry
                        )
                )
        );

        auto stmt_0 = std::make_shared<EndgenousSymbol>("stmt0", d1);

        auto d2 = BinaryOperator::Sub(
                ExogenousSymbol::Make(stmt_0->Name()),
                BinaryOperator::Mul(
                        ExogenousSymbol::Make("vol"),
                        time_to_expiry
                )
        );

        auto stmt_1 = std::make_shared<EndgenousSymbol>("stmt1", d2);

        auto pv = BinaryOperator::Mul(
                ExogenousSymbol::Make("K"),
                Exp::Make(
                        BinaryOperator::Mul(
                                BinaryOperator::Sub(
                                        Constant::Make(0.0),
                                        ExogenousSymbol::Make("r")
                                ),
                                time_to_expiry
                        )
                )
        );

        auto stmt_2 = std::make_shared<EndgenousSymbol>("stmt2", pv);

        auto black = BinaryOperator::Sub(
                BinaryOperator::Mul(
                        Phi::Make(stmt_0),
                        ExogenousSymbol::Make("S")
                ),
                BinaryOperator::Mul(
                        Phi::Make(stmt_1),
                        stmt_2
                )
        );

        auto stmt_3 = std::make_shared<EndgenousSymbol>("stmt3", black);

        f.AddStatement(stmt_0);
        f.AddStatement(stmt_1);
        f.AddStatement(stmt_2);
        f.AddStatement(stmt_3);

        std::ofstream fstr("prog.cxx");
        fstr << R"(
#include <cstdio>
#include <cmath>
)";

        StringCodeGenerator cg;
        cg.Emit(fstr, f);
        fstr << R"(

double black_fd(double epsilon, double t, double d_t, double T, double d_T, double r, double d_r, double S, double d_S, double K, double d_K, double vol, double d_vol){
        double dummy;
        double lower = black( t - d_t*epsilon/2 , &dummy, T - d_T*epsilon/2  , &dummy, r - d_r*epsilon/2  , &dummy, S - d_S*epsilon/2  , &dummy, K - d_K*epsilon/2  , &dummy, vol - d_vol*epsilon/2, &dummy);
        double upper = black( t + d_t*epsilon/2 , &dummy, T + d_T*epsilon/2  , &dummy, r + d_r*epsilon/2  , &dummy, S + d_S*epsilon/2  , &dummy, K + d_K*epsilon/2  , &dummy, vol + d_vol*epsilon/2, &dummy);
        double finite_diff = ( upper - lower ) / epsilon;
        return finite_diff;
}
int main(){
        double t   = 0.0;
        double T   = 10.0;
        double r   = 0.04;
        double S   = 50;
        double K   = 60;
        double vol = 0.2;

        double epsilon = 1e-10;

        double d_t = 0.0;
        double d_T = 0.0;
        double d_r = 0.0;
        double d_S = 0.0;
        double d_K = 0.0;
        double d_vol = 0.0;
        double value = black( t  , &d_t, T  , &d_T, r  , &d_r, S  , &d_S, K  , &d_K, vol, &d_vol);

        double d1 = 1/ ( vol * std::sqrt(T - t)) *  ( std::log(S/K) + ( r + vol*vol/2)*(T-t));

        double dummy;
        double lower = black( t - epsilon/2 , &dummy, T  , &dummy, r  , &dummy, S  , &dummy, K  , &dummy, vol, &dummy);
        double upper = black( t + epsilon/2 , &dummy, T  , &dummy, r  , &dummy, S  , &dummy, K  , &dummy, vol, &dummy);
        double finite_diff = ( upper - lower ) / epsilon;
        double residue = d_t - finite_diff;

        printf("%f,%f,%f,%f,%f,%f => %f,%f => %f,%f,%f\n", t, T, r, S, K, vol, value, d1, d_t, finite_diff, residue);

        printf("d[t]  ,%f,%f\n", d_t  ,  black_fd(epsilon, t, 1, T  , 0, r  , 0, S  , 0, K  , 0, vol, 0));
        printf("d[T]  ,%f,%f\n", d_T  ,  black_fd(epsilon, t, 0, T  , 1, r  , 0, S  , 0, K  , 0, vol, 0));
        printf("d[r]  ,%f,%f\n", d_r  ,  black_fd(epsilon, t, 0, T  , 0, r  , 1, S  , 0, K  , 0, vol, 0));
        printf("d[S]  ,%f,%f\n", d_S  ,  black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 1, K  , 0, vol, 0));
        printf("d[K]  ,%f,%f\n", d_K  ,  black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 0, K  , 1, vol, 0));
        printf("d[vol],%f,%f\n", d_vol,  black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 0, K  , 0, vol, 1));

}
)";
}

void black_scholes_frontend(){

        using namespace Frontend;
        using Frontend::Log;
        using Frontend::Exp;
        using Frontend::Phi;

        Function f("black");
        f.AddArgument("t");
        f.AddArgument("T");
        f.AddArgument("r");
        f.AddArgument("S");
        f.AddArgument("K");
        f.AddArgument("vol");

        auto d1    = f.AddStatement(Stmt("d1"   , (1.0 / ( Var("vol") * ((Var("T") - Var("t")) ^ 0.5) )) * ( Log(Var("S") / "K") +   ("r" + ( Var("vol") ^ 2.0 ) / 2 ) * (Var("T") - Var("t")) )));
        auto d2    = f.AddStatement(Stmt("d2"   , d1 - "vol" * (Var("T") - Var("t"))));
        auto pv    = f.AddStatement(Stmt("pv"   , "K" * Exp( -Var("r") * ( Var("T") - Var("t") ) )));
        auto black = f.AddStatement(Stmt("black", Phi(d1) * "S" - Phi(d2) * pv));

        std::ofstream fstr("prog.cxx");
        fstr << R"(
#include <cstdio>
#include <cmath>
)";

        StringCodeGenerator cg;
        cg.Emit(fstr, f);
        fstr << R"(

double black_fd(double epsilon, double t, double d_t, double T, double d_T, double r, double d_r, double S, double d_S, double K, double d_K, double vol, double d_vol){
        double dummy;
        double lower = black( t - d_t*epsilon/2 , &dummy, T - d_T*epsilon/2  , &dummy, r - d_r*epsilon/2  , &dummy, S - d_S*epsilon/2  , &dummy, K - d_K*epsilon/2  , &dummy, vol - d_vol*epsilon/2, &dummy);
        double upper = black( t + d_t*epsilon/2 , &dummy, T + d_T*epsilon/2  , &dummy, r + d_r*epsilon/2  , &dummy, S + d_S*epsilon/2  , &dummy, K + d_K*epsilon/2  , &dummy, vol + d_vol*epsilon/2, &dummy);
        double finite_diff = ( upper - lower ) / epsilon;
        return finite_diff;
}
int main(){
        double t   = 0.0;
        double T   = 10.0;
        double r   = 0.04;
        double S   = 50;
        double K   = 60;
        double vol = 0.2;

        double epsilon = 1e-10;

        double d_t = 0.0;
        double d_T = 0.0;
        double d_r = 0.0;
        double d_S = 0.0;
        double d_K = 0.0;
        double d_vol = 0.0;
        double value = black( t  , &d_t, T  , &d_T, r  , &d_r, S  , &d_S, K  , &d_K, vol, &d_vol);

        double d1 = 1/ ( vol * std::sqrt(T - t)) *  ( std::log(S/K) + ( r + vol*vol/2)*(T-t));

        double dummy;
        double lower = black( t - epsilon/2 , &dummy, T  , &dummy, r  , &dummy, S  , &dummy, K  , &dummy, vol, &dummy);
        double upper = black( t + epsilon/2 , &dummy, T  , &dummy, r  , &dummy, S  , &dummy, K  , &dummy, vol, &dummy);
        double finite_diff = ( upper - lower ) / epsilon;
        double residue = d_t - finite_diff;

        printf("%f,%f,%f,%f,%f,%f => %f,%f => %f,%f,%f\n", t, T, r, S, K, vol, value, d1, d_t, finite_diff, residue);

        printf("d[t]  ,%f,%f\n", d_t  ,  black_fd(epsilon, t, 1, T  , 0, r  , 0, S  , 0, K  , 0, vol, 0));
        printf("d[T]  ,%f,%f\n", d_T  ,  black_fd(epsilon, t, 0, T  , 1, r  , 0, S  , 0, K  , 0, vol, 0));
        printf("d[r]  ,%f,%f\n", d_r  ,  black_fd(epsilon, t, 0, T  , 0, r  , 1, S  , 0, K  , 0, vol, 0));
        printf("d[S]  ,%f,%f\n", d_S  ,  black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 1, K  , 0, vol, 0));
        printf("d[K]  ,%f,%f\n", d_K  ,  black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 0, K  , 1, vol, 0));
        printf("d[vol],%f,%f\n", d_vol,  black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 0, K  , 0, vol, 1));

}
)";
}

void black_scholes_template(){
        auto black_eval = BlackScholesCallOption::Build<double>{};

        double t   = 0.0;
        double T   = 10.0;
        double r   = 0.04;
        double S   = 50;
        double K   = 60;
        double vol = 0.2;

        std::cout << "black_eval(t,T,r,S,K,vol) => " << black_eval.Evaluate(t,T,r,S,K,vol) << "\n"; // __CandyPrint__(cxx-print-scalar,black_eval(t,T,r,S,K,vol))

        auto ad_kernel = BlackScholesCallOption::Build<DoubleKernel>{};

        auto as_black = ad_kernel.Evaluate(
                DoubleKernel::BuildFromExo("t"),
                DoubleKernel::BuildFromExo("T"),
                DoubleKernel::BuildFromExo("r"),
                DoubleKernel::BuildFromExo("S"),
                DoubleKernel::BuildFromExo("K"),
                DoubleKernel::BuildFromExo("vol")
        );

        Function f("black");
        f.AddArgument("t");
        f.AddArgument("T");
        f.AddArgument("r");
        f.AddArgument("S");
        f.AddArgument("K");
        f.AddArgument("vol");

        using namespace Frontend;

        std::unordered_set< std::shared_ptr<Operator > > seen;
        struct StackFrame{
                explicit StackFrame(std::shared_ptr<EndgenousSymbol > op)
                        : Op{op}
                {
                        auto deps_set = Op->EndgenousDependencies();
                        Deps.assign(deps_set.begin(), deps_set.end());
                }
                std::shared_ptr<EndgenousSymbol > Op;
                std::vector<std::shared_ptr<EndgenousSymbol > > Deps;
        };
        std::vector<StackFrame> stack{StackFrame{std::reinterpret_pointer_cast<EndgenousSymbol>(as_black.as_operator_())}};
        for(size_t ttl=1000;stack.size() && ttl;--ttl){
                auto& frame = stack.back();
                if( frame.Deps.size() == 0 ){
                        if( seen.count(frame.Op) == 0 ){
                                seen.insert(frame.Op);
                                auto black = f.AddStatement(frame.Op);
                                #if 0
                                std::cout << "----------TERMINAL--------------\n";
                                frame.Op->Display();
                                #endif
                        }
                        stack.pop_back();
                        continue;
                }
                auto dep = frame.Deps.back();
                frame.Deps.pop_back();

                stack.push_back(StackFrame{dep});

        }

        std::ofstream fstr("prog.cxx");
        fstr << R"(
#include <cstdio>
#include <cmath>
#include <iostream>
#include <boost/timer/timer.hpp>
)";

        StringCodeGenerator cg;
        cg.Emit(fstr, f);
        fstr << R"(

double black_fd(double epsilon, double t, double d_t, double T, double d_T, double r, double d_r, double S, double d_S, double K, double d_K, double vol, double d_vol){
        double dummy;
        double lower = black( t - d_t*epsilon/2 , &dummy, T - d_T*epsilon/2  , &dummy, r - d_r*epsilon/2  , &dummy, S - d_S*epsilon/2  , &dummy, K - d_K*epsilon/2  , &dummy, vol - d_vol*epsilon/2, &dummy);
        double upper = black( t + d_t*epsilon/2 , &dummy, T + d_T*epsilon/2  , &dummy, r + d_r*epsilon/2  , &dummy, S + d_S*epsilon/2  , &dummy, K + d_K*epsilon/2  , &dummy, vol + d_vol*epsilon/2, &dummy);
        double finite_diff = ( upper - lower ) / epsilon;
        return finite_diff;
}
int main(){
        double t   = 0.0;
        double T   = 10.0;
        double r   = 0.04;
        double S   = 50;
        double K   = 60;
        double vol = 0.2;

        double epsilon = 1e-10;

        double d_t = 0.0;
        double d_T = 0.0;
        double d_r = 0.0;
        double d_S = 0.0;
        double d_K = 0.0;
        double d_vol = 0.0;
        double value = black( t  , &d_t, T  , &d_T, r  , &d_r, S  , &d_S, K  , &d_K, vol, &d_vol);

        double d1 = 1/ ( vol * std::sqrt(T - t)) *  ( std::log(S/K) + ( r + vol*vol/2)*(T-t));

        double dummy;
        double lower = black( t - epsilon/2 , &dummy, T  , &dummy, r  , &dummy, S  , &dummy, K  , &dummy, vol, &dummy);
        double upper = black( t + epsilon/2 , &dummy, T  , &dummy, r  , &dummy, S  , &dummy, K  , &dummy, vol, &dummy);
        double finite_diff = ( upper - lower ) / epsilon;
        double residue = d_t - finite_diff;

        printf("%f,%f,%f,%f,%f,%f => %f,%f => %f,%f,%f\n", t, T, r, S, K, vol, value, d1, d_t, finite_diff, residue);

        printf("d[t]  ,%f,%f\n", d_t  ,  black_fd(epsilon, t, 1, T  , 0, r  , 0, S  , 0, K  , 0, vol, 0));
        printf("d[T]  ,%f,%f\n", d_T  ,  black_fd(epsilon, t, 0, T  , 1, r  , 0, S  , 0, K  , 0, vol, 0));
        printf("d[r]  ,%f,%f\n", d_r  ,  black_fd(epsilon, t, 0, T  , 0, r  , 1, S  , 0, K  , 0, vol, 0));
        printf("d[S]  ,%f,%f\n", d_S  ,  black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 1, K  , 0, vol, 0));
        printf("d[K]  ,%f,%f\n", d_K  ,  black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 0, K  , 1, vol, 0));
        printf("d[vol],%f,%f\n", d_vol,  black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 0, K  , 0, vol, 1));

        // time profile
        for(volatile size_t N = 100;;N*=2){
                boost::timer::cpu_timer timer;
                for(volatile size_t idx=0;idx!=N;++idx){
                        double value = black( t  , &d_t, T  , &d_T, r  , &d_r, S  , &d_S, K  , &d_K, vol, &d_vol);
                }
                std::string ad_time = timer.format(4, "%w");
                timer.start();
                for(volatile size_t idx=0;idx!=N;++idx){
                        black_fd(epsilon, t, 1, T  , 0, r  , 0, S  , 0, K  , 0, vol, 0);
                        black_fd(epsilon, t, 0, T  , 1, r  , 0, S  , 0, K  , 0, vol, 0);
                        black_fd(epsilon, t, 0, T  , 0, r  , 1, S  , 0, K  , 0, vol, 0);
                        black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 1, K  , 0, vol, 0);
                        black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 0, K  , 1, vol, 0);
                        black_fd(epsilon, t, 0, T  , 0, r  , 0, S  , 0, K  , 0, vol, 1);
                }
                std::string fd_time = timer.format(4, "%w");
                std::cout << N << "," << fd_time << "," << ad_time << "\n";
        }

}
)";
}

struct RemoveEndgenousFolder{
        std::shared_ptr<Operator> Fold(std::shared_ptr<Operator> root){
                if( root->Kind() == OPKind_EndgenousSymbol ){
                        auto as_endgenous = std::reinterpret_pointer_cast<EndgenousSymbol>(root);
                        return this->Fold(as_endgenous->Expr());
                }

                if( root->IsNonTerminal() ){
                        for(size_t idx=0;idx!=root->Arity();++idx){
                                auto folded = this->Fold(root->At(idx));
                                root->Rebind(idx, folded);
                        }
                }

                return root;
        }
};

struct RemoveEndo : OperatorTransform{
        virtual std::shared_ptr<Operator> Apply(std::shared_ptr<Operator> const& ptr){
                auto candidate = ptr->Clone(shared_from_this());
                if( candidate->Kind() == OPKind_EndgenousSymbol ){
                        if( auto typed = std::dynamic_pointer_cast<EndgenousSymbol>(candidate)){
                                return typed->Expr();
                        }
                }
                return candidate;
        }
};
struct RemapUnique : OperatorTransform{
        explicit RemapUnique(std::string const& prefix = "__symbol_")
                : prefix_{prefix}
        {
                std::cerr << " RemapUnique()\n";
        }
        ~RemapUnique(){
                std::cerr << "~RemapUnique()\n";
        }
        void mutate_prefix(std::string const& prefix){
                prefix_ = prefix;
        }
        virtual std::shared_ptr<Operator> Apply(std::shared_ptr<Operator> const& ptr){

                auto candidate = ptr->Clone(shared_from_this());

                auto key = std::make_tuple(
                        candidate->NameInvariantOfChildren(),
                        candidate->Children()
                        );

                auto iter = ops_.find(key);
                if( iter != ops_.end() )
                        return iter->second;

                if(
                    candidate->Kind() != OPKind_EndgenousSymbol &&
                    #if 0
                    candidate->Kind() != OPKind_ExogenousSymbol &&
                    #endif
                    candidate->Kind() != OPKind_Constant )
                {

                        std::stringstream ss;
                        ss << prefix_ << (ops_.size()+1);
                        auto endogous_sym = EndgenousSymbol::Make(ss.str(), candidate);

                        ops_[key] = endogous_sym;
                        return endogous_sym;
                } else {
                        ops_[key] = candidate;
                        return candidate;
                }
        }
private:
        std::string prefix_;
        std::map<
                std::tuple<
                        std::string,
                        std::vector<std::shared_ptr<Operator> >
                >,
                std::shared_ptr<Operator>
        > ops_;
};

void black_scholes_template_opt(){

        auto ad_kernel = BlackScholesCallOption::Build<DoubleKernel>{};

        auto as_black = ad_kernel.Evaluate(
                DoubleKernel::BuildFromExo("t"),
                DoubleKernel::BuildFromExo("T"),
                DoubleKernel::BuildFromExo("r"),
                DoubleKernel::BuildFromExo("S"),
                DoubleKernel::BuildFromExo("K"),
                DoubleKernel::BuildFromExo("vol")
        );

        SymbolTable ST;
        ST("t"  , 0.0);
        ST("T"  , 10.0);
        ST("r"  , 0.04);
        ST("S"  , 50);
        ST("K"  , 60);
        ST("vol", 0.2);

        RemoveEndgenousFolder remove_endogous;
        Transform::FoldZero constant_fold;

        auto black_expr = as_black.as_operator_();

        std::cout << "--------- black_expr -----------\n";
        //black_expr->Display();
        std::cout << "black_expr->Eval(ST) => " << black_expr->Eval(ST) << "\n"; // __CandyPrint__(cxx-print-scalar,black_expr->Eval(ST))

        auto removed_endo = remove_endogous.Fold(black_expr);

        auto params = std::vector<std::string>{ "t", "T", "r", "S", "K", "vol" };

        std::vector<std::shared_ptr<Operator> > ticker;
        for(auto const& s : params){
                auto raw_diff = removed_endo->Diff(s);
                ticker.push_back(raw_diff);
        }

        auto unique_mapper = std::make_shared<RemapUnique>();

        std::ofstream out("black_better.h");

        out << "double black(";
        for(size_t idx=0;idx!=params.size();++idx){
                if( idx != 0 ){
                        out << ", ";
                }
                out << "double " << params[idx] << ", double* d_" << params[idx];
        }
        out << "){\n";

        std::unordered_set<std::shared_ptr<Operator> > seen;
        std::shared_ptr<EndgenousSymbol> return_;
        for(size_t idx=0;idx!=ticker.size();++idx){
                auto constant_folded = constant_fold.Fold(ticker[idx]);

                auto unique          = constant_folded->Clone(unique_mapper);

                auto dependents = unique->DepthFirstAnySymbolicDependency();

                for(auto const& dep : dependents.DepthFirst){
                        // first emit all the expressions we need
                        if( seen.count(dep) > 0 )
                                continue;
                        seen.insert(dep);

                        out << "    double " << std::left << std::setw(15) << dep->Name() << " = ";
                        dep->Expr()->EmitCode(out);
                        out << ";\n";
                }

                out << "    *d_" << params[idx] << " = ";
                unique->EmitCode(out);
                out << ";\n";

                //unique->Display();
        }

        auto constant_folded = constant_fold.Fold(removed_endo);

        auto unique          = constant_folded->Clone(unique_mapper);

        auto dependents = unique->DepthFirstAnySymbolicDependency();

        for(auto const& dep : dependents.DepthFirst){
                // first emit all the expressions we need
                if( seen.count(dep) > 0 )
                        continue;
                seen.insert(dep);

                out << "    double " << std::left << std::setw(15) << dep->Name() << " = ";
                dep->Expr()->EmitCode(out);
                out << ";\n";
        }
        out << "    return ";
        unique->EmitCode(out);
        out << ";\n";
        out << "}\n";

}

struct DataFlow;

struct DotCompiler{
        DotCompiler(){
                order.emplace_back();
        }
        std::stringstream nodes;
        std::stringstream edges;
        std::vector<std::vector<std::string> > order;
};

struct DataFlowGraph{
        void Add(std::shared_ptr<DataFlow> const& flow);
        void EmitDot(std::ostream& out)const;
        void EmitCppCode(std::ostream& out)const;
        void CollectADFlow(std::vector<std::shared_ptr<EndgenousSymbol> > & computation);
        void Display(std::ostream& out = std::cout)const;
        void EmitInstructions(InstructionBlock& B)const;
        void EmitADInstructions(Operator::DependentsProfile& information,
                                std::shared_ptr<RemapUnique> RU,
                                InstructionBlock& B)const;
private:
        std::vector<std::shared_ptr<DataFlow> > rank_;
        std::unordered_map<std::string,std::shared_ptr<DataFlow> > index_;
};

struct DataFlow{
        DataFlow(std::shared_ptr<EndgenousSymbol> sym)
                :sym_(sym)
        {}
        auto Expr()const{ return sym_; }
        auto Name()const{ return sym_->Name(); }
        void AddParent(DataFlow* ptr){
                parents_.push_back(ptr);
        }
        void AddChild(DataFlow* ptr){
                children_.push_back(ptr);
        }
        #if 0
        void EmitADInstructionsImpl(Operator::DependentsProfile& information, std::shared_ptr<RemapUnique> RU, InstructionBlock& B)const
        {
                auto remapped = sym_->Expr()->Clone(RU);

                auto dependents = remapped->DepthFirstAnySymbolicDependency();
                for(auto const& dep : dependents.DepthFirst ){
                        if( information.Set.count(dep) == 1 )
                                continue;
                        information.Set.insert(dep);
                        B.Add(std::make_shared<InstructionDeclareVariable>(
                                        dep->Name(),
                                        dep->Expr()));
                }

                auto exo = std::reinterpret_pointer_cast<EndgenousSymbol>(remapped);

                B.Add(std::make_shared<InstructionDeclareVariable>(
                                exo->Name(),
                                exo->Expr()));
                if( children_.empty() ){
                        std::vector<std::string> deps;
                        auto dependents = sym_->DepthFirstAnySymbolicDependencyAndThis();
                        for(auto const& ptr : dependents.DepthFirst ){
                                deps.push_back(ptr->Name());
                        }
                        B.Add(std::make_shared<InstructionComment>(deps));
                        B.Add(std::make_shared<InstructionReturn>(sym_->Name()));
                }
        }
        #endif
        void EmitInstructionsImpl(InstructionBlock& B)const{
                B.Add(std::make_shared<InstructionDeclareVariable>(
                                sym_->Name(),
                                sym_->Expr()));
                if( children_.empty() ){
                        std::vector<std::string> deps;
                        auto dependents = sym_->DepthFirstAnySymbolicDependencyAndThis();
                        for(auto const& ptr : dependents.DepthFirst ){
                                deps.push_back(ptr->Name());
                        }
                        B.Add(std::make_shared<InstructionComment>(deps));
                        B.Add(std::make_shared<InstructionReturn>(sym_->Name()));
                }
        }
        void EmitADInstructionsImpl(Operator::DependentsProfile& information, std::shared_ptr<RemapUnique> RU, InstructionBlock& B)const
        {
                auto make_ad_sym = [](auto const& name){
                        return "__rev_ad_" + name;
                };

                if( children_.size() == 0 ){
                        B.Add(std::make_shared<InstructionDeclareVariable>(
                                make_ad_sym(sym_->Name()),
                                Constant::Make(1.0)
                        ));
                } else {
                        static auto removed_end = std::make_shared<RemoveEndo>();
                        static Transform::FoldZero constant_fold;
                        // forward
                        auto make_node_back = [&](auto child){
                                return constant_fold.Fold(
                                        BinaryOperator::Mul(
                                                child->Expr()->Expr()->Diff(sym_->Name()),
                                                ExogenousSymbol::Make(make_ad_sym(child->Name()))
                                        )
                                );
                        };
                        std::shared_ptr<Operator> head = make_node_back(children_[0]);
                        for(size_t idx=1;idx<children_.size();++idx){
                                head = BinaryOperator::Add(
                                        head,
                                        make_node_back(children_[idx])
                                );
                        }

                        #if 0
                        auto remapped = head;
                        #else
                        std::stringstream cpp_expr;
                        head = head->Clone(removed_end);
                        head->EmitCode(cpp_expr);
                        //head = remove_endogous.Fold(head);
                        //auto folded = constant_fold.Fold(head);
                        auto remapped = head->Clone(RU);

                        B.Add(std::make_shared<InstructionComment>(std::vector<std::string>{"BEGIN " + cpp_expr.str()}));
                        auto dependents = remapped->DepthFirstAnySymbolicDependency();
                        for(auto const& dep : dependents.DepthFirst ){
                                if( information.Set.count(dep) == 1 )
                                        continue;
                                information.Set.insert(dep);
                                B.Add(std::make_shared<InstructionDeclareVariable>(
                                                dep->Name(),
                                                dep->Expr()));

                        }
                        B.Add(std::make_shared<InstructionComment>(std::vector<std::string>{"END " + cpp_expr.str()}));
                        #endif

                        B.Add(std::make_shared<InstructionDeclareVariable>(
                                make_ad_sym(sym_->Name()),
                                remapped));
                }

                if( parents_.size() == 0 ){
                        auto make_ptr_sym = [](auto const& name){
                                return "d_" + name;
                        };
                        auto param_name = std::dynamic_pointer_cast<ExogenousSymbol>(sym_->Expr());
                        B.Add(std::make_shared<InstructionPointerAssignment>(
                                        make_ptr_sym(param_name->Name()),
                                        make_ad_sym(sym_->Name())));
                }

#if 0

                auto make_ptr_sym = [](auto const& name){
                        return "d_" + name;
                };
                if( parents_.empty() ){
                        auto param_name = std::dynamic_pointer_cast<ExogenousSymbol>(sym_->Expr());
                        out << "*" << make_ptr_sym(param_name->Name()) << " = " << make_ad_sym(sym_->Name()) << ";\n";
                }
#endif
        }
        void EmitEvalCodeImpl(std::ostream& out)const{
                out << "double " << sym_->Name() << " = ";
                sym_->Expr()->EmitCode(out);
                out << ";\n";
                if( children_.size() == 0 ){
                        auto make_ad_sym = [](auto const& name){
                                return "__rev_ad_" + name;
                        };
                        auto make_ptr_sym = [](auto const& name){
                                return "d_" + name;
                        };
                        std::cout << sym_->Name() << "\n";
                        //sym_->Display();
                        if( auto param_name = std::dynamic_pointer_cast<ExogenousSymbol>(sym_->Expr()) ){
                                out << "*" << make_ptr_sym(param_name->Name()) << " = " << make_ad_sym(sym_->Name()) << ";\n";
                        } else {
                                out << "// unexpcted\n";
                        }
                }

                auto make_ad_sym = [](auto const& name){
                        return "__rev_ad_" + name;
                };
                auto make_ptr_sym = [](auto const& name){
                        return "d_" + name;
                };
                if( parents_.empty() ){
                        auto param_name = std::dynamic_pointer_cast<ExogenousSymbol>(sym_->Expr());
                        out << "*" << make_ptr_sym(param_name->Name()) << " = " << make_ad_sym(sym_->Name()) << ";\n";
                }
        }
        void EmitADFlowImpl(std::vector<std::shared_ptr<EndgenousSymbol> > & computation){
                auto make_ad_sym = [](auto const& name){
                        return "__rev_ad_" + name;
                };
                if( children_.size() > 0 ){
                        static Transform::FoldZero constant_fold;
                        // forward
                        auto make_node_back = [&](auto child){
                                return constant_fold.Fold(
                                        BinaryOperator::Mul(
                                                child->Expr()->Expr()->Diff(sym_->Name()),
                                                ExogenousSymbol::Make(make_ad_sym(child->Name()))
                                        )
                                );
                        };
                        std::shared_ptr<Operator> head = make_node_back(children_[0]);
                        for(size_t idx=1;idx<children_.size();++idx){
                                head = BinaryOperator::Add(
                                        head,
                                        make_node_back(children_[idx])
                                );
                        }

                        computation.push_back(EndgenousSymbol::Make( make_ad_sym(sym_->Name()), head));

                }
        }
        void EmitReverseADCodeImpl(std::ostream& out)const{
                auto make_ad_sym = [](auto const& name){
                        return "__rev_ad_" + name;
                };
                out << "double " << make_ad_sym(sym_->Name()) << " = ";
                if( children_.size() > 0 ){
                        static Transform::FoldZero constant_fold;
                        // forward
                        auto make_node_back = [&](auto child){
                                return constant_fold.Fold(
                                        BinaryOperator::Mul(
                                                child->Expr()->Expr()->Diff(sym_->Name()),
                                                ExogenousSymbol::Make(make_ad_sym(child->Name()))
                                        )
                                );
                        };
                        std::shared_ptr<Operator> head = make_node_back(children_[0]);
                        for(size_t idx=1;idx<children_.size();++idx){
                                head = BinaryOperator::Add(
                                        head,
                                        make_node_back(children_[idx])
                                );
                        }

                        head->EmitCode(out);
                } else {
                        out << "1.0";
                }
                out << ";\n";

                auto make_ptr_sym = [](auto const& name){
                        return "d_" + name;
                };
                if( parents_.empty() ){
                        auto param_name = std::dynamic_pointer_cast<ExogenousSymbol>(sym_->Expr());
                        out << "*" << make_ptr_sym(param_name->Name()) << " = " << make_ad_sym(sym_->Name()) << ";\n";
                }

        }

        void EmitDot(DotCompiler& compiler)const{
                if( parents_.size() ){
                        compiler.order.emplace_back(std::vector<std::string>{sym_->Name()});
                } else {
                        compiler.order[0].push_back(sym_->Name());
                }
                compiler.nodes << sym_->Name() << "[shape=record, label=\"<expr>";
                compiler.nodes << sym_->Name() << " = ";
                sym_->Expr()->EmitCode(compiler.nodes);

                #if 0
                for(auto const& ptr : parents_){
                        auto name = ptr->Name();
                        auto diff = sym_->Expr()->Diff(name);
                        Transform::FoldZero constant_fold;
                        auto folded = constant_fold.Fold(diff);
                        compiler.nodes << "|D[" << name << "] = ";
                        folded->EmitCode(compiler.nodes);
                }
                #endif
                Transform::FoldZero constant_fold;
                compiler.nodes << "|<diff>D[" << sym_->Name() << "]";
                if( parents_.size() > 0 ){

                        // forward
                        auto make_node_fwd = [&](auto name){
                                return constant_fold.Fold(
                                        BinaryOperator::Mul(
                                                sym_->Expr()->Diff(name),
                                                ExogenousSymbol::Make("D[" + name + "]")
                                        )
                                );
                        };
                        std::shared_ptr<Operator> head = make_node_fwd(parents_[0]->Name());
                        for(size_t idx=1;idx<parents_.size();++idx){
                                head = BinaryOperator::Add(
                                        head,
                                        make_node_fwd(parents_[idx]->Name())
                                );
                        }

                        compiler.nodes << " = ";
                        head->EmitCode(compiler.nodes);

                }

                compiler.nodes << "|<bdiff>B[" << sym_->Name() << "]";
                if( children_.size() > 0 ){
                        // forward
                        auto make_node_back = [&](auto child){
                                return constant_fold.Fold(
                                        BinaryOperator::Mul(
                                                child->Expr()->Expr()->Diff(sym_->Name()),
                                                ExogenousSymbol::Make("B[" + child->Name() + "]")
                                        )
                                );
                        };
                        std::shared_ptr<Operator> head = make_node_back(children_[0]);
                        for(size_t idx=1;idx<children_.size();++idx){
                                head = BinaryOperator::Add(
                                        head,
                                        make_node_back(children_[idx])
                                );
                        }

                        compiler.nodes << " = ";
                        head->EmitCode(compiler.nodes);
                }
                compiler.nodes << "\"];\n";
                #if 1
                for(auto const& ptr : parents_){

                        auto name = ptr->Name();

                        auto diff = sym_->Expr()->Diff(name);
                        auto folded = constant_fold.Fold(diff);

                        compiler.edges << name << ":diff -> " << sym_->Name() << ":diff [color=blue,label=\"";
                        folded->EmitCode(compiler.edges);
                        compiler.edges << "\"];\n";
                }
                #endif
                for(auto const& ptr : children_ ){

                        auto diff = ptr->Expr()->Expr()->Diff(sym_->Name());
                        auto folded = constant_fold.Fold(diff);

                        compiler.edges << ptr->Name() << ":bdiff -> " << sym_->Name() << ":bdiff [color=red,label=\"";
                        folded->EmitCode(compiler.edges);
                        compiler.edges << "\"];\n";
                }

        }
private:
        std::shared_ptr<EndgenousSymbol> sym_;
        std::vector<DataFlow*> parents_;
        std::vector<DataFlow*> children_;
};
void DataFlowGraph::CollectADFlow(std::vector<std::shared_ptr<EndgenousSymbol> > & computation){
        for(auto const& flow : rank_){
                flow->EmitADFlowImpl(computation);
        }
}

void DataFlowGraph::EmitDot(std::ostream& out)const{
        DotCompiler dc;
        for(auto const& flow : rank_){
                flow->EmitDot(dc);
        }
        out << "digraph{\n";
        out << dc.nodes.str();
        out << dc.edges.str();

        out << "node [shape = none];\n";
        for(size_t idx=0;idx!=dc.order.size();++idx){
                if( idx != 0 )
                        out << "->";
                out << idx;
        }
        out << "[arrowhead=none,shape=none]\n";
        for(size_t idx=0;idx!=dc.order.size();++idx){
                out << "{rank=same;" << idx;
                for(auto const& name : dc.order[idx] ){
                        out << "," << name;
                }
                out << "}\n";
        }
        out << "}\n";
}
void DataFlowGraph::Display(std::ostream& out)const{
        for(auto const& flow : rank_){
                std::cout << "--: " << flow->Name() << "\n";
        }
}
void DataFlowGraph::EmitInstructions(InstructionBlock& B)const{
        for(auto const& flow : rank_){
                flow->EmitInstructionsImpl(B);
        }
}
void DataFlowGraph::EmitADInstructions(Operator::DependentsProfile& information,
                                       std::shared_ptr<RemapUnique> RU,
                                       InstructionBlock& B)const
{
        for(size_t idx=rank_.size();idx;){
                --idx;
                rank_[idx]->EmitADInstructionsImpl(information, RU, B);
        }
}
void DataFlowGraph::EmitCppCode(std::ostream& out)const{
        out << R"(

#include <cstdio>
#include <cmath>
#include <iostream>

double black(double t, double* d_t, double T, double* d_T, double r, double* d_r, double S, double* d_S, double K, double* d_K, double vol, double* d_vol){
)";
        for(auto const& flow : rank_){
                flow->EmitEvalCodeImpl(out);
        }
#if 0
for(size_t idx=rank_.size();idx;){
        --idx;
        rank_[idx]->EmitReverseADCodeImpl(out);
}
#endif
        out << "return " << rank_.back()->Name() << ";\n";
        out << "}";
        out <<
R"(

int main(){
        double t   = 0.0;
        double T   = 10.0;
        double r   = 0.04;
        double S   = 50;
        double K   = 60;
        double vol = 0.2;

        double d_t = 0.0;
        double d_T = 0.0;
        double d_r = 0.0;
        double d_S = 0.0;
        double d_K = 0.0;
        double d_vol = 0.0;
        double value = black( t  , &d_t, T  , &d_T, r  , &d_r, S  , &d_S, K  , &d_K, vol, &d_vol);

        std::cout << "black = " << value << "\n";
        std::cout << "d_t   = " << d_t << "\n";
        std::cout << "d_T   = " << d_T << "\n";
        std::cout << "d_r   = " << d_r << "\n";
        std::cout << "d_S   = " << d_S << "\n";
        std::cout << "d_K   = " << d_K << "\n";
        std::cout << "d_vol = " << d_vol << "\n";
}
)";
}

void DataFlowGraph::Add(std::shared_ptr<DataFlow> const& flow){
        rank_.push_back(flow);
        index_[flow->Name()] = flow;
        auto dependents = flow->Expr()->DepthFirstAnySymbolicDependencyNoRecurse();

        auto link_dependency = [&](auto& child, auto& parent){
                child->AddParent(parent.get());
                parent->AddChild(child.get());
        };

        for(auto const& ptr : dependents.DepthFirst ){
                auto iter = index_.find(ptr->Name());
                if( iter == index_.end() )
                        throw std::domain_error("cant find node");
                link_dependency(flow, iter->second);
        }
}

void reverse_test(){
        using namespace Frontend;
        using Frontend::Sin;
        #if 0
        auto x1 = Var("x1");
        auto x2 = Var("x2");
        auto expr_ = Break("z", Break("y", x1 * x2 + Sin(x1)));
        auto expr = expr_.as_operator_();
        #else
        auto ad_kernel = BlackScholesCallOption::Build<DoubleKernel>{};

        auto as_black = ad_kernel.Evaluate(
                DoubleKernel::BuildFromExo("t"),
                DoubleKernel::BuildFromExo("T"),
                DoubleKernel::BuildFromExo("r"),
                DoubleKernel::BuildFromExo("S"),
                DoubleKernel::BuildFromExo("K"),
                DoubleKernel::BuildFromExo("vol")
        );
        auto expr_ = as_black.as_operator_();
        RemoveEndgenousFolder remove_endogous;
        auto expr = remove_endogous.Fold(expr_);
        #endif

        auto unique_mapper = std::make_shared<RemapUnique>("w");
        auto unique        = Break("value", expr->Clone(unique_mapper)).as_operator_();
        //unique->Display();

        auto dependents = unique->DepthFirstAnySymbolicDependencyAndThis();
        std::vector<std::shared_ptr<DataFlow> > flow;
        DataFlowGraph graph;
        for(auto const& dep : dependents.DepthFirst){
                auto ptr = std::make_shared<DataFlow>(dep);
                flow.push_back(ptr);
                graph.Add(ptr);
        }
        #if 0
        for(auto const& step : flow){
                step->Expr()->Display();
        }
        #endif

        std::ofstream out("graph.dot");
        graph.EmitDot(out);
        out.close();
        std::system("dot -Tpng graph.dot -o graph.png");

        InstructionBlock B;
        graph.EmitInstructions(B);

        std::cout << "dependents.Set.size() => " << dependents.Set.size() << "\n"; // __CandyPrint__(cxx-print-scalar,dependents.Set.size())
        graph.EmitADInstructions(dependents, unique_mapper, B);

        std::ofstream code("black.cpp");
        code << R"(

#include <cstdio>
#include <cmath>
#include <iostream>

double black(double t, double* d_t, double T, double* d_T, double r, double* d_r, double S, double* d_S, double K, double* d_K, double vol, double* d_vol){
)";

        B.EmitCode(code);
        code <<
R"(
}

int main(){
        double t   = 0.0;
        double T   = 10.0;
        double r   = 0.04;
        double S   = 50;
        double K   = 60;
        double vol = 0.2;

        double d_t = 0.0;
        double d_T = 0.0;
        double d_r = 0.0;
        double d_S = 0.0;
        double d_K = 0.0;
        double d_vol = 0.0;
        double value = black( t  , &d_t, T  , &d_T, r  , &d_r, S  , &d_S, K  , &d_K, vol, &d_vol);

        std::cout << "black = " << value << "\n";
        std::cout << "d_t   = " << d_t << "\n";
        std::cout << "d_T   = " << d_T << "\n";
        std::cout << "d_r   = " << d_r << "\n";
        std::cout << "d_S   = " << d_S << "\n";
        std::cout << "d_K   = " << d_K << "\n";
        std::cout << "d_vol = " << d_vol << "\n";
}
)";

        #if 0
        std::vector<std::shared_ptr<EndgenousSymbol> > ad_computation;
        graph.Display();
        graph.CollectADFlow(ad_computation);
        std::vector<std::shared_ptr<Operator> > computation;
        computation.push_back(unique);

        unique_mapper->mutate_prefix("__rev_ad_");

        for(auto& ptr : ad_computation ){
                computation.push_back(ptr->Clone(unique_mapper));
                //computation.push_back(ptr);
                //ptr->Display();
        }

        std::cout << "ad_computation.size() => " << ad_computation.size() << "\n"; // __CandyPrint__(cxx-print-scalar,computation.size())
        std::cout << "computation.size() => " << computation.size() << "\n"; // __CandyPrint__(cxx-print-scalar,computation.size())

        do{

                DataFlowGraph ad_graph;
                Operator::DependentsProfile dependents;
                for(auto const& ptr : computation ){
                        ptr->CollectDepthFirstAnySymbolicDependency(dependents, true);
                }
                for(auto const& dep : dependents.DepthFirst){
                        auto ptr = std::make_shared<DataFlow>(dep);
                        flow.push_back(ptr);
                        ad_graph.Add(ptr);
                }
                ad_graph.Display();
                std::cerr << __FILE__ << ":" << __LINE__ << ":A\n"; // __CandyTag__A
                std::ofstream code("black.cpp");
                ad_graph.EmitCppCode(code);
                code.close();
        }while(0);
        #endif

}

int main(){
        //black_scholes();
        //black_scholes_frontend();
        //black_scholes_template();
        //black_scholes_template_opt();

        try{
                reverse_test();
        } catch ( std::exception const& e ){
                std::cerr << "Exception: " << e.what() << "\n";
        }
}


/*
 * This file is part of bogus, a C++ sparse block matrix library.
 *
 * Copyright 2013 Gilles Daviet <gdaviet@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/.
*/

#ifndef BOGUS_POLYNOMIAL_IMPL_HPP
#define BOGUS_POLYNOMIAL_IMPL_HPP

#ifndef BOGUS_WITHOUT_EIGEN
#include <Eigen/Eigenvalues>
#endif

#include "Polynomial.hpp"
#include "NumTraits.hpp"

namespace bogus
{

namespace polynomial {

#ifndef BOGUS_WITHOUT_EIGEN

#ifdef _MSC_VER
#define BOGUS_TLS_SPEC __declspec( thread )
#else
// gcc on MacOS X does not support thread local storage
#if !defined( __GNUC__ ) or !defined( __APPLE__ )
#define BOGUS_TLS_SPEC __thread
#endif
#endif

template< unsigned Dimension, typename Scalar >
struct CompanionMatrix
{
	typedef Eigen::Matrix< Scalar, Dimension, Dimension > BaseType ;

#ifdef BOGUS_TLS_SPEC
	typedef typename BaseType::MapType ReturnType ;

	static ReturnType get()
	{
		static BOGUS_TLS_SPEC double s_matrix_data[ Dimension*Dimension ] ;
		static BOGUS_TLS_SPEC bool s_matrix_initialized = false ;

		ReturnType matrix( s_matrix_data ) ;

		if( !s_matrix_initialized )
		{
			matrix.template block< 1, Dimension-1> ( 0, 0 ).setZero() ;
			matrix.template block< Dimension - 1, Dimension -1 >( 1, 0 ).setIdentity() ;
			s_matrix_initialized = true ;
		}

		return matrix ;
	}
#else
	typedef BaseType ReturnType ;

	static ReturnType get()
	{
		static BaseType s_matrix ;
		static bool s_matrix_initialized = false ;

		if( !s_matrix_initialized )
		{
			s_matrix.template block< 1, Dimension-1> ( 0, 0 ).setZero() ;
			s_matrix.template block< Dimension - 1, Dimension -1 >( 1, 0 ).setIdentity() ;
			s_matrix_initialized = true ;
		}

		return s_matrix ;
	}
#endif
} ;

template< unsigned Dimension, typename Scalar >
unsigned RootsFinder< Dimension, Scalar>::getRealRoots(const Scalar *coeffs, Scalar *realRoots,
		RealRootsFilter filter )
{
	typedef CompanionMatrix< Dimension, Scalar > CM ;
	typename CM::ReturnType matrix = CM::get() ;

	matrix.template block< Dimension, 1 >( 0, Dimension - 1 ) = -Eigen::Matrix< Scalar, Dimension, 1 >::Map( coeffs ) ;
	const typename Eigen::EigenSolver< typename CM::BaseType >::EigenvalueType& ev = matrix.eigenvalues() ;

	unsigned count = 0 ;
	for( unsigned i = 0 ; i < Dimension ; ++i )
	{
		if( NumTraits< Scalar >::isZero( std::imag( ev[i] ) ) )
		{
			const bool discard =
					( filter == StrictlyPositiveRoots && std::real( ev[i] ) <= 0 ) ||
					( filter == StrictlyNegativeRoots && std::real( ev[i] ) >= 0 ) ;
			if( !discard ) realRoots[ count++ ] = std::real( ev[i] ) ;
		}
	}
	return count ;
}

#else
template< unsigned Dimension, typename Scalar >
unsigned RootsFinder< Dimension, Scalar>::getRealRoots(const Scalar *coeffs, Scalar *realRoots,
		RealRootsFilter filter )
{
	assert( 0 && "bogus::Polynomial::RootsFinder::getRealRoots requires Eigen" ) ;
	return 0 ;
}
#endif

template< typename Scalar >
struct PossiblyDegenerateRootsFinder< 0, Scalar >
{
	static unsigned getRealRoots( const Scalar* coeffs,
								  Scalar* realRoots,
								  RealRootsFilter filter = AllRoots )
	{
		realRoots[0] = 0. ;
		return filter == AllRoots && NumTraits< Scalar >::isZero( coeffs[0] ) ;
	}
} ;

template< unsigned Dimension, typename Scalar >
unsigned PossiblyDegenerateRootsFinder< Dimension, Scalar>::getRealRoots( Scalar *coeffs, Scalar *realRoots,
		RealRootsFilter filter )
{
	if( NumTraits< Scalar >::isZero( coeffs[ Dimension ] ) )
	{
		return PossiblyDegenerateRootsFinder< Dimension - 1, Scalar >::getRealRoots( coeffs, realRoots, filter ) ;
	}
	const Scalar inv = 1./coeffs[Dimension] ;
	for( unsigned k = 0 ; k < Dimension ; ++k )
	{
		coeffs[k] *= inv ;
	}
	return RootsFinder< Dimension, Scalar >::getRealRoots( coeffs, realRoots, filter ) ;
}

} //namespace polynomial

} //namespace bogus

#endif


/* =========================================================================
   Copyright (c) 2010-2012, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/*
*
*   Tutorial:  Use of the iterative solvers in ViennaCL with Eigen (http://eigen.tuxfamily.org/)
*
*/

//
// include necessary system headers
//
#include <iostream>

#ifndef NDEBUG
 #define NDEBUG
#endif

//
// Include Eigen headers
//
#include <Eigen/Core>
#include <Eigen/Sparse>

// Must be set prior to any ViennaCL includes if you want to use ViennaCL algorithms on Eigen objects
#define VIENNACL_WITH_EIGEN 1

//
// ViennaCL includes
//
#include "viennacl/linalg/ilu.hpp"
#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/bicgstab.hpp"
#include "viennacl/linalg/gmres.hpp"
#include "viennacl/io/matrix_market.hpp"

// Some helper functions for this tutorial:
#include "Random.hpp"
#include "vector-io.hpp"
#include "../benchmarks/benchmark-utils.hpp"

int main(int, char *[])
{
  typedef float ScalarType;

  Eigen::SparseMatrix<ScalarType, Eigen::RowMajor> eigen_matrix(65025, 65025);
  Eigen::VectorXf eigen_rhs;
  Eigen::VectorXf eigen_result;
  Eigen::VectorXf ref_result;
  Eigen::VectorXf residual;

  //
  // Read system from file
  //
  std::cout << "Reading matrix..." << std::endl;
  eigen_matrix.reserve(65025 * 7);
  if (!viennacl::io::read_matrix_market_file(eigen_matrix, "../examples/testdata/mat65k.mtx"))
  {
    std::cout << "Error reading Matrix file" << std::endl;
    return 0;
  }
  //eigen_matrix.endFill();
  std::cout << "Done: reading matrix" << std::endl;

  if (!readVectorFromFile("../examples/testdata/rhs65025.txt", eigen_rhs))
  {
    std::cout << "Error reading RHS file" << std::endl;
    return 0;
  }

  if (!readVectorFromFile("../examples/testdata/result65025.txt", ref_result))
  {
    std::cout << "Error reading Result file" << std::endl;
    return 0;
  }

  //CG solver:
  std::cout << "----- Running CG -----" << std::endl;
  eigen_result = viennacl::linalg::solve(eigen_matrix, eigen_rhs, viennacl::linalg::cg_tag());

  residual = eigen_matrix * eigen_result - eigen_rhs;
  std::cout << "Relative residual: " << viennacl::linalg::norm_2(residual) / viennacl::linalg::norm_2(eigen_rhs) << std::endl;

  //BiCGStab solver:
  std::cout << "----- Running BiCGStab -----" << std::endl;
  eigen_result = viennacl::linalg::solve(eigen_matrix, eigen_rhs, viennacl::linalg::bicgstab_tag());

  residual = eigen_matrix * eigen_result - eigen_rhs;
  std::cout << "Relative residual: " << viennacl::linalg::norm_2(residual) / viennacl::linalg::norm_2(eigen_rhs) << std::endl;

  //GMRES solver:
  std::cout << "----- Running GMRES -----" << std::endl;
  eigen_result = viennacl::linalg::solve(eigen_matrix, eigen_rhs, viennacl::linalg::gmres_tag());

  residual = eigen_matrix * eigen_result - eigen_rhs;
  std::cout << "Relative residual: " << viennacl::linalg::norm_2(residual) / viennacl::linalg::norm_2(eigen_rhs) << std::endl;

}


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

#include <armadillo>

#include <visualization_msgs/MarkerArray.h>

#include <ergodic_exploration/grid.hpp>
#include <ergodic_exploration/numerics.hpp>

namespace ergodic_exploration
{
using arma::mat;
using arma::vec;
struct Gaussian;
typedef std::vector<Gaussian> GaussianList;

/** @brief 2D gaussian */
struct Gaussian
{
  /** @brief Constructor */
  Gaussian()
  {
  }

  /**
   * @brief Constructor
   * @param mu - mean [mean x, mean y]
   * @param sigmas - standard deviations [sigma x, sigma y]
   */
  Gaussian(const vec& mu, const vec& sigmas)
    : mu(mu), cov(arma::diagmat(square(sigmas))), cov_inv(inv(cov))
  {
  }

  /**
   * @brief Evaluate gaussian
   * @param pt - point [x y]
   * @return evaluated gaussian at pt
   */
  double operator()(const vec& pt) const
  {
    const vec diff = pt - mu;
    return std::exp(-0.5 * dot(diff.t() * cov_inv, diff));
  }

  /**
   * @brief Evaluate gaussian
   * @param pt - point [x y]
   * @param trans - translation from map frame to fourier domain
   * @return evaluated gaussian at pt translated by trans
   * @details the translation is used to translate the mean into the fourier domain
   */
  double operator()(const vec& pt, const vec& trans) const
  {
    // DEBUG
    // if (any(mu - trans) < 0.0)
    // {
    //   std::cout << "WARNING: Targert mean not within fourier domain" << std::endl;
    // }

    // translate mu into frame of fourier domain
    const vec diff = pt - (mu - trans);
    return std::exp(-0.5 * dot(diff.t() * cov_inv, diff));
  }

  vec mu;       // mean
  mat cov;      // covariance
  mat cov_inv;  // inverse of covariance
};

/** @brief Target distribution */
class Target
{
public:
  /** @brief Constructor */
  Target();

  /**
   * @brief Constructor
   * @param gaussians - list of target gaussians
   */
  Target(const GaussianList& gaussians);

  /**
   * @brief Adds gaussian to list
   * @param g - gaussians
   */
  void addGaussian(const Gaussian& g);

  /**
   * @brief Remove gaussian from list
   * @param idx - index of gaussian to remove
   */
  void deleteGaussian(unsigned int idx);

  /**
   * @brief Evaluate the list of gaussians
   * @param pt - point [x y]
   * @param trans - translation from map frame to fourier domain
   * @return value of the list of gaussians evaluated at pt translated by trans
   * @details the translation is used to translate the mean into the fourier domain
   */
  double evaluate(const vec& pt, const vec& trans) const;

  /**
   * @brief Evaluate the target distribution
   * @param trans - translation from map frame to fourier domain
   * @param phi_grid - discretization of fourier domain
   * @return target evaluated at each grid cell in phi_grid
   * @details the translation is used to translate the mean into the fourier domain
   */
  vec fill(const vec& trans, const mat& phi_grid) const;

  /**
   * @brief Visualize target distribution
   * @param frame - target frame
   * @return target is visualized as an ellipse
   */
  visualization_msgs::MarkerArray markers(const std::string& frame) const;

private:
  GaussianList gaussians_;  // list of target gaussians
};
}  // namespace ergodic_exploration
#endif


// A numerically integrated implementation of the circuit described in
// "Rolling Your Own Circuit Simulator with Eigen and Boost.ODEInt"
// Author: Jeff Trull <edaskel@att.net>

/*
Copyright (c) 2014 Jeffrey E. Trull

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

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

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

#include <iostream>
#include <array>
#include <boost/numeric/odeint.hpp>

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

struct circuit {
    circuit(double r, double l, double c) : r_(r), l_(l), c_(c) {}

    void operator()(state_t const& x, state_t& dxdt, double t) {
        // calculate state derivatives from current state
        dxdt[0] = ((1 - x[0]) / r_ - x[1]) / c_;  // KCL at V_out node
        dxdt[1] = x[0] / l_;                      // from V_out = L * dI_L/dt
    }
private:
    double r_, l_, c_;
};

int main() {
    using namespace boost::numeric::odeint;
    circuit ckt(100.0, 20e-6, 20e-9);
    state_t x{0.0, 0.0};                    // initial conditions
    integrate( ckt, x, 0.0, 10e-6, 0.1e-6,  // time range and increment
               [](state_t const& x, double t) {
                   std::cout << t << " " << x[0] << std::endl;
               });
}