// discrete_distribution.hpp
//
// Copyright (c) 2009
// Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_RANDOM_DISCRETE_DISTRIBUTION_HPP_INCLUDED
#define BOOST_RANDOM_DISCRETE_DISTRIBUTION_HPP_INCLUDED

#include <vector>
#include <cassert>
#include <limits>
#include <numeric>
#include <utility>

#include <boost/range.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/variate_generator.hpp>

namespace boost {
namespace random {

template<class IntType, class WeightType>
class discrete_distribution {
public:
    typedef WeightType input_type;
    typedef IntType result_type;

    template<class Iter>
    discrete_distribution(Iter begin, Iter end) : weights(begin, end), data(weights.size()) {
        std::size_t size = weights.size();
        //assert(size <= (std::numeric_limits<IntType>::max)());
        std::vector<std::pair<WeightType, IntType> > below_average;
        std::vector<std::pair<WeightType, IntType> > above_average;
        WeightType weight_sum = std::accumulate(weights.begin(), weights.end(), static_cast<WeightType>(0));
        WeightType weight_average = weight_sum / size;
        for(std::size_t i = 0; i < size; ++i) {
            if(weights[i] < weight_average) {
                below_average.push_back(std::make_pair(weights[i] / weight_average, static_cast<IntType>(i)));
            } else {
                above_average.push_back(std::make_pair(weights[i] / weight_average, static_cast<IntType>(i)));
            }
        }
        typedef typename range_iterator<
            std::vector<
                std::pair<WeightType, IntType>
            >
        >::type iter_;
            iter_ b_iter = below_average.begin();
            iter_ b_end = below_average.end();
            iter_ a_iter = above_average.begin();
            iter_ a_end = above_average.end();
        while(b_iter != b_end && a_iter != a_end) {
            data[b_iter->second] = std::make_pair(b_iter->first, a_iter->second);
            a_iter->first -= (1 - b_iter->first);
            if(a_iter->first < 1) {
                *b_iter = *a_iter++;
            } else {
                ++b_iter;
            }
        }
        for(; b_iter != b_end; ++b_iter) {
            data[b_iter->second].first = 1;
        }
        for(; a_iter != a_end; ++a_iter) {
            data[a_iter->second].first = 1;
        }
    }
    template<class Engine>
    IntType operator()(Engine& eng) const {
        assert(!data.empty());
        boost::variate_generator<Engine&, boost::uniform_01<WeightType> > real_gen(eng, boost::uniform_01<WeightType>());
        WeightType test = real_gen() * data.size();
        IntType result = static_cast<IntType>(test);
        if(test - result < data[result].first) {
            return result;
        } else {
            return(data[result].second);
        }
    }

    result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return 0; }
    result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const { return static_cast<result_type>(weights.size() - 1); }
private:
    std::vector<WeightType> weights;
    std::vector<std::pair<WeightType, IntType> > data;
};

}
}

#endif


#include "common/common.hpp"

#include "geodb/hilbert.hpp"

#include <boost/program_options.hpp>
#include <fmt/format.h>
#include <fmt/ostream.h>

#include <iostream>

namespace po = boost::program_options;

using std::cout;
using std::cerr;

void parse_options(int argc, char** argv);

template<u32 Dimension, u32 Precision>
json curve_json();

int main(int argc, char** argv) {
    return tpie_main([&]{
        parse_options(argc, argv);

        json curves = json::array({
            curve_json<2, 1>(),
            curve_json<2, 2>(),
            curve_json<2, 3>(),
            curve_json<3, 1>(),
            curve_json<3, 2>(),
            curve_json<3, 3>(),
        });

        fmt::print(cout, "{}\n", curves.dump(4));
        return 0;
    });
}

void parse_options(int argc, char** argv) {
    po::options_description options("Options");
    options.add_options()
            ("help,h", "Show this message.");

    po::variables_map vm;
    try {
        po::command_line_parser p(argc, argv);
        p.options(options);
        po::store(p.run(), vm);

        if (vm.count("help")) {
            fmt::print(cerr, "Usage: {0}\n"
                             "\n"
                             "Outputs hilbert curve points for certain dimensions and precisions.\n"
                             "\n"
                             "{1}",
                       argv[0], options);
            throw exit_main(0);
        }

        po::notify(vm);
    } catch (const po::error& e) {
        fmt::print(cerr, "Failed to parse arguments: {}.\n", e.what());
        throw exit_main(1);
    }
}

/// Creates a vector of points that represents a walk over
/// the hilbert curve with the given Dimension and Precision.
template<u32 Dimension, u32 Precision>
auto curve_points()
{
    using curve_t = geodb::hilbert_curve<Dimension, Precision>;
    using point_t = typename curve_t::point_t;
    using index_t = typename curve_t::index_t;

    const index_t count = curve_t::index_count;

    std::vector<point_t> result(count);
    for (index_t i = 0; i < count; ++i) {
        result[i] = curve_t::hilbert_index_inverse(i);
    }
    return result;
}

template<u32 Dimension, u32 Precision>
json curve_json() {
    const auto points = curve_points<Dimension, Precision>();

    json json_data = json::object();
    json_data["dimension"] = Dimension;
    json_data["precision"] = Precision;

    json& json_points = json_data["points"] = json::array();
    for (const auto& point : points) {
        json jp = json::array();
        for (const auto& coord : point) {
            jp.push_back(coord.to_ullong());
        }
        json_points.push_back(jp);
    }
    return json_data;
}


#ifndef VECTORANDCIRCULARMATH_HPP_
#define VECTORANDCIRCULARMATH_HPP_

#include <boost/version.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/algorithm/string.hpp>
#include <opencv2/opencv.hpp>
#include <utility>
#include <vector>
#include <array>
#include <cmath>
#include <cassert>

inline bool operator==(const cv::Vec4i& lhs, const cv::Vec4i& rhs) {
  return ((lhs[0] == rhs[0]) &&
          (lhs[1] == rhs[1]) &&
          (lhs[2] == rhs[2]) &&
          (lhs[3] == rhs[3]));
};

inline bool operator<(const cv::Vec4i& lhs, const cv::Vec4i& rhs) {
  if (lhs[0] < rhs[0]) return true;
  else if (lhs[0] > rhs[0]) return false;
  if (lhs[1] < rhs[1]) return true;
  else if (lhs[1] > rhs[1]) return false;
  if (lhs[2] < rhs[2]) return true;
  else if (lhs[2] > rhs[2]) return false;
  if (lhs[3] < rhs[3]) return true;
  return false;
};
inline bool lessThanVec4i(const cv::Vec4i& lhs, const cv::Vec4i& rhs) {
  return (lhs < rhs);
};
inline bool lessThanArray5i(const std::array<int, 5>& lhs, const std::array<int, 5>& rhs) {
  return (lhs < rhs);
};

namespace vc_math {

#if BOOST_VERSION/100 < 1050
constexpr double pi = 3.1415926535897932384626433832795;
constexpr double two_pi = pi*2;
constexpr double inv_sqrt_2pi = 0.3989422804014327;
constexpr double degree = pi/180.0;
constexpr double radian = 180.0/pi;
constexpr double half_pi = pi/2.0;
#else
constexpr double pi = boost::math::constants::pi<double>();
constexpr double two_pi = boost::math::constants::two_pi<double>();
constexpr double inv_sqrt_2pi = 1.0/boost::math::constants::root_two_pi<double>();
constexpr double degree = boost::math::constants::degree<double>();
constexpr double radian = boost::math::constants::radian<double>();
constexpr double half_pi = boost::math::constants::half_pi<double>();
#endif

constexpr double INVALID_ANGLE = 361.0;

inline double normal_pdf(double x, double m, double s) {
  double a = (x - m) / s;
  return (inv_sqrt_2pi / s) * exp(-0.5 * a * a);
};

inline double log_normal_pdf(double x, double m, double s) {
  double a = (x - m) / s;
  return log(inv_sqrt_2pi) - log(s) -0.5 * a * a;
};

/**
 * Generates a pair of random samples from the unit Normal distribution,
 * using the Box-Mueller method.
 */
inline std::pair<double, double> randn() {
  double U = double(rand())/RAND_MAX;
  double V = double(rand())/RAND_MAX;
  double sqrtMinusTwolnU = sqrt(-2*log(U));
  double TwoPiV = two_pi * V;
  return std::pair<double, double>(sqrtMinusTwolnU * cos(TwoPiV), sqrtMinusTwolnU * sin(TwoPiV));
};

/**
 * Computes angular (and general modulo-'range') magnitude, signed
 */
inline double angularMag(
    double a,
    double b,
    double range = 360.0) {
  double d = b - a + range/2;
  d = (d > 0) ? d - floor(d/range)*range - range/2 : d - (floor(d/range) + 1)*range + range/2;
  return d;
};

/**
 * Computes angular (and general modulo-'range') distance
 */
inline double angularDist(double a, double b, double range = 360.0) { return fabs(angularMag(a, b, range)); };

/**
 * Computes Eulidean distance between 2 2-D points
 */
inline double dist(double x1, double y1, double x2, double y2) {
  return sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2));
};
inline double dist(const cv::Point2d& xy1, const cv::Point2d& xy2) {
  return sqrt((xy1.x-xy2.x)*(xy1.x-xy2.x)+(xy1.y-xy2.y)*(xy1.y-xy2.y));
};
inline double dist(const cv::Point2f& xy1, const cv::Point2f& xy2) {
  return sqrt((xy1.x-xy2.x)*(xy1.x-xy2.x)+(xy1.y-xy2.y)*(xy1.y-xy2.y));
};
inline double dist(const cv::Point2i& xy1, const cv::Point2i& xy2) {
  return sqrt((xy1.x-xy2.x)*(xy1.x-xy2.x)+(xy1.y-xy2.y)*(xy1.y-xy2.y));
};
inline double dist(const cv::Vec4i& xy12) {
  return sqrt((xy12[0]-xy12[2])*(xy12[0]-xy12[2])+(xy12[1]-xy12[3])*(xy12[1]-xy12[3]));
};
inline double distSqrd(double x1, double y1, double x2, double y2) {
  return (x1-x2)*(x1-x2)+(y1-y2)*(y1-y2);
};
inline double distSqrd(const cv::Point2d& xy1, const cv::Point2d& xy2) {
  return (xy1.x-xy2.x)*(xy1.x-xy2.x)+(xy1.y-xy2.y)*(xy1.y-xy2.y);
};
inline double distSqrd(const cv::Point2f& xy1, const cv::Point2f& xy2) {
  return (xy1.x-xy2.x)*(xy1.x-xy2.x)+(xy1.y-xy2.y)*(xy1.y-xy2.y);
};
inline double distSqrd(const cv::Point2i& xy1, const cv::Point2i& xy2) {
  return (xy1.x-xy2.x)*(xy1.x-xy2.x)+(xy1.y-xy2.y)*(xy1.y-xy2.y);
};
inline double distSqrd(const cv::Vec4i& xy12) {
  return (xy12[0]-xy12[2])*(xy12[0]-xy12[2])+(xy12[1]-xy12[3])*(xy12[1]-xy12[3]);
};

/**
 * Wraps angle in degrees to [0, maxAngle) range
 */
inline constexpr double wrapAngle(double angleDeg, double maxAngle = 360.0) {
  return (angleDeg - floor(angleDeg/maxAngle)*maxAngle);
};

/**
 * Count number of entries in matrix that are not equal
 *
 * NOTE: if Mat represents a colored image, then it will count different
 * color channels separately.
 */
inline unsigned long long countNotEqual(const cv::Mat& a, const cv::Mat& b) {
  if (a.size != b.size || a.channels() != b.channels()) {
    return std::max(a.rows * a.cols * a.channels(), b.rows * b.cols * b.channels());
  }

  cv::Mat not_equal;
  cv::compare(a, b, not_equal, cv::CMP_NE);
  cv::Scalar sum_not_equal = sum(sum(not_equal) / 255);
  return sum_not_equal[0] + sum_not_equal[1] + sum_not_equal[2] + sum_not_equal[3];
};

inline unsigned long long countNotEqual(const std::vector<cv::Point>& a,
    const std::vector<cv::Point>& b) {
  unsigned long long count = abs(a.size() - b.size());
  size_t minSize = std::min(a.size(), b.size());
  std::vector<cv::Point>::const_iterator itA = a.begin();
  std::vector<cv::Point>::const_iterator itB = b.begin();
  for (unsigned int i = 0; i < minSize; i++, itA++, itB++) {
    if (*itA != *itB) {
      count += 1;
    }
  }
  return count;
};

/**
 * Computes the Euclidean distance between a 2-D point and a 2-D line
 */
inline double distPointLine(
    const cv::Point& point,
    const cv::Vec4f& currFit) {
  // http://local.wasp.uwa.edu.au/~pbourke/geometry/pointline/
  double vx = currFit[0], vy = currFit[1], x1 = currFit[2], y1 = currFit[3];
  if (vx == 0 && vy == 0) {
    return 0; // P1 = P2!
  }
  double x3 = point.x, y3 = point.y;
  double u = ((x3-x1)*vx + (y3-y1)*vy)/(vx*vx+vy*vy);
  double dx = x1 + u*vx - x3, dy = y1 + u*vy - y3;
  return sqrt(dx*dx + dy*dy);
};

/**
 * Finds closest point to given line in given set of points.
 */
inline cv::Point findClosestPointToLine(
    const std::vector<cv::Point>& points,
    const cv::Vec4f& line) {
  // Identify the 8-connected set of edgels closest to the previous line fit
  std::vector<cv::Point>::const_iterator itPoints = points.begin();
  std::vector<cv::Point>::const_iterator itPointsEnd = points.end();
  double closestDist = distPointLine(*itPoints, line);
  cv::Point closestPoint = *itPoints;
  double currDist;
  for (itPoints++; itPoints != itPointsEnd; itPoints++) {
    currDist = distPointLine(*itPoints, line);
    if (currDist < closestDist) {
      closestDist = currDist;
      closestPoint = *itPoints;
    }
  }
  return closestPoint;
};

 /**
  * Computes 3D point on plane closest to specified 3D point
  *
  * Justification:
  * The equation of a plane is: n * v = d, where n is the normal vector,
  * v is an arbitrary 3D point, and d is a constant offset.
  *
  * Given query point v', then the closest point on the above plane is:
  * v* = v' + a * n, for some unknown factor a
  *
  * Also, we know that n * v* = d, by definition of the plane.
  *
  * Thus, n * (v' + a * n) = d,
  * i.e. a = ( d - n * v' ) / ( n * n ),
  * which allows us to then solve for v* by substituting in a
  *
  * (side-note: n * n is guaranteed to be always non-zero for plane to exist)
  */
inline cv::Vec3d findClosestPointOnPlane(const cv::Vec3d& vp,
    const cv::Vec3d& n, double d) {
   double a = (d - n.dot(vp)) / n.dot(n);
   cv::Vec3d vs = vp + a*n;
   return vs;
};

/**
 * Computes 2D point on line closest to specified 2D point
 *
 * Justification:
 * The equation of a line is: vla + a * vd,
 * where vd = vlb - vla, for some constant a
 *
 * Let vdd be orthogonal to vd. The resulting point vr satisfied both:
 * vr = vla + a * vd, for some constant a
 * vr = vp - d * vdd, for some constant d
 *
 * Hence, vd * a + vdd * b = vp - vla. We can now solve a and d using the normal equation.
 */
inline cv::Vec2d findClosestPointOnLine2(const cv::Vec2d& vp,
    const cv::Vec2d& vla, const cv::Vec2d& vlb) {
  if (vla == vlb) { return vp; }
  cv::Vec2d vd = vlb - vla;

  double det = vd[0]*vd[0] + vd[1]*vd[1];
  double a = (vd[0] * (vp[0] - vla[0]) + vd[1] * (vp[1] - vla[1])) / det;
  //double d = (vd[1] * (vp[0] - vla[0]) - vd[0] * (vp[1] - vla[1])) / det;
  cv::Vec2d result = vla + a * vd;

  return result;
};
/**
 * same as above, but takes in normal form of line: (n1, n2) * (x, y) = nd
 */
inline cv::Vec2d findClosestPointOnLine2(const cv::Vec2d& vp,
    double n1, double n2, double nd) {
  // Make sure normal vector of line has non-zero norm
  double det = n1*n1 + n2*n2;
  if (det == 0) { return vp; }

  // Compute point on line
  double vlx = 0, vly = 0;
  if (n1 == 0) { vly = nd/n2; }
  else { vlx = nd/n1; }

  // Compute vr = vl + a * vd, where vd is orthogonal to n
  double a = (n2 * (vp[0] - vlx) - n1 * (vp[1] - vly)) / det;

  return cv::Vec2d(vlx + a*n2, vly - a*n1);
};

/**
 * Computes 3D point on line closest to specified 3D point
 *
 * Justification:
 * The equation of a line is: vla + a * vld,
 * where vld = vlb - vla, for some constant a
 *
 * Denote the resulting point on the line as vr, then the vector vn = (vr->vp) is
 * orthogonal to vld, as well as to a 3rd orthogonal vector vo.
 * vo can be computed as vld x vnp, where vnp = (vl->vp) for any point vl on the line.
 *
 * Then, we can link the 2 points vla and vp together, as:
 * vnp = vp - vla = a * vld + d * vn
 *
 * There are 2 unknowns and 3 equations here, so we can solve this using the
 * normal equation.
 */
inline cv::Vec3d findClosestPointOnLine3(const cv::Vec3d& vp,
    const cv::Vec3d& vla, const cv::Vec3d& vlb) {
  cv::Vec3d result;
  if (vla == vlb) { return vp; }
  cv::Vec3d vld = vlb - vla;
  cv::Vec3d vnp = vp - vla;
  if (vp == vla) { vnp = vp - vlb; }
  cv::Vec3d vo = vld.cross(vnp);
  cv::Vec3d vn = vld.cross(vo);

  cv::Mat A(3, 2, CV_64FC1);
  cv::Mat b(3, 1, CV_64FC1);
  for (unsigned int i = 0; i < 3; i++) {
    A.at<double>(i, 0) = vld[i];
    A.at<double>(i, 1) = vn[i];
    b.at<double>(i, 0) = vnp[i];
  }
  cv::Mat sln = (A.t()*A).inv()*(A.t()*b);
  //double a = sln.at<double>(0, 0);
  double d = sln.at<double>(1, 0);
  result = vp - d*vn;

  return result;
};

/**
 * Find the closest point to a number of planes, where each plane is represented
 * by their normal vector n, and constant offset d, which are stored into
 * matrices A = [n1; n2; ...], and b = [d1; d2; ...].
 *
 * The query point vp is only used in degenerate cases; see below.
 *
 * For 1 single plane, this function returns the closest point on plane to
 * the query point vp.
 *
 * Otherwise, we check if (A'*A) is invertible, and if it is, then we can use
 * the normal equation to solve for the closest point.
 *
 * If (A'*A) is not invertible, then we choose the first 2 planes that have
 * different normals, and find their intersecting line. We then compute
 * the closest point on line to the query point.
 */
cv::Vec3d findClosestPointToPlanes3(const cv::Vec3d& vp,
    const cv::Mat& A, const cv::Mat& b);

/**
 * Find the closest point to a number of lines, where each line is represented
 * by their normal vector n, and constant offset d, which are stored into
 * matrices A = [n1; n2; ...], and b = [d1; d2; ...].
 *
 * The query point vp is only used in degenerate cases; see below.
 *
 * For 1 single line, this function returns the closest point on line to
 * the query point vp.
 *
 * Otherwise, we check if the lines are parallel via det(A) != 0. If they
 * intersect, then we can use the normal equation to solve for the closest point.
 *
 * If all lines are parallel, then we average over the offsets b, to get an
 * average line. We then return the closest point on this average line to the
 * query point.
 */
cv::Vec2d findClosestPointToLines2(const cv::Vec2d& vp,
    const cv::Mat& A, const cv::Mat& b);

/**
 * Check if the line segments (a<->b) and (c<->d) intersect with each other
 *
 * From: http://gamedev.stackexchange.com/questions/26004/how-to-detect-2d-line-on-line-collision
 *
 * WARNING: algorithm returns true for special case where the line segments
 *          are co-linear and do not overlap
 */
inline bool isIntersecting(const cv::Point2f& a, const cv::Point2f& b,
    const cv::Point2f& c, const cv::Point2f& d) {
  float denominator = ((b.x - a.x) * (d.y - c.y)) - ((b.y - a.y) * (d.x - c.x));
  float numerator1 = ((a.y - c.y) * (d.x - c.x)) - ((a.x - c.x) * (d.y - c.y));
  float numerator2 = ((a.y - c.y) * (b.x - a.x)) - ((a.x - c.x) * (b.y - a.y));

  if (denominator == 0) return numerator1 == 0 && numerator2 == 0;

  float r = numerator1 / denominator;
  float s = numerator2 / denominator;

  return (r >= 0 && r <= 1) && (s >= 0 && s <= 1);
};

/**
 * Computes x intercept (in pixels, intersection with bottom of image) and
 * slope (in degrees, 0' = top of image) given line in image
 *
 * \param line: straight line; format: [dx, dy, x0, y0]
 * \param imWidth: image width, in pixels
 * \param imHeight: image height, in pixels
 */
std::pair<double, double> computeXInterceptAndSlopeFromLine(
    cv::Vec4f line,
    unsigned int imWidth,
    unsigned int imHeight);

/**
 * Computes heading angle (in degrees) given line in overhead image, and
 * given preferred heading directionality.
 *
 * \param line: straight line; format: [dx, dy, x0, y0]
 * \param imWidth: image width, in pixels
 * \param imHeight: image height, in pixels
 * \param preferredDirDeg: preferred direction of heading, in degrees (0 = top of image, 90 = right of image)
 */
double computeHeadingFromOverheadLine(
    cv::Vec4f line,
    unsigned int imWidth,
    unsigned int imHeight,
    double preferredDirDeg);

/**
 * Computes point closest to border in the direction of heading,
 * centered at image's center.
 *
 * \param heading: desired heading, in degrees; 0 = North/top of image; 90 = East/right of image
 * \param imWidth: image width
 * \param imHeight: image height
 * \param marginWidth: width of margin around image borders to avoid when computing intersection
 */
cv::Point computeHeadingBorderIntersection(
    double heading,
    unsigned int imWidth,
    unsigned int imHeight,
    unsigned int marginWidth);

/**
 * Computes and returns closest point within set of points
 */
inline cv::Point findClosestPoint(
    const std::vector<cv::Point>& points,
    const cv::Point targetPoint) {
  std::vector<cv::Point>::const_iterator itPoints = points.begin();
  std::vector<cv::Point>::const_iterator itPointsEnd = points.end();
  cv::Point result = *itPoints;
  double bestDistSqrd =
      (itPoints->x - targetPoint.x)*(itPoints->x - targetPoint.x) +
      (itPoints->y - targetPoint.y)*(itPoints->y - targetPoint.y);
  double currDistSqrd;
  for (itPoints++; itPoints != itPointsEnd; itPoints++) {
    currDistSqrd =
        (itPoints->x - targetPoint.x)*(itPoints->x - targetPoint.x) +
        (itPoints->y - targetPoint.y)*(itPoints->y - targetPoint.y);
    if (currDistSqrd < bestDistSqrd) {
      result = *itPoints;
      bestDistSqrd = currDistSqrd;
    }
  }

  return result;
};

/**
 * Computes (minimum) scale factor between 2 sizes (i.e. prefer letterbox over
 * cropping)
 */
inline double computeScaleFactor(const cv::Size& from, const cv::Size& to) {
  if (from == cv::Size() || to == cv::Size()) {
    return 1.0;
  } else {
    return std::min(double(to.width) / double(from.width),
        double(to.height) / double(from.height));
  }
};

/**
 * Computes orientation of line segment (of the form [x1, y1, x2, y2]), in
 * radians.
 */
inline double orientation(const cv::Vec4i& seg) {
  return std::atan2(seg[3] - seg[1], seg[2] - seg[0]);
};
inline double orientation(const cv::Point2i& ptA, const cv::Point2i& ptB) {
  return std::atan2(ptB.y - ptA.y, ptB.x - ptA.x);
};
inline double orientation(const cv::Point2d& ptA, const cv::Point2d& ptB) {
  return std::atan2(ptB.y - ptA.y, ptB.x - ptA.x);
};

/**
 * Sort values in increasing order
 * NOTE: faster than recursive implementations and specifically
 *       cv::sort(vec4iA, vec4iB, CV_SORT_EVERY_COLUMN | CV_SORT_ASCENDING)
 */
inline cv::Vec4i sort(cv::Vec4i v) {
  cv::Vec4i s(v);
  if (v[0] > v[1]) {
    s[0] = v[1]; s[1] = v[0];
  }
  if (v[2] > v[3]) {
    s[2] = v[3]; s[3] = v[2];
  }

  if (s[0] > s[2]) {
    v[0] = s[2];
    if (s[3] > s[1]) {
      v[1] = s[0]; v[2] = s[1]; v[3] = s[3];
    } else if (s[3] > s[0]) {
      v[1] = s[0]; v[2] = s[3]; v[3] = s[1];
    } else {
      v[1] = s[3]; v[2] = s[0]; v[3] = s[1];
    }
  } else {
    v[0] = s[0];
    if (s[1] > s[3]) {
      v[1] = s[2]; v[2] = s[3]; v[3] = s[1];
    } else if (s[1] > s[2]) {
      v[1] = s[2]; v[2] = s[1]; v[3] = s[3];
    } else {
      v[1] = s[1]; v[2] = s[2]; v[3] = s[3];
    }
  }
  return v;
};

/**
 * Cycles vector such that smallest value is listed first, but the (cyclic)
 * ordering of values are preserved
 */
inline cv::Vec4i minCyclicOrder(cv::Vec4i v) {
  if ((v[0] <= v[1]) && (v[0] <= v[2]) && (v[0] <= v[3])) {
    return v;
  } else if ((v[1] <= v[0]) && (v[1] <= v[2]) && (v[1] <= v[3])) {
      return cv::Vec4i(v[1], v[2], v[3], v[0]);
  } else if ((v[2] <= v[0]) && (v[2] <= v[1]) && (v[2] <= v[3])) {
    return cv::Vec4i(v[2], v[3], v[0], v[1]);
  } else { // if ((v[3] <= v[0]) && (v[3] <= v[1]) && (v[3] <= v[2])) {
    return cv::Vec4i(v[3], v[0], v[1], v[2]);
  }
};

/**
 * Sorts and removes duplicate entries in-place
 */
inline void unique(std::vector<cv::Vec4i>& v) {
  std::sort(v.begin(), v.end(), lessThanVec4i);

  std::vector<cv::Vec4i> u;
  for (const cv::Vec4i& d: v) {
    if (u.empty()) { u.push_back(d); }
    else if (u.back() < d) { u.push_back(d); }
  }
  v.swap(u);
};
inline void unique(std::vector< std::array<int, 5> >& v) {
  std::sort(v.begin(), v.end(), lessThanArray5i);

  std::vector< std::array<int, 5> > u;
  for (const std::array<int, 5>& d: v) {
    if (u.empty()) { u.push_back(d); }
    else if (u.back() < d) { u.push_back(d); }
  }
  v.swap(u);
};

/**
 * Returns dot product of 2 line segments (of the form [x1, y1, x2, y2])
 */
inline double dot(const cv::Vec4i& A, const cv::Vec4i& B) {
  return (A[2]-A[0])*(B[2]-B[0]) + (A[3]-A[1])*(B[3]-B[1]);
};

/**
 * Returns dot product of 2 line segments
 */
inline double dot(const cv::Point2f& endA1, const cv::Point2f& endA2,
    const cv::Point2f& endB1, const cv::Point2f& endB2) {
  return (endA2.x-endA1.x)*(endB2.x-endB1.x) + (endA2.y-endA1.y)*(endB2.y-endB1.y);
};

/**
 * Converts a rotation matrix into a quaternion
 *
 * From: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
 */
inline void rotMat2quat(const cv::Mat rotMat,
    double& w, double& x, double& y, double& z) {
  assert(rotMat.rows == 3 && rotMat.cols == 3 && rotMat.isContinuous() &&
      rotMat.elemSize() == sizeof(double));

  double tr = rotMat.at<double>(0, 0) + rotMat.at<double>(1, 1) + rotMat.at<double>(2, 2);
  double s;

  if (tr > 0) {
    s = 0.5/sqrt(tr + 1.0);
    w = 0.25/s;
    x = (rotMat.at<double>(2, 1) - rotMat.at<double>(1, 2))*s;
    y = (rotMat.at<double>(0, 2) - rotMat.at<double>(2, 0))*s;
    z = (rotMat.at<double>(1, 0) - rotMat.at<double>(0, 1))*s;
  } else if (rotMat.at<double>(0, 0) > rotMat.at<double>(1, 1) &&
      rotMat.at<double>(0, 0) > rotMat.at<double>(2, 2)) {
    s = 2.0*sqrt(1.0 + 2*rotMat.at<double>(0, 0) - tr);
    w = (rotMat.at<double>(2, 1) - rotMat.at<double>(1, 2))/s;
    x = 0.25*s;
    y = (rotMat.at<double>(0, 1) + rotMat.at<double>(1, 0))/s;
    z = (rotMat.at<double>(0, 2) + rotMat.at<double>(2, 0))/s;
  } else if (rotMat.at<double>(1, 1) > rotMat.at<double>(2, 2)) {
    s = 2.0*sqrt(1.0 + 2*rotMat.at<double>(1, 1) - tr);
    w = (rotMat.at<double>(0, 2) - rotMat.at<double>(2, 0))/s;
    x = (rotMat.at<double>(0, 1) + rotMat.at<double>(1, 0))/s;
    y = 0.25*s;
    z = (rotMat.at<double>(1, 2) + rotMat.at<double>(2, 1))/s;
  } else {
    s = 2.0*sqrt(1.0 + 2*rotMat.at<double>(2, 2) - tr);
    w = (rotMat.at<double>(1, 0) - rotMat.at<double>(0, 1))/s;
    x = (rotMat.at<double>(0, 2) + rotMat.at<double>(2, 0))/s;
    y = (rotMat.at<double>(1, 2) + rotMat.at<double>(2, 1))/s;
    z = 0.25*s;
  }
};

inline cv::Mat quat2RotMat(double w, double x, double y, double z) {
  double distSqrd = x*x+y*y+z*z+w*w;
  if (distSqrd < 1e-15) { return cv::Mat::zeros(3, 3, CV_64FC1); }
  double s = 2.0/distSqrd;
  double xs = x * s,   ys = y * s,   zs = z * s;
  double wx = w * xs,  wy = w * ys,  wz = w * zs;
  double xx = x * xs,  xy = x * ys,  xz = x * zs;
  double yy = y * ys,  yz = y * zs,  zz = z * zs;
  cv::Mat result(3, 3, CV_64FC1);
  double* data = (double*) result.data;
  *data = 1.0 - (yy + zz); data++;
  *data = xy - wz; data++;
  *data = xz + wy; data++;
  *data = xy + wz; data++;
  *data = 1.0 - (xx + zz); data++;
  *data = yz - wx; data++;
  *data = xz - wy; data++;
  *data = yz + wx; data++;
  *data = 1.0 - (xx + yy);
  return result;
};

/**
 * Converts a quaternion to Euler angles
 * Assume right-hand coordinate frame, and static-x-y-z (sxyz) rotation ordering
 */
inline std::array<double, 3> quat2euler(double w, double x, double y, double z) {
  double distSqrd = x*x+y*y+z*z+w*w;
  if (distSqrd < 1e-15) { std::array<double, 3>{ {0, 0, 0} }; }
  w /= distSqrd;
  x /= distSqrd;
  y /= distSqrd;
  z /= distSqrd;

  return std::array<double, 3>{ {
    atan2(2*(x*w + z*y), (w*w - x*x - y*y + z*z)),
    asin(2*(y*w - x*z)),
    atan2(2*(x*y + z*w), (w*w + x*x - y*y - z*z))} };
};
inline std::array<double, 3> quat2euler(std::array<double, 4> wxyz) {
  return quat2euler(wxyz[0], wxyz[1], wxyz[2], wxyz[3]);
};

/**
 * Computes (normalized) inverse of a given quaternion
 */
inline std::array<double, 4> quatInv(double w, double x, double y, double z) {
  double distSqrd = x*x+y*y+z*z+w*w;
  if (distSqrd < 1e-15) { std::array<double, 4>{ {0, 0, 0, 0} }; }
  return std::array<double, 4>{ {w/distSqrd,
    -x/distSqrd, -y/distSqrd, -z/distSqrd} };
};

inline cv::Mat str2mat(const std::string& s, int rows,
    int type = CV_64F, int channels = 1) {
  std::string input = s;
  auto it = std::remove_if(std::begin(input), std::end(input),
      [](char c) { return (c == ',' || c == ';' || c == ':'); });
  input.erase(it, std::end(input));

  cv::Mat mat(0, 0, type);
  std::istringstream iss(input);
  double currNum;
  while (!iss.eof()) {
    iss >> currNum;
    mat.push_back(currNum);
  }
  return mat.reshape(channels, rows);
};

inline std::vector<double> str2doublesVec(const std::string& doublesStr) {
  std::list<std::string> tokens;
  boost::split(tokens, doublesStr, boost::is_any_of(", "), boost::token_compress_on);
  std::vector<double> doublesVec;
  for (const std::string& t: tokens) {
    doublesVec.push_back(boost::lexical_cast<double>(t));
  }
  return doublesVec;
};

/**
 * Check if 2 (convex) polygons overlap, using the dividing axis algorithm:
 * - if 2 convex polygons do not intersect, then there exists a line that passes between them
 * - such a line only exists if formed by one of the polygons' sides
 *
 * Note that two polygons sharing an edge, or whose one's endpoint intersects
 * the other's edge, are considered to be overlapping.
 */
inline bool checkPolygonOverlap(const std::vector<cv::Point2f>& cornersA, const std::vector<cv::Point2f>& cornersB) {
  // Compute angles perpendicular to each of the polygons' sides
  std::vector<double> projectionAngles;
  unsigned int i, j;
  cv::Point2f vec;
  for (i = 0; i < cornersA.size(); i++) {
    j = (i == 0) ? cornersA.size() - 1 : i - 1;
    vec = cornersA[i] - cornersA[j];
    if (vec.x == 0 && vec.y == 0) continue;
    projectionAngles.push_back(atan2(vec.x, vec.y)); // NOTE: (x, y) arguments swapped to compute perpendicular angle
  }
  for (i = 0; i < cornersB.size(); i++) {
    j = (i == 0) ? cornersB.size() - 1 : i - 1;
    vec = cornersB[i] - cornersB[j];
    if (vec.x == 0 && vec.y == 0) continue;
    projectionAngles.push_back(atan2(vec.x, vec.y));
  }

  // Scan for dividing axis line
  bool overlap = true;
  for (const double& angle: projectionAngles) {
    double projAMin = std::numeric_limits<double>::infinity();
    double projAMax = -std::numeric_limits<double>::infinity();
    double projBMin = std::numeric_limits<double>::infinity();
    double projBMax = -std::numeric_limits<double>::infinity();
    double cosAngle = cos(angle);
    double sinAngle = sin(angle);
    for (const cv::Point2f& pt: cornersA) {
      double proj = cosAngle * pt.x - sinAngle * pt.y;
      if (proj < projAMin) projAMin = proj;
      if (proj > projAMax) projAMax = proj;
    }
    for (const cv::Point2f& pt: cornersB) {
      double proj = cosAngle * pt.x - sinAngle * pt.y;
      if (proj < projBMin) projBMin = proj;
      if (proj > projBMax) projBMax = proj;
    }
    if ((projAMax < projBMin) || (projAMin > projBMax)) {
      overlap = false;
      break;
    }
  }

  return overlap;
};

};

#endif /* VECTORANDCIRCULARMATH_HPP_ */


/*
 * determinant.hh
 *
 *  Created on: Sep 21, 2011
 *      Author: bzflubko
 */

#ifndef DETERMINANT_HH_
#define DETERMINANT_HH_

#include <dune/common/fmatrix.hh>
#include <boost/static_assert.hpp>
#include "utilities/get.hh"
#include "fem/fixdune.hh"

/// \cond internals
namespace DeterminantDetail {

  template <typename MatrixType>
  Dune::FieldMatrix<double,MatrixType::rows-1,MatrixType::cols-1> getMinor(MatrixType const& matrix, int rowIndex, int colIndex)
  {
    BOOST_STATIC_ASSERT( (MatrixType::rows > 1) && (MatrixType::cols > 1) );
    Dune::FieldMatrix<double,MatrixType::rows-1,MatrixType::cols-1> subMatrix(0);
    for(int ii=0, iii=0; ii<MatrixType::rows; ++ii)
    {
      if(ii!=rowIndex)
      {
        for(int kk=0, kkk=0; kk<MatrixType::cols; ++kk)
        {
          if(kk!=colIndex)
          {
            subMatrix[iii][kkk]=matrix[ii][kk];
            ++kkk;
          }
        }
        ++iii;
      }
    }

    return subMatrix;
  }

}
/// \endcond

namespace Kaskade {

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/* DETERMINANT                                                                                */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

/**
 * \ingroup linalgbasic
 * \brief Class for computing determinants of matrices and (directional) derivatives thereof.
 *
 * On construction, the Determinant class takes a reference to the matrix (or a functor returning the matrix) and computes
 * determinants and derivatives on demand. That is, if the referenced matrix is modified, the modification is reflected
 * in the next determinant computation.
 */
template <int dim, class Source=Dune::FieldMatrix<double,dim,dim> >
class Determinant;

template <class Source>
class Determinant<2,Source>
{
public:
  typedef Source source_type;
  typedef Dune::FieldMatrix<double,2,2> MatrixType;

private:
  typedef Get<MatrixType,Source> GetMatrix;

  double composeResult(MatrixType const& m1, MatrixType const& m2) const { return m1[0][0]*m2[1][1] + m1[1][1]*m2[0][0] - (m1[0][1]*m2[1][0] + m1[1][0]*m2[0][1]); }

public:
  /**
   * \brief Constructor.
   *
   * \param s reference to the source of the matrix of which the determinant is to be computed. This can either
   *          be a Dune::FieldMatrix<double,n,n> itself or a functor returning such a matrix. Note that the source
   *          is held by reference and needs to exist as long as the Determinant object exists.
   */
  explicit Determinant(Source& s) : matrix(s){}

  double operator()() const { return d0(); }

  double d0() const { return matrix()[0][0]*matrix()[1][1] - matrix()[0][1]*matrix()[1][0]; }

  double d1(MatrixType const& dA) const { return composeResult(matrix(),dA); }

  double d2(MatrixType const& dA, MatrixType const& dB) const { return composeResult(dB,dA); }

  double d3(MatrixType const&, MatrixType const&, MatrixType const&) const { return 0; }

private:
  GetMatrix matrix;
  //MatrixType &matrix;
};

/// Determinant
template <class Source>
class Determinant<3,Source>
{
public:
  typedef Source source_type;
  typedef Dune::FieldMatrix<double,3,3> MatrixType;

private:
  typedef Get<MatrixType,Source> GetMatrix;

  template<int a1, int a2, int b1, int b2, int c1, int c2> double tp(MatrixType const& t) const
  {
    return t[a1][a2]*t[b1][b2]*t[c1][c2];
  }

  template<int a1, int a2, int b1, int b2, int c1, int c2> double circular(MatrixType const& t, MatrixType const& u, MatrixType const& v) const
  {
    return t[a1][a2]*u[b1][b2]*v[c1][c2]+v[a1][a2]*t[b1][b2]*u[c1][c2]+u[a1][a2]*v[b1][b2]*t[c1][c2];
  }

  template<int a1, int a2, int b1, int b2, int c1, int c2> double dtp(MatrixType const& t, MatrixType const& dv) const
  {
    return circular<a1,a2,b1,b2,c1,c2>(t,t,dv);
  }

  template<int a1, int a2, int b1, int b2, int c1, int c2> double ddtp(MatrixType const& t, MatrixType const& dv, MatrixType const& dw) const
  {
    return circular<a1,a2,b1,b2,c1,c2>(t,dv,dw)+circular<a1,a2,b1,b2,c1,c2>(t,dw,dv);
  }

  template<int a1, int a2, int b1, int b2, int c1, int c2> double dddtp(MatrixType const& du, MatrixType const& dv, MatrixType const& dw) const
  {
    return circular<a1,a2,b1,b2,c1,c2>(du,dv,dw)+circular<a1,a2,b1,b2,c1,c2>(du,dw,dv);
  }

public:
  /**
   * \brief Constructor.
   *
   * \param s reference to the source of the matrix of which the determinant is to be computed. This can either
   *          be a Dune::FieldMatrix<double,n,n> itself or a functor returning such a matrix. Note that the source
   *          is held by reference and needs to exist as long as the Determinant object exists.
   */
  explicit Determinant(Source &s): matrix(s){}

  double operator()() const { return d0(); }

  double d0() const
  {
    return tp<0,0,1,1,2,2>(matrix())+tp<0,1,1,2,2,0>(matrix())+tp<0,2,1,0,2,1>(matrix())
        - tp<2,0,1,1,0,2>(matrix())-tp<2,1,1,2,0,0>(matrix())-tp<2,2,1,0,0,1>(matrix());
  }

  double d1(MatrixType const& dv) const
  {
    return dtp<0,0,1,1,2,2>(matrix(),dv)+dtp<0,1,1,2,2,0>(matrix(),dv)+dtp<0,2,1,0,2,1>(matrix(),dv)
        -dtp<2,0,1,1,0,2>(matrix(),dv)-dtp<2,1,1,2,0,0>(matrix(),dv)-dtp<2,2,1,0,0,1>(matrix(),dv);
  }

  double d2(MatrixType const& dv, MatrixType const& dw) const
  {
    return ddtp<0,0,1,1,2,2>(matrix(),dv,dw)+ddtp<0,1,1,2,2,0>(matrix(),dv,dw)+ddtp<0,2,1,0,2,1>(matrix(),dv,dw)
        - ddtp<2,0,1,1,0,2>(matrix(),dv,dw)-ddtp<2,1,1,2,0,0>(matrix(),dv,dw)-ddtp<2,2,1,0,0,1>(matrix(),dv,dw);
  }

  double d3(MatrixType const& du, MatrixType const& dv, MatrixType const& dw) const
  {
    return dddtp<0,0,1,1,2,2>(du,dv,dw)+dddtp<0,1,1,2,2,0>(du,dv,dw)+dddtp<0,2,1,0,2,1>(du,dv,dw)
        - dddtp<2,0,1,1,0,2>(du,dv,dw)-dddtp<2,1,1,2,0,0>(du,dv,dw)-dddtp<2,2,1,0,0,1>(du,dv,dw);
  }

private:
  GetMatrix matrix;
//  MatrixType &matrix;
};

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/* ADJUGATE                                                                                */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
template <int dim, class Source=Dune::FieldMatrix<double,dim,dim> > class Adjugate;

template <int dim, class Source=Dune::FieldMatrix<double,dim,dim> > class Cofactor;

template <class Source>
class Adjugate<2, Source>{
public:
  typedef Dune::FieldMatrix<double,2,2> MatrixType;
private:
  typedef Get<MatrixType,Source> GetMatrix;

  void composeResult(MatrixType const& m) const
  {
    result[0][0] = m[1][1]; result[0][1] = -m[1][0];
    result[1][0] = -m[0][1]; result[1][1] = m[0][0];
  }

public:
  explicit Adjugate(Source &s) : matrix(s), result(0){}

  MatrixType d0() const
  {
    composeResult(matrix());
    return result;
  }

  MatrixType d1(MatrixType const& dA) const
  {
    composeResult(dA);
    return result;
  }

  MatrixType d2(MatrixType const&, MatrixType const&) const { return 0.0*result; }

  MatrixType d3(MatrixType const&, MatrixType const&, MatrixType const&) { return 0.0*result; }

  //Cofactor<2,Source> transpose() const { return Cofactor<2,Source>(matrix); }

private:
  GetMatrix matrix;
//  MatrixType &matrix;
  mutable MatrixType result;
};

/**
 * Adjugate of a 3x3-matrix
 */
template <class Source>
class Adjugate<3,Source>
{
public:
  enum{ dim=3 };
  typedef Dune::FieldMatrix<double,dim,dim> MatrixType;
  typedef typename GetSubType<double,dim,MatrixType>::type SubMatrixType;
private:
  typedef Get<MatrixType,Source> GetMatrix;

public:
  explicit Adjugate(Source &s) :matrix(s){}

  MatrixType d0() const
  {
    for(int i=0; i<dim; ++i)
      for(int j=0; j<dim; ++j)
      {
        SubMatrixType tmp = DeterminantDetail::getMinor(matrix(),i,j);
        int sign = 1;
        if((i+j)%2) sign = -1;
        result[j][i] = sign*Determinant<dim-1,SubMatrixType>(tmp).d0();
      }
    return result;
  }

  MatrixType d1(MatrixType const& dv) const{
    for(int i=0; i<dim; ++i){
      for(int j=0; j<dim; ++j){
        int i1 = (i+1)%dim; int i2 = (i+2)%dim;
        int j1 = (j+1)%dim; int j2 = (j+2)%dim;
        result[j][i] = matrix()[i1][j1]*dv[i2][j2] + matrix()[i2][j2]*dv[i1][j1] -
            matrix()[i1][j2]*dv[i2][j1] - matrix()[i2][j1]*dv[i1][j2];
        //result[j][i] *= ((i+j)%2==0) ? 1. : -1.;
      }
    }
    return result;
  }

  MatrixType d2(MatrixType const& dv, MatrixType const& dw) const {
    result = MatrixType(0);
    for(int i=0; i<dim; ++i){
      for(int j=0; j<dim; ++j){
        int i1 = (i+1)%dim; int i2 = (i+2)%dim;
        int j1 = (j+1)%dim; int j2 = (j+2)%dim;

        result[j][i] += dv[i2][j2]*dw[i1][j1] + dv[i1][j1]*dw[i2][j2];
        result[j][i] -= dv[i2][j1]*dw[i1][j2] + dv[i1][j2]*dw[i2][j1];
        //result[j][i] *= ((i+j)%2==0) ? 1. : -1.;
      }
    }

    return result;
  }

  MatrixType d3(MatrixType const&, MatrixType const&, MatrixType const&) const
  {
    return 0.0*matrix();
  }

  //Cofactor<3,Source> transpose() const { return Cofactor<3,Source>(matrix); }

private:
  GetMatrix matrix;
//  MatrixType &matrix;
  mutable MatrixType result;
};

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/* Cofactor matrix                                                                                */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/**
 * implemented as adjugate transposed
 */
template <int dimension, class Source>
class Cofactor{
public:
  enum{ dim=dimension };
  typedef Dune::FieldMatrix<double,dim,dim> MatrixType;
  typedef Dune::FieldMatrix<double,dim-1,dim-1> SubMatrixType;
private:
  typedef Get<MatrixType,Source> GetMatrix;

public:
  explicit Cofactor(Source& s) : adj(s){}

  MatrixType const d0() const { return transpose( adj.d0() ); }

  MatrixType const d1(MatrixType const& dv) const { return transpose( adj.d1(dv) ); }

  MatrixType const d2(MatrixType const& dv, MatrixType const& dw) const
  { return transpose( adj.d2(dv,dw) ); }

  MatrixType const d3(MatrixType const& dv, MatrixType const& dw, MatrixType const& dx) const
  { return transpose( adj.d3(dv,dw,dx) ); }

  //Adjugate<dim,Source> transpose() const { return adj; }

private:
  Adjugate<dim,Source> const adj;
};

} // end of namespace Kaskade

#endif /* DETERMINANT_HH_ */


#include "sparse_solver.hpp"
#include <Eigen/Sparse>
#include <iostream>
#include <stdexcept>

std::unique_ptr<sparse_solver_interface> make_sparse_solver(
    const std::string& type, const std::string& preconditioner) {
  using std::make_unique;
  using namespace Eigen;

  if (type == "BiCGSTAB") {
    if (preconditioner == "IncompleteLUT")
      return make_unique<sparse_solver<
          BiCGSTAB<SparseMatrix<double>, IncompleteLUT<double>>>>();
    else if (preconditioner == "Diagonal")
      return make_unique<sparse_solver<
          BiCGSTAB<SparseMatrix<double>, DiagonalPreconditioner<double>>>>();
    else if (preconditioner == "Identity")
      return make_unique<sparse_solver<
          BiCGSTAB<SparseMatrix<double>, IdentityPreconditioner>>>();
    else if (preconditioner == "SimplicialCholesky")
      return make_unique<sparse_solver<BiCGSTAB<
          SparseMatrix<double>, SimplicialCholesky<SparseMatrix<double>>>>>();
    else {
      std::stringstream ss;
      ss << "Error: " << __FILE__ << ": " << __LINE__;
      if (preconditioner.empty())
        ss << "\n  Empty preconditioner found\n";
      else
        ss << "\n  Invalid preconditioner found: " << preconditioner << "\n";
      ss << "  Valid preconditioners are:\n"
         << "    SimplicialCholesky,\n"
         << "    IncompleteLUT,\n"
         << "    Diagonal,\n"
         << "    Identity" << std::endl;
      throw std::invalid_argument(ss.str());
    }
  } else if (type == "SparseQR") {
    return make_unique<
        sparse_solver<SparseQR<SparseMatrix<double>, COLAMDOrdering<int>>>>();
  } else if (type == "SparseLU")
    return make_unique<sparse_solver<SparseLU<SparseMatrix<double>>>>();
  else {
    std::stringstream ss;
    ss << "Error: " << __FILE__ << ": " << __LINE__
       << "\n  Invalid sparse solver type found: " << type
       << "\n  Valid sparse solver types are:\n"
       << "    BiCGSTAB,\n"
       << "    SparseQR,\n"
       << "    SparseLU" << std::endl;
    throw std::invalid_argument(ss.str());
  }
}


/*=============================================================================
Copyright 2020 Syed Ali Hasan <alihasan9922@gmail.com>

Distributed under the Boost Software License, Version 1.0. (See accompanying
file License.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/

#include <iostream>
#include <stack>

//Graph
#include <boost/graph/graphviz.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/named_function_params.hpp>

//Matrix
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/astronomy/coordinate/utility/utility.hpp>

//Angle
#include <boost/units/io.hpp>
#include <boost/units/systems/angle/degrees.hpp>
#include <boost/units/systems/si/plane_angle.hpp>

namespace bu = boost::units;
namespace bg = boost::geometry;
namespace bac = boost::astronomy::coordinate;

using namespace boost::units;
using namespace boost::units::si;
using namespace boost::astronomy::coordinate;

enum class COORDINATE_SYSTEM {
  HORIZON,
  EQUATORIAL_HA_DEC,
  EQUATORIAL_RA_DEC,
  ECLIPTIC,
  GALACTIC};

struct CoordinateData
{
  COORDINATE_SYSTEM coordinate_system;
  std::string coordinate_name;
};

struct EdgeData
{
  std::string edge_label;
  matrix<double> conv_matrix;
};

using graph_t = boost::adjacency_list<boost::vecS, boost::vecS,
    boost::directedS,
    CoordinateData,
    EdgeData>;

using vertex_t = boost::graph_traits<graph_t>::vertex_descriptor;

template
<
  typename CoordinateType = double,
  typename Angle = bu::quantity<bu::si::plane_angle, CoordinateType>
>
matrix<double> convert(const COORDINATE_SYSTEM src,
                       const COORDINATE_SYSTEM dest,
                       const Angle& phi,
                       const Angle& st,
                       const Angle& obliquity,
                       coord_sys<2, bg::cs::spherical<bg::radian>, CoordinateType> source_coordinate)
{
  bac::column_vector<double,
      quantity<bu::si::plane_angle, double>,
      double>
      col_vec(bg::get<0>(source_coordinate.get_point()) * radians, bg::get<1>(source_coordinate.get_point()) * radians);

  graph_t G;

  const int graph_size = 5;

  vertex_t vd0 = boost::add_vertex(CoordinateData{COORDINATE_SYSTEM::HORIZON,"Horizon"}, G);
  vertex_t vd1 = boost::add_vertex(CoordinateData{COORDINATE_SYSTEM::EQUATORIAL_HA_DEC,"Equatorial_HA_Dec"}, G);
  vertex_t vd2 = boost::add_vertex(CoordinateData{COORDINATE_SYSTEM::EQUATORIAL_RA_DEC,"Equatorial_RA_Dec"}, G);
  vertex_t vd3 = boost::add_vertex(CoordinateData{COORDINATE_SYSTEM::ECLIPTIC,"Ecliptic"}, G);
  vertex_t vd4 = boost::add_vertex(CoordinateData{COORDINATE_SYSTEM::GALACTIC,"Galactic"}, G);

  boost::add_edge(vd0, vd1, EdgeData{"Horizon to Equatorial HA Dec", bac::ha_dec_horizon<double, quantity<bu::degree::plane_angle>, double>(phi).get()}, G);
  boost::add_edge(vd1, vd0, EdgeData{"Equatorial HA Dec to Horizon", bac::ha_dec_horizon<double, quantity<bu::degree::plane_angle>, double>(phi).get()}, G);
  boost::add_edge(vd1, vd2, EdgeData{"Equatorial HA Dec to Equatorial RA Dec", bac::ha_dec_ra_dec<double, quantity<bu::degree::plane_angle>, double>(st).get()}, G);
  boost::add_edge(vd2, vd1, EdgeData{"Equatorial RA Dec to Equatorial HA Dec", bac::ha_dec_ra_dec<double, quantity<bu::degree::plane_angle>, double>(st).get()}, G);
  boost::add_edge(vd2, vd3, EdgeData{"Equatorial RA Dec to Ecliptic", bac::ra_dec_to_ecliptic<double, quantity<bu::degree::plane_angle>, double>(obliquity).get()}, G);
  boost::add_edge(vd3, vd2, EdgeData{"Ecliptic to Equatorial RA Dec", bac::ecliptic_to_ra_dec<double, quantity<bu::degree::plane_angle>, double>(obliquity).get()}, G);
  boost::add_edge(vd2, vd4, EdgeData{"Equatorial RA Dec to Galactic", bac::ra_dec_to_galactic<double>().get()}, G);
  boost::add_edge(vd4, vd2, EdgeData{"Galactic to Equatorial RA Dec", bac::galactic_to_ra_dec<double>().get()}, G);

  //Predecessor Array
  boost::array<int, graph_size> predecessors{0};
  predecessors[(int)src] = (int)src;

  boost::breadth_first_search(G, (int)src, boost::visitor(
      boost::make_bfs_visitor(
          boost::record_predecessors(predecessors.begin(),
                                     boost::on_tree_edge{}))));

  //Get traversed path
  std::vector<int> store_path;

  int p = (int)dest;
  while (p != (int)src)
  {
    store_path.push_back(p);
    p = predecessors[p];
  }
  store_path.push_back(p);

  matrix<double> ans = col_vec.get();

  //Matrix Multiplication
  for(auto it = store_path.rbegin(); it + 1 != store_path.rend(); ++it)
    ans = prod(G[boost::edge(*it,*(it+1),G).first].conv_matrix, ans);

  return ans;
}


// BLAS level 3
// hermitian matrix (kind of ;o)

#include <stddef.h>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/blas.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/hermitian.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace blas = boost::numeric::bindings::blas;
namespace bindings = boost::numeric::bindings;
namespace tag = boost::numeric::bindings::tag;

using std::cout;
using std::cin;
using std::endl;

typedef double real_t;
typedef std::complex<real_t> cmplx_t;

typedef ublas::matrix<cmplx_t, ublas::column_major> cm_t;
typedef ublas::matrix<cmplx_t, ublas::row_major> rm_t;

typedef ublas::hermitian_adaptor<cm_t, ublas::upper> uchemm_t;
typedef ublas::hermitian_adaptor<cm_t, ublas::lower> lchemm_t;
typedef ublas::hermitian_adaptor<rm_t, ublas::upper> urhemm_t;
typedef ublas::hermitian_adaptor<rm_t, ublas::lower> lrhemm_t;

int main (int argc, char **argv) {
  size_t n = 0;
  if (argc > 1) {
    n = atoi(argv [1]);
  }

  cout << endl;

  if (n <= 0) {
    cout << "n -> ";
    cin >> n;
  }
  cout << endl;

  cm_t uc (n, n);
  uchemm_t ucs (uc);
  cm_t lc (n, n);
  lchemm_t lcs (lc);
  rm_t ur (n, n);
  urhemm_t urs (ur);
  rm_t lr (n, n);
  lrhemm_t lrs (lr);

  init_symm (ucs, 'u');
  init_symm (lcs, 'l');
  init_symm (urs, 'u');
  init_symm (lrs, 'l');

  print_m (ucs, "a == ucs");
  cout << endl;
  print_m_data (ucs, "ucs");
  cout << endl;

  print_m (lcs, "a == lcs");
  cout << endl;
  print_m_data (lcs, "lcs");
  cout << endl;

  print_m (urs, "a == urs");
  cout << endl;
  print_m_data (urs, "urs");
  cout << endl;

  print_m (lrs, "a == lrs");
  cout << endl;
  print_m_data (lrs, "lrs");
  cout << endl;

  cm_t cbl (n, n+1);
  rm_t rbl (n, n+1);
  cm_t ccl (n, n+1);
  rm_t rcl (n, n+1);

  init_m (cbl, rws1());
  init_m (rbl, rws1());

  print_m (cbl, "b == cb");
  cout << endl;
  print_m (rbl, "b == rb");
  cout << endl;

  blas::hemm ( tag::left(), 1.0, ucs, cbl, 0.0, ccl);
  print_m (ccl, "c = a b");
  cout << endl;

  blas::hemm ( tag::left(), 1.0, lcs, cbl, 0.0, ccl);
  print_m (ccl, "c = a b");
  cout << endl;

  blas::hemm ( tag::left(), 1.0, urs, rbl, 0.0, rcl);
  print_m (rcl, "c = a b");
  cout << endl;

  blas::hemm ( tag::left(), 1.0, lrs, rbl, 0.0, rcl);
  print_m (rcl, "c = a b");
  cout << endl;

  cm_t cbr (n+1, n);
  rm_t rbr (n+1, n);
  cm_t ccr (n+1, n);
  rm_t rcr (n+1, n);

  init_m (cbr, cls1());
  init_m (rbr, cls1());

  print_m (cbr, "b == cb");
  cout << endl;
  print_m (rbr, "b == rb");
  cout << endl;

  blas::hemm ( tag::right(), 1.0, ucs, cbr, 0.0, ccr);
  print_m (ccr, "c = b a");
  cout << endl;

  blas::hemm ( tag::right(), 1.0, lcs, cbr, 0.0, ccr);
  print_m (ccr, "c = b a");
  cout << endl;

  blas::hemm ( tag::right(), 1.0, urs, rbr, 0.0, rcr);
  print_m (rcr, "c = b a");
  cout << endl;

  blas::hemm ( tag::right(), 1.0, lrs, rbr, 0.0, rcr);
  print_m (rcr, "c = b a");
  cout << endl;

  cout << endl;

}


/*

	This file is part of PEST++.

	PEST++ 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 3 of the License, or
	(at your option) any later version.

	PEST++ 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.

	You should have received a copy of the GNU General Public License
	along with PEST++.  If not, see<http://www.gnu.org/licenses/>.
*/
#include "eigen_tools.h"
#include <Eigen/Dense>
#include "Transformable.h"
#include <vector>
#include <set>
#include <algorithm>
#include <iomanip>
#include <string>
#include <cstdio>
#include <cstdint>

using namespace Eigen;
using namespace std;

void get_MatrixXd_row_abs_max(const MatrixXd &m, int row, int *max_col, double *max_val)
{
	size_t nrows = m.rows();
	size_t ncols = m.cols();
	assert(row <= nrows);

	*max_col = -999;
	*max_val = 0;
	for (size_t icol=0; icol<ncols; ++icol)
	{
		if(abs(m(row,icol)) > abs(*max_val))
		{
			*max_col = icol;
			*max_val = m(row,icol);
		}
	}
}

VectorXd stlvec_2_egienvec(const std::vector<double> &stl_vec)
{
	size_t len = stl_vec.size();
	VectorXd la_vec(len);
	for (size_t i=0; i<len; ++i)
	{
		la_vec(i) = stl_vec[i];
	}
	return la_vec;
}

vector<double> egienvec_2_stlvec(const VectorXd &eigen_vec)
{
	size_t len = eigen_vec.size();
	vector<double> stl_vec;
	stl_vec.reserve(len);
	for (size_t i = 0; i<len; ++i)
	{
		stl_vec.push_back(eigen_vec(i));
	}
	return stl_vec;
}

Eigen::VectorXd transformable_2_egien_vec(const Transformable &data, vector<string> oredered_names)
{
	size_t len = oredered_names.size();
	VectorXd new_vec(len);
	auto data_end = data.end();

	for (size_t i = 0; i<len; ++i)
	{
		const auto &it = data.find(oredered_names[i]);
		assert(it != data_end);
		new_vec(i) = it->second;
	}
	return new_vec;
}

void matrix_del_cols(Eigen::SparseMatrix<double> &mat, const vector<size_t> &col_id_vec)
{
	if (!col_id_vec.empty())
	{
		size_t ncols = mat.cols();
		set<int> del_col_set(col_id_vec.begin(), col_id_vec.end());

		std::vector<Eigen::Triplet<int> > triplet_list;

		// add rows to be retained to the beginning of the  permuatation matrix
		int icol_new = 0;
		int n_col_save = 0;
		for (int icol_old = 0; icol_old<ncols; ++icol_old)
		{
			if (del_col_set.find(icol_old) == del_col_set.end())
			{
				triplet_list.push_back(Eigen::Triplet<int>(icol_old, icol_new, 1));
				++icol_new;
				++n_col_save;
			}
		}
		// add rows to be deleted to end to permuatation matrix
		for (int icol_old :  col_id_vec)
		{
			triplet_list.push_back(Eigen::Triplet<int>(icol_old, icol_new, 1));
			++icol_new;
		}
		Eigen::SparseMatrix<double> perm_sparse_matrix(ncols, ncols);
		perm_sparse_matrix.setZero();
		perm_sparse_matrix.setFromTriplets(triplet_list.begin(), triplet_list.end());
		Eigen::SparseMatrix<double> new_matrix = mat * perm_sparse_matrix;
		mat = new_matrix.leftCols(n_col_save);
	}
}

Eigen::SparseMatrix<double> get_diag_matrix(const Eigen::SparseMatrix<double> &mat)
{
	auto  ncols = mat.cols();
	auto  nrows = mat.rows();

	assert(ncols == nrows);
	if (nrows != ncols)
	{
		PestError("Error in get_diag_matrix: Can not return the diagonal of a non-square matrix");
	}
	VectorXd diag_vec = mat.diagonal();
	std::vector<Eigen::Triplet<double> > triplet_list;
	triplet_list.reserve(nrows);
	for (size_t i = 0; i<nrows; ++i)
	{
		triplet_list.push_back(Eigen::Triplet<double>(i, i, diag_vec(i)));
	}

	Eigen::SparseMatrix<double> diag_matrix;
	diag_matrix.resize(nrows, ncols);
	diag_matrix.setZero();
	diag_matrix.setFromTriplets(triplet_list.begin(), triplet_list.end());
	return diag_matrix;
}

void print(const MatrixXd &mat, ostream & fout)
{
	size_t nrows = mat.rows();
	size_t ncols = mat.cols();

	for (size_t i=0; i<nrows; ++i)
	{
		for (size_t j=0; j<ncols; ++j)
		{
			fout << mat(i,j);
			if (j < ncols-1) {fout << ", ";}
		}
		fout << endl;
	}

}

void print(const MatrixXd &mat, ostream & fout, int n_per_line)
{
	size_t nrows = mat.rows();
	size_t ncols = mat.cols();

	for (size_t i=0; i<nrows; ++i)
	{
		for (size_t j=0; j<ncols; ++j)
		{
			fout << setw(15) << setiosflags(ios::right) << mat(i,j);
			if ((j+1) % (n_per_line) == 0 || j+1==ncols)
			{
				fout << '\n';
			}
		}
		fout << endl;
	}

}

void print(const VectorXd &vec, ostream & fout, int n_per_line)
{
	size_t n = vec.size();

	for (size_t i=0; i<n; ++i)
	{
		fout << showpoint << setw(15) << setiosflags(ios::right) << vec(i);
			if ((i+1) % (n_per_line) == 0 || i+1==n)
		{
			fout << '\n';
		}

	}
}

bool save_triplets_bin(const SparseMatrix<double> &mat, ostream &fout)
{
	int32_t xyn[3] = { mat.rows(), mat.cols(), mat.nonZeros() };
	fout.write((char*)xyn, sizeof(xyn));

	for (int k = 0; k < mat.outerSize(); ++k)
	{
		SparseMatrix<double>::InnerIterator it(mat, k);
		for (; it; ++it)
		{
			int32_t rc[2] = { it.row(), it.col() };
			fout.write((char*)rc, sizeof(rc));
			double v = it.value();
			fout.write((char*)&v, sizeof(v));
		}
	}
	return true;
}

bool save_vector_bin(const VectorXd &vec, ostream &fout)
{
	int32_t size = vec.size();
	//vector<double> buf = egienvec_2_stlvec(vec);
	fout.write((char*)&size, sizeof(size));
	fout.write((char*)vec.data(), sizeof(double)*size);
	return true;
}

bool load_vector_bin(VectorXd &vec, istream &fin)
{
	int32_t size = 0;
	fin.read((char*)&size, sizeof(size));
	vec.resize(size);
	fin.read((char*)vec.data(), sizeof(double)*size);
	return true;
}

bool load_triplets_bin(SparseMatrix<double> &a, istream &fin)
{
	int32_t xyn[3];
	fin.read((char*)xyn, sizeof(xyn));
	a.resize(xyn[0], xyn[1]);
	vector<Triplet<double>> trips(xyn[2]);

	for (int k = 0; k < trips.size(); ++k)
	{
		int32_t rc[2];
		fin.read((char*)rc, sizeof(rc));
		double v;
		fin.read((char*)&v, sizeof(v));

		trips[k] = Triplet<double>(rc[0], rc[1], v);
	}
	a.setFromTriplets(trips.begin(), trips.end());
	return true;
}

Eigen::SparseMatrix<double> eigenvec_2_diagsparse(Eigen::VectorXd vec)
{
	vector<Eigen::Triplet<double>> triplet_list;
	for (int i = 0; i != vec.size(); i++)
		triplet_list.push_back(Eigen::Triplet<double>(i, i, vec[i]));
	Eigen::SparseMatrix<double> mat(vec.size(), vec.size());
	mat.setZero();
	mat.setFromTriplets(triplet_list.begin(), triplet_list.end());
	return mat;

}


#pragma once
#include <algorithm>

#include <boost/variant/static_visitor.hpp>
#include <boost/format.hpp>

#include "expression.hpp"
#include "visitors.hpp"

namespace metaSMT {
  namespace expression {
    inline logic_expression simplify( logic_expression e );

    struct simplify_visitor : public boost::static_visitor< logic_expression > {
      simplify_visitor() {}

      logic_expression operator() ( bool b ) const {
        return b;
      }

      logic_expression operator() ( logic::predicate p ) const {
        return p;
      }

      logic_expression operator() ( unary_expression<logic_tag, predtags::not_tag> expr ) const {
        logic_expression arg = simplify(expr.expr);

        if ( has_type<bool>( arg ) ) {
          bool b = boost::get<bool>( arg );
          return !b;
        }

        if ( has_type< unary_expression<logic_tag, predtags::not_tag> >(arg) ) {
          unary_expression<logic_tag, predtags::not_tag> not_arg = boost::get<unary_expression<logic_tag, predtags::not_tag> >(arg);
          return simplify(not_arg.expr);
        }

        return unary_expression<logic_tag, predtags::not_tag>(arg);
      }

      logic_expression operator() ( binary_expression<logic_tag, predtags::equal_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        // ( expr e == expr e ) ==> true
        if ( lhs == rhs ) {
          return true;
        }

        return binary_expression<logic_tag, predtags::equal_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<logic_tag, predtags::nequal_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        // ( expr e != expr e ) ==> false
        if ( lhs == rhs ) {
          return false;
        }

        return binary_expression<logic_tag, predtags::nequal_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<logic_tag, predtags::implies_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        // true  -> predicate p ==> predicate p
        // false -> predicate p ==> true
        if ( has_type<bool>( lhs ) ) {
          bool b = boost::get<bool>(lhs);
          if ( b ) {
            return rhs;
          } else {
            return true;
          }
        }

        // predicate p --> true  ==> true
        // predicate p --> false ==> !(predicate p)
        if ( has_type< bool >( rhs ) ) {
          bool b = boost::get<bool>( rhs );
          if ( b ) {
            return true;
          } else {
            return unary_expression<logic_tag, predtags::not_tag>(lhs);
          }
        }

        // ( expr e -> expr e ) ==> true
        if ( lhs == rhs ) {
          return true;
        }

        return binary_expression<logic_tag, predtags::implies_tag>(lhs, rhs);
      }

      logic_expression operator() ( nary_expression<logic_tag, predtags::and_tag> expr ) const {
        typedef nary_expression<logic_tag, predtags::and_tag> nary_and;
        nary_and::ContainerType v;
        for (nary_and::ContainerType::const_iterator it = expr.begin(), ie = expr.end();
             it != ie; ++it) {
          logic_expression e = simplify( *it );
          if ( has_type< bool >( e ) ) {
            bool b = boost::get<bool>( e );
            if ( b ) {
              // skip trues
              continue;
            } else {
              // simplify to false
              return false;
            }
          }

          // merge with child
          if ( has_type<nary_and>(e) ) {
            nary_and child = boost::get<nary_and>(e);
            v.insert( v.end(), child.begin(), child.end() );
            continue;
          }

          v.push_back( e );
        }

        unsigned const size = v.size();
        if (size == 0) {
          return true;
        }
        else if (size == 1) {
          return v[0];
        }

        return nary_and(v);
      }

      logic_expression operator() ( nary_expression<logic_tag, predtags::or_tag> expr ) const {
        typedef nary_expression<logic_tag, predtags::or_tag> nary_or;
        nary_or::ContainerType v;
        for (nary_or::ContainerType::const_iterator it = expr.begin(), ie = expr.end();
             it != ie; ++it) {
          logic_expression e = simplify( *it );
          if ( has_type< bool >( e ) ) {
            bool b = boost::get<bool>( e );
            if ( b ) {
              // simplify to true
              return true;
            } else {
              // skip falses
              continue;
            }
          }

          // merge with child
          if ( has_type<nary_or>(e) ) {
            nary_or child = boost::get<nary_or>(e);
            v.insert( v.end(), child.begin(), child.end() );
            continue;
          }

          v.push_back( e );
        }

        unsigned const size = v.size();
        if (size == 0) {
          return false;
        }
        else if (size == 1) {
          return v[0];
        }

        return nary_or(v);
      }

      logic_expression operator() ( binary_expression<logic_tag, predtags::nand_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        // !( true  /\ expr ) ==> !(expr)
        // !( false /\ expr ) ==> true
        if ( has_type< bool >( lhs ) ) {
          bool b = boost::get<bool>( lhs );
          if ( b ) {
            return simplify( unary_expression<logic_tag, predtags::not_tag>(rhs) );
          } else {
            return true;
          }
        }

        // !( expr /\ true  ) ==> !(expr)
        // !( expr /\ false ) ==> true
        if ( has_type< bool >( rhs ) ) {
          bool b = boost::get<bool>( rhs );
          if ( b ) {
            return simplify( unary_expression<logic_tag, predtags::not_tag>(lhs) );
          } else {
            return true;
          }
        }

        // !( expr e /\ expr e ) ==> !( expr e )
        if ( lhs == rhs ) {
          return simplify( unary_expression<logic_tag, predtags::not_tag>(lhs) );
        }

        return binary_expression<logic_tag, predtags::nand_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<logic_tag, predtags::nor_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        // !( true  \/ expr ) ==> false
        // !( false \/ expr ) ==> !(expr)
        if ( has_type< bool >( lhs ) ) {
          bool b = boost::get<bool>( lhs );
          if ( b ) {
            return false;
          } else {
            return simplify( unary_expression<logic_tag, predtags::not_tag>(rhs) );
          }
        }

        // !( expr \/ true  ) ==> false
        // !( expr \/ false ) ==> !(expr)
        if ( has_type< bool >( rhs ) ) {
          bool b = boost::get<bool>( rhs );
          if ( b ) {
            return true;
          } else {
            return simplify( unary_expression<logic_tag, predtags::not_tag>(lhs) );
          }
        }

        // !( predicate p \/ predicate p ) ==> !( predicate p )
        if ( lhs == rhs ) {
          return simplify( unary_expression<logic_tag, predtags::not_tag>(lhs) );
        }

        return binary_expression<logic_tag, predtags::nor_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<logic_tag, predtags::xor_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        // ( true  xor expr ) ==> !(expr)
        // ( false xor expr ) ==> expr
        if ( has_type< bool >( lhs ) ) {
          bool b = boost::get<bool>( lhs );
          if ( b ) {
            return simplify( unary_expression<logic_tag, predtags::not_tag>(rhs) );
          } else {
            return rhs;
          }
        }

        // ( expr xor true  ) ==> !(expr)
        // ( expr xor false ) ==> expr
        if ( has_type< bool >( rhs ) ) {
          bool b = boost::get<bool>( rhs );
          if ( b ) {
            return simplify( unary_expression<logic_tag, predtags::not_tag>(lhs) );
          } else {
            return lhs;
          }
        }

        // ( expr e xor expr e ) ==> false
        if ( lhs == rhs ) {
          return false;
        }

        return binary_expression<logic_tag, predtags::xor_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<logic_tag, predtags::xnor_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        // !( true  xor expr ) ==> expr
        // !( false xor expr ) ==> !(expr)
        if ( has_type< bool >( lhs ) ) {
          bool b = boost::get<bool>( lhs );
          if ( b ) {
            return rhs;
          } else {
            return simplify( unary_expression<logic_tag, predtags::not_tag>(rhs) );
          }
        }

        // !( expr xor true  ) ==> expr
        // !( expr xor false ) ==> !(expr)
        if ( has_type< bool >( rhs ) ) {
          bool b = boost::get<bool>( rhs );
          if ( b ) {
            return lhs;
          } else {
            return simplify( unary_expression<logic_tag, predtags::not_tag>(lhs) );
          }
        }

        // !( expr e xor expr e ) ==> true
        if ( lhs == rhs ) {
          return true;
        }

        return binary_expression<logic_tag, predtags::xnor_tag>(lhs, rhs);
      }

      logic_expression operator() ( ternary_expression<logic_tag, predtags::ite_tag> expr ) const {
        logic_expression cond = simplify(expr.expr1);
        logic_expression true_expr = simplify(expr.expr2);
        logic_expression false_expr = simplify(expr.expr3);

        // ite( true,  t_expr, f_expr) ==> t_expr
        // ite( false, t_expr, f_expr) ==> f_expr
        if ( has_type< bool >( cond ) ) {
          bool b = boost::get<bool>( cond );
          if ( b ) {
            return true_expr;
          } else {
            return false_expr;
          }
        }

        // ite( cond, expr, expr ) ==> expr
        if ( true_expr == false_expr ) {
          return true_expr;
        }

        return ternary_expression<logic_tag, predtags::ite_tag>(cond, true_expr, false_expr);
      }

      logic_expression operator() ( bit0_const b ) const {
        return b;
      }

      logic_expression operator() ( bit1_const b ) const {
        return b;
      }

      template < typename OpTag >
      logic_expression operator() ( bv_const<OpTag> const &expr ) const {
        if (expr.width == 1) {
          if (expr.value == 0) {
            return logic::QF_BV::bit0;
          }
          else {
            return logic::QF_BV::bit1;
          }
        }
        return expr;
      }

      logic_expression operator() ( logic::QF_BV::bitvector bv ) const {
        return bv;
      }

      logic_expression operator() ( unary_expression<bv_tag, bvtags::bvnot_tag> expr ) const {
        logic_expression arg = simplify(expr.expr);
        // not( bit0 ) ==> bit1
        if ( has_type< bit0_const >( arg ) ) {
          return logic::QF_BV::bit1;
        }
        // not( bit1 ) ==> bit0
        else if ( has_type< bit1_const >( arg ) ) {
          return logic::QF_BV::bit0;
        }
        else if ( has_type< unary_expression<bv_tag, bvtags::bvnot_tag> >(arg) ) {
          unary_expression<bv_tag, bvtags::bvnot_tag> not_arg = boost::get<unary_expression<bv_tag, bvtags::bvnot_tag> >(arg);
          return simplify(not_arg.expr);
        }

        return unary_expression<bv_tag, bvtags::bvnot_tag>(arg);
      }

      logic_expression operator() ( unary_expression<bv_tag, bvtags::bvneg_tag> expr ) const {
        return expr;
      }

      logic_expression operator() ( binary_expression<bv_tag, bvtags::bvand_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        // ( expr e /\ expr e ) ==> expr e
        if ( lhs == rhs ) {
          return lhs;
        }

        if ( has_type< binary_expression<bv_tag, bvtags::bvand_tag> >(lhs) ) {
          // TODO
        }

        if ( has_type< binary_expression<bv_tag, bvtags::bvand_tag> >(rhs) ) {
          // TODO
        }

        return binary_expression<bv_tag, bvtags::bvand_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<bv_tag, bvtags::bvnand_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        // !( expr e /\ expr e ) ==> !(expr e)
        if ( lhs == rhs ) {
          return unary_expression<bv_tag, bvtags::bvnot_tag>(lhs);
        }

        return binary_expression<bv_tag, bvtags::bvnand_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<bv_tag, bvtags::bvor_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        // ( expr e \/ expr e ) ==> expr e
        if ( lhs == rhs ) {
          return lhs;
        }

        return binary_expression<bv_tag, bvtags::bvor_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<bv_tag, bvtags::bvnor_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        // !( expr e \/ expr e ) ==> !( expr e )
        if ( lhs == rhs ) {
          return unary_expression<bv_tag, bvtags::bvnot_tag>(lhs);
        }

        return binary_expression<bv_tag, bvtags::bvnor_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<bv_tag, bvtags::bvxor_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        return binary_expression<bv_tag, bvtags::bvxor_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<bv_tag, bvtags::bvxnor_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        return binary_expression<bv_tag, bvtags::bvxnor_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<bv_tag, bvtags::bvshl_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        // expr e << 0 ==> expr e
        if ( has_type< bit0_const >(rhs) ) {
          return lhs;
        }

        return binary_expression<bv_tag, bvtags::bvshl_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<bv_tag, bvtags::bvshr_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        // expr e >> 0 ==> expr e
        if ( has_type< bit0_const >(rhs) ) {
          return lhs;
        }

        return binary_expression<bv_tag, bvtags::bvshr_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<bv_tag, bvtags::bvashr_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        // expr e >> 0 ==> expr e
        if ( has_type< bit0_const >(rhs) ) {
          return lhs;
        }

        return binary_expression<bv_tag, bvtags::bvashr_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<bv_tag, bvtags::bvadd_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        // expr e + 0 ==> expr e
        if ( has_const_value(rhs, 0) ) {
          return lhs;
        }

        // 0 + expr e ==> expr e
        if ( has_const_value(lhs, 0) ) {
          return rhs;
        }

        return binary_expression<bv_tag, bvtags::bvadd_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<bv_tag, bvtags::bvsub_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        // expr e - 0 ==> expr e
        if ( has_const_value(rhs, 0) ) {
          return lhs;
        }

        // 0 - expr e ==> expr -e
        if ( has_const_value(lhs, 0) ) {
          return unary_expression<bv_tag, bvtags::bvneg_tag>(rhs);
        }

        return binary_expression<bv_tag, bvtags::bvsub_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<bv_tag, bvtags::bvmul_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        // TODO: expr e * 0 ==> 0
        // TODO: 0 * expr e ==> 0

        return binary_expression<bv_tag, bvtags::bvmul_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<bv_tag, bvtags::bvsrem_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        return binary_expression<bv_tag, bvtags::bvsrem_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<bv_tag, bvtags::bvsdiv_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        return binary_expression<bv_tag, bvtags::bvsdiv_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<bv_tag, bvtags::bvurem_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        return binary_expression<bv_tag, bvtags::bvurem_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<bv_tag, bvtags::bvudiv_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        return binary_expression<bv_tag, bvtags::bvudiv_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<bv_tag, bvtags::bvcomp_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        return binary_expression<bv_tag, bvtags::bvcomp_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<logic_tag, bvtags::bvslt_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        return binary_expression<logic_tag, bvtags::bvslt_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<logic_tag, bvtags::bvsgt_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        return binary_expression<logic_tag, bvtags::bvsgt_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<logic_tag, bvtags::bvsle_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        return binary_expression<logic_tag, bvtags::bvsle_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<logic_tag, bvtags::bvsge_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        return binary_expression<logic_tag, bvtags::bvsge_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<logic_tag, bvtags::bvult_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        return binary_expression<logic_tag, bvtags::bvult_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<logic_tag, bvtags::bvugt_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        return binary_expression<logic_tag, bvtags::bvugt_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<logic_tag, bvtags::bvule_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        return binary_expression<logic_tag, bvtags::bvule_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<logic_tag, bvtags::bvuge_tag> expr ) const {
        logic_expression lhs = simplify(expr.left);
        logic_expression rhs = simplify(expr.right);

        return binary_expression<logic_tag, bvtags::bvuge_tag>(lhs, rhs);
      }

      logic_expression operator() ( binary_expression<bv_tag, bvtags::concat_tag> expr ) const {
        return expr;
      }

      template < typename T >
      logic_expression operator() ( extract_expression<T> expr ) const {
        return expr;
      }

      template < typename T >
      logic_expression operator() ( extend_expression<T> expr ) const {
        return expr;
      }

      // Fallback
      template < typename T >
      logic_expression operator() ( T const & ) const {
        assert(false);
        return false;
      }

      boost::function<std::string(unsigned)> table_;
    }; // simplify_visitor

    inline logic_expression simplify( logic_expression e ) {
      return boost::apply_visitor( simplify_visitor(), e );
    }
  } // expression
} // metaSMT


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_FUNCTION_SCALAR_NTHROOT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_FUNCTION_SCALAR_NTHROOT_HPP_INCLUDED
#include <boost/simd/function/fast.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/function/scalar/is_inf.hpp>
#endif
#ifndef BOOST_SIMD_NO_INVALID
#include <boost/simd/function/scalar/is_nan.hpp>
#endif
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/scalar/abs.hpp>
#include <boost/simd/function/scalar/is_ltz.hpp>
#include <boost/simd/function/scalar/is_odd.hpp>
#include <boost/simd/function/scalar/minusone.hpp>
#include <boost/simd/function/scalar/pow.hpp>
#include <boost/simd/function/scalar/rec.hpp>
#include <boost/simd/function/scalar/sign.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( nthroot_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::integer_<A1> >
                          )
  {
    inline A0 operator() ( A0 a0, A1 a1) const BOOST_NOEXCEPT
    {
#ifndef BOOST_SIMD_NO_INVALID
      if (is_nan(a0)) return a0;
#endif
      auto is_ltza0 = is_ltz(a0);
      auto is_odda1 = is_odd(a1);
      if (is_ltza0 && !is_odda1) return Nan<A0>();
      A0 x = bs::abs(a0);
      if (x == One<A0>()) return a0;
      if (!a1) return (x < One<A0>()) ? Zero<A0>() : sign(a0)*Inf<A0>();
      if (!a0) return Zero<A0>();
#ifndef BOOST_SIMD_NO_INFINITIES
      if (is_inf(a0)) return (a1) ? a0 : One<A0>();
#endif
      A0 aa1 = static_cast<A0>(a1);
      A0 y = bs::pow(x,rec(aa1));
      // Correct numerical errors (since, e.g., 64^(1/3) is not exactly 4)
      // by one iteration of Newton's method
      if (y) y -= (bs::pow(y, aa1) - x) / (aa1* bs::pow(y,minusone(aa1)));
      return (is_ltza0 && is_odda1)? -y : y;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( nthroot_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::integer_<A1> >
                          , boost::simd::fast_tag
                          )
  {
    inline A0 operator() ( A0 a0, A1 a1
                         , fast_tag const&) const BOOST_NOEXCEPT
    {
      auto is_ltza0 = is_ltz(a0);
      auto is_odda1 = is_odd(a1);
      if (is_ltza0 && !is_odda1) return Nan<A0>();
      A0 x = bs::abs(a0);
      if (x == One<A0>()) return a0;
      if (!a1) return (x < One<A0>()) ? Zero<A0>() : sign(a0)*Inf<A0>();
      if (!a0) return Zero<A0>();
      A0 aa1 = static_cast<A0>(a1);
      A0 y = bs::pow(x,rec(aa1));
      return (is_ltza0 && is_odda1)? -y : y;
    }
  };
} } }

#endif


// A numerically integrated implementation of the circuit described in
// "Rolling Your Own Circuit Simulator with Eigen and Boost.ODEInt"
// using Eigen to store and manipulate circuit values
// 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>
#include <Eigen/Dense>

#include "mna.hpp"

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

struct circuit {
    circuit(double r, double l, double c) {

        using namespace Eigen;

        typedef Matrix<double, 4, 4> matrix4_t;
        matrix4_t G = matrix4_t::Zero();
        matrix4_t C = matrix4_t::Zero();

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

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

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

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

        // regularize so C is non-singular
        // First perform Gaussian elimination with full pivoting
        // This will put non-zero elements in the upper left, generally
        auto lu_fact = C.fullPivLu();
        matrix4_t Cprime = lu_fact.matrixLU().template triangularView<Upper>();

        // repeat process on G matrix and u vector using the factored components
        // LU produces a factorization P^-1 * L * U * Q^-1
        matrix4_t L = lu_fact.matrixLU().template triangularView<UnitLower>();
        matrix4_t P = lu_fact.permutationP();
        matrix4_t Q = lu_fact.permutationQ();
        matrix4_t Gprime = L.fullPivLu().solve(P * G * Q);   // permute rows and columns
        vector4_t Bprime = L.fullPivLu().solve(P * B);       // rows only
        Matrix<double, 1, 4> Dprime = D * Q;

        // Use the "rank" (# independent rows/columns) to determine where to split
        int sz = lu_fact.rank();
        int remainder = 4 - sz;

        // Produce smaller (and hopefully non-singular) matrices for calculation
        MatrixXd Cnew = Cprime.topLeftCorner(sz, sz);   // the rest is zero

        // break up Gprime into chunks based on the size of the nonzero portion of Cprime
        MatrixXd G11  = Gprime.topLeftCorner(sz, sz);
        MatrixXd G12  = Gprime.topRightCorner(sz, remainder);
        MatrixXd G21  = Gprime.bottomLeftCorner(remainder, sz);
        MatrixXd G22  = Gprime.bottomRightCorner(remainder, remainder);

        // solve for the (sz) state variables we will retain
        auto G22LU = G22.fullPivLu();
        MatrixXd Gnew = G11 - G12 * G22LU.solve(G21);

        // adjust input and output connections
        Matrix<double, Dynamic, 1> Bnew;
        Bnew = Bprime.topRows(sz) - G12 * G22LU.solve(Bprime.bottomRows(remainder));
        Matrix<double, 1, Dynamic> Dnew;
        Dnew = Dprime.leftCols(sz) - Dprime.rightCols(remainder) * G22LU.solve(G21);

        // verify the new C is non-singular
        assert(Cnew.rows() == Cnew.fullPivLu().rank());

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

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

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

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

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

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

        result = drift_term_ * xvec + input_term_ ;   // input is always 1V
    }

private:
    Eigen::MatrixXd drift_term_;            // connects current state to development over time
    Eigen::MatrixXd input_term_;            // connects input to development
    Eigen::Matrix<double, 1, Eigen::Dynamic> s2o_; // transforms state to output
};

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
               [ckt](state_t const& x, double t) {
                   std::cout << t << " " << ckt.state2output(x) << std::endl;
               });
}