#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_ */


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

  @copyright 2016 NumScale SAS
  @copyright 2016 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_FUNCTION_MANTISSA_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_MANTISSA_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-ieee
    Function object implementing mantissa capabilities

    Returns the signed mantissa of the floating input.

    @par Semantic:

    @code
    T r = mantissa(x);
    @endcode

    is similar to:

    @code
    T r = x*pow(2, -exponent(x));
    @endcode

    @par Note
    The @ref exponent e and signed @ref mantissa m of a floating point entry a are related by
    \f$a = m\times 2^e\f$, with the absolute value of m between one (included) ans two (excluded).

    @see frexp

  **/
  const boost::dispatch::functor<tag::mantissa_> mantissa = {};
} }
#endif

#include <boost/simd/function/scalar/mantissa.hpp>
#include <boost/simd/function/simd/mantissa.hpp>

#endif


#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;

}


#include <tdp/testing/testing.h>
#include <Eigen/Dense>
#include <tdp/slam/keyframe_slam.h>
#include <tdp/data/managed_image.h>

#include <isam/isam.h>
#include <isam/Slam.h>
#include <isam/robust.h>

using namespace tdp;
TEST(setup, KeyframeSLAM) {
  SE3f T_wk0;
  SE3f T_wk1 (SO3f::Rx(5.*M_PI/180.), Eigen::Vector3f(0,0,1));
  SE3f dT_01 (SO3f::Rx(6.*M_PI/180.), Eigen::Vector3f(0,0,1.1));

  KeyframeSLAM kfSLAM;
  kfSLAM.AddOrigin(T_wk0);
  kfSLAM.AddIcpOdometry(0,1,T_wk0.Inverse()*T_wk1);
  kfSLAM.AddLoopClosure(0,1,dT_01);

  kfSLAM.PrintGraph();
  kfSLAM.PrintValues();
  kfSLAM.Optimize();
  kfSLAM.PrintValues();
  kfSLAM.Optimize();
  kfSLAM.PrintValues();
}

TEST(setup, isam) {
  isam::Slam slam;

  isam::Properties props;
  props.verbose=true;
  props.method=isam::Method::GAUSS_NEWTON;
  props.method=isam::Method::LEVENBERG_MARQUARDT;
  props.epsilon_rel = 1e-6;
  props.epsilon_abs = 1e-6;
  slam.set_properties(props);

  float a = 5.*M_PI/180.;
  Eigen::Matrix3d Ra;
  Ra << 1, 0, 0,
       0, cos(a), -sin(a),
       0, sin(a), cos(a);
  a = 6.*M_PI/180.;
  Eigen::Matrix3d Rb;
  Rb << 1, 0, 0,
       0, cos(a), -sin(a),
       0, sin(a), cos(a);

  Eigen::Vector3d ta(0,0,1);
  Eigen::Vector3d tb(0,0,1.1);
  Eigen::Vector3d tc(0.01,0,1.1);

  isam::Pose3d origin(ta, isam::Rot3d(Ra));
  isam::Point3d p0(5.,1.,2.);

  // first monocular camera
  isam::Pose3d_Node* pose0 = new isam::Pose3d_Node();
  slam.add_node(pose0);

  // create a prior on the camera position
  isam::Noise noise6 = isam::Information(100. * isam::eye(6));
  isam::Pose3d_Factor* prior = new isam::Pose3d_Factor(pose0, origin, noise6);
  slam.add_factor(prior);

  // second monocular camera
  isam::Pose3d_Node* pose1 = new isam::Pose3d_Node();
  slam.add_node(pose1);

  isam::Pose3d delta(tb, isam::Rot3d(Rb));
  isam::Pose3d delta2(tc, isam::Rot3d(Rb));
  isam::Pose3d_Pose3d_Factor* odo = new isam::Pose3d_Pose3d_Factor(pose0,
      pose1, delta, noise6);
  slam.add_factor(odo);
  isam::Pose3d_Pose3d_Factor* odo2 = new isam::Pose3d_Pose3d_Factor(pose0,
      pose1, delta2, noise6);
  slam.add_factor(odo2);

  // optimize
  slam.update();
  std::cout << "After optimization:" << std::endl;
  std::cout << pose0->value() << std::endl;
  std::cout << pose1->value() << std::endl;

}

int main(int argc, char **argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


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

#include <dlib/matrix.h>
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <vector>
#include "../stl_checked.h"
#include "../array.h"
#include "../rand.h"
#include "checkerboard.h"
#include <dlib/statistics.h>

#include "tester.h"
#include <dlib/svm.h>

namespace
{

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

    logger dlog("test.svm_c_linear");

    typedef matrix<double, 0, 1> sample_type;
    typedef std::vector<std::pair<unsigned int, double> > sparse_sample_type;

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

    void run_prior_test()
    {
        typedef matrix<double,3,1> sample_type;
        typedef linear_kernel<sample_type> kernel_type;

        svm_c_linear_trainer<kernel_type> trainer;

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

        sample_type samp;
        samp = 0, 0, 1; samples.push_back(samp); labels.push_back(+1);
        samp = 0, 1, 0; samples.push_back(samp); labels.push_back(-1);

        trainer.set_c(10);
        decision_function<kernel_type> df = trainer.train(samples, labels);

        trainer.set_prior(df);

        samples.clear();
        labels.clear();
        samp = 1, 0, 0; samples.push_back(samp); labels.push_back(+1);
        samp = 0, 1, 0; samples.push_back(samp); labels.push_back(-1);

        df = trainer.train(samples, labels);

        samp = 0, 0, 1; samples.push_back(samp); labels.push_back(+1);
        matrix<double,1,2> rs = test_binary_decision_function(df, samples, labels);
        dlog << LINFO << rs;
        DLIB_TEST(rs(0) == 1);
        DLIB_TEST(rs(1) == 1);

        dlog << LINFO << trans(df.basis_vectors(0));
        DLIB_TEST(df.basis_vectors(0)(0) > 0);
        DLIB_TEST(df.basis_vectors(0)(1) < 0);
        DLIB_TEST(df.basis_vectors(0)(2) > 0);
    }

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

        svm_c_linear_trainer<kernel_type> trainer;

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

        sample_type samp;
        samp[0] = 1; samples.push_back(samp); labels.push_back(+1); samp.clear();
        samp[1] = 1; samples.push_back(samp); labels.push_back(-1); samp.clear();

        trainer.set_c(10);
        decision_function<kernel_type> df = trainer.train(samples, labels);

        trainer.set_prior(df);

        samples.clear();
        labels.clear();
        samp[2] = 1; samples.push_back(samp); labels.push_back(+1); samp.clear();
        samp[1] = 1; samples.push_back(samp); labels.push_back(-1); samp.clear();

        df = trainer.train(samples, labels);

        matrix<double,1,2> rs = test_binary_decision_function(df, samples, labels);
        dlog << LINFO << rs;
        DLIB_TEST(rs(0) == 1);
        DLIB_TEST(rs(1) == 1);

        matrix<double,0,1> w = sparse_to_dense(df.basis_vectors(0));
        dlog << LINFO << trans(w);
        DLIB_TEST(w(0) > 0.1);
        DLIB_TEST(w(1) < -0.1);
        DLIB_TEST(w(2) > 0.1);
    }

    void get_simple_points (
        std::vector<sample_type>& samples,
        std::vector<double>& labels
    )
    {
        samples.clear();
        labels.clear();
        sample_type samp(2);

        samp = 0,0;
        samples.push_back(samp);
        labels.push_back(-1);

        samp = 0,1;
        samples.push_back(samp);
        labels.push_back(-1);

        samp = 3,0;
        samples.push_back(samp);
        labels.push_back(+1);

        samp = 3,1;
        samples.push_back(samp);
        labels.push_back(+1);
    }

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

    void get_simple_points_sparse (
        std::vector<sparse_sample_type>& samples,
        std::vector<double>& labels
    )
    {
        samples.clear();
        labels.clear();
        sparse_sample_type samp;

        samp.push_back(make_pair(0, 0.0));
        samp.push_back(make_pair(1, 0.0));
        samples.push_back(samp);
        labels.push_back(-1);

        samp.clear();
        samp.push_back(make_pair(0, 0.0));
        samp.push_back(make_pair(1, 1.0));
        samples.push_back(samp);
        labels.push_back(-1);

        samp.clear();
        samp.push_back(make_pair(0, 3.0));
        samp.push_back(make_pair(1, 0.0));
        samples.push_back(samp);
        labels.push_back(+1);

        samp.clear();
        samp.push_back(make_pair(0, 3.0));
        samp.push_back(make_pair(1, 1.0));
        samples.push_back(samp);
        labels.push_back(+1);
    }

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

    void test_sparse (
    )
    {
        print_spinner();
        dlog << LINFO << "test with sparse vectors";
        std::vector<sparse_sample_type> samples;
        std::vector<double> labels;

        sample_type samp;

        get_simple_points_sparse(samples,labels);

        svm_c_linear_trainer<sparse_linear_kernel<sparse_sample_type> > trainer;
        trainer.set_c(1e4);
        //trainer.be_verbose();
        trainer.set_epsilon(1e-11);

        double obj;
        decision_function<sparse_linear_kernel<sparse_sample_type> > df = trainer.train(samples, labels, obj);
        dlog << LDEBUG << "obj: "<< obj;
        DLIB_TEST_MSG(abs(obj - 0.72222222222) < 1e-7, obj);

        DLIB_TEST(abs(df(samples[0]) - (-1)) < 1e-6);
        DLIB_TEST(abs(df(samples[1]) - (-1)) < 1e-6);
        DLIB_TEST(abs(df(samples[2]) - (1)) < 1e-6);
        DLIB_TEST(abs(df(samples[3]) - (1)) < 1e-6);

        // While we are at it, make sure the krr_trainer works with sparse samples
        krr_trainer<sparse_linear_kernel<sparse_sample_type> > krr;

        df = krr.train(samples, labels);
        DLIB_TEST(abs(df(samples[0]) - (-1)) < 1e-6);
        DLIB_TEST(abs(df(samples[1]) - (-1)) < 1e-6);
        DLIB_TEST(abs(df(samples[2]) - (1)) < 1e-6);
        DLIB_TEST(abs(df(samples[3]) - (1)) < 1e-6);

        // Now test some of the sparse helper functions
        DLIB_TEST(max_index_plus_one(samples) == 2);
        DLIB_TEST(max_index_plus_one(samples[0]) == 2);

        matrix<double,3,1> m;
        m = 1;
        add_to(m, samples[3]);
        DLIB_TEST(m(0) == 1 + samples[3][0].second);
        DLIB_TEST(m(1) == 1 + samples[3][1].second);
        DLIB_TEST(m(2) == 1);

        m = 1;
        subtract_from(m, samples[3]);
        DLIB_TEST(m(0) == 1 - samples[3][0].second);
        DLIB_TEST(m(1) == 1 - samples[3][1].second);
        DLIB_TEST(m(2) == 1);

        m = 1;
        add_to(m, samples[3], 2);
        DLIB_TEST(m(0) == 1 + 2*samples[3][0].second);
        DLIB_TEST(m(1) == 1 + 2*samples[3][1].second);
        DLIB_TEST(m(2) == 1);

        m = 1;
        subtract_from(m, samples[3], 2);
        DLIB_TEST(m(0) == 1 - 2*samples[3][0].second);
        DLIB_TEST(m(1) == 1 - 2*samples[3][1].second);
        DLIB_TEST(m(2) == 1);

    }

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

    void test_dense (
    )
    {
        print_spinner();
        dlog << LINFO << "test with dense vectors";
        std::vector<sample_type> samples;
        std::vector<double> labels;

        sample_type samp;

        get_simple_points(samples,labels);

        svm_c_linear_trainer<linear_kernel<sample_type> > trainer;
        trainer.set_c(1e4);
        //trainer.be_verbose();
        trainer.set_epsilon(1e-11);

        double obj;
        decision_function<linear_kernel<sample_type> > df = trainer.train(samples, labels, obj);
        dlog << LDEBUG << "obj: "<< obj;
        DLIB_TEST_MSG(abs(obj - 0.72222222222) < 1e-7, abs(obj - 0.72222222222));
        // There shouldn't be any margin violations since this dataset is so trivial.  So that means the objective
        // should be exactly the squared norm of the decision plane (times 0.5).
        DLIB_TEST_MSG(abs(length_squared(df.basis_vectors(0))*0.5 + df.b*df.b*0.5 - 0.72222222222) < 1e-7,
                      length_squared(df.basis_vectors(0))*0.5 + df.b*df.b*0.5);

        DLIB_TEST(abs(df(samples[0]) - (-1)) < 1e-6);
        DLIB_TEST(abs(df(samples[1]) - (-1)) < 1e-6);
        DLIB_TEST(abs(df(samples[2]) - (1)) < 1e-6);
        DLIB_TEST(abs(df(samples[3]) - (1)) < 1e-6);
    }

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

    class tester_svm_c_linear : public tester
    {
    public:
        tester_svm_c_linear (
        ) :
            tester ("test_svm_c_linear",
                    "Runs tests on the svm_c_linear_trainer.")
        {}

        void perform_test (
        )
        {
            test_dense();
            test_sparse();
            run_prior_test();
            run_prior_sparse_test();

            // test mixed sparse and dense dot products
            {
                std::map<unsigned int, double> sv;
                matrix<double,0,1> dv(4);

                dv = 1,2,3,4;

                sv[0] = 1;
                sv[3] = 1;

                DLIB_TEST(dot(sv,dv) == 5);
                DLIB_TEST(dot(dv,sv) == 5);
                DLIB_TEST(dot(dv,dv) == 30);
                DLIB_TEST(dot(sv,sv) == 2);

                sv[10] = 9;
                DLIB_TEST(dot(sv,dv) == 5);
            }

            // test mixed sparse dense assignments
            {
                std::map<unsigned int, double> sv, sv2;
                std::vector<std::pair<unsigned int, double> > sv3;
                matrix<double,0,1> dv(4), dv2;

                dv = 1,2,3,4;

                sv[0] = 1;
                sv[3] = 1;

                assign(dv2, dv);

                DLIB_TEST(dv2.size() == 4);
                DLIB_TEST(dv2(0) == 1);
                DLIB_TEST(dv2(1) == 2);
                DLIB_TEST(dv2(2) == 3);
                DLIB_TEST(dv2(3) == 4);

                assign(sv2, dv);
                DLIB_TEST(sv2.size() == 4);
                DLIB_TEST(sv2[0] == 1);
                DLIB_TEST(sv2[1] == 2);
                DLIB_TEST(sv2[2] == 3);
                DLIB_TEST(sv2[3] == 4);

                assign(sv2, sv);
                DLIB_TEST(sv2.size() == 2);
                DLIB_TEST(sv2[0] == 1);
                DLIB_TEST(sv2[1] == 0);
                DLIB_TEST(sv2[2] == 0);
                DLIB_TEST(sv2[3] == 1);

                assign(sv3, sv);
                DLIB_TEST(sv3.size() == 2);
                DLIB_TEST(sv3[0].second == 1);
                DLIB_TEST(sv3[1].second == 1);
                DLIB_TEST(sv3[0].first == 0);
                DLIB_TEST(sv3[1].first == 3);

                assign(sv3, dv);
                DLIB_TEST(sv3.size() == 4);
                DLIB_TEST(sv3[0].second == 1);
                DLIB_TEST(sv3[1].second == 2);
                DLIB_TEST(sv3[2].second == 3);
                DLIB_TEST(sv3[3].second == 4);
                DLIB_TEST(sv3[0].first == 0);
                DLIB_TEST(sv3[1].first == 1);
                DLIB_TEST(sv3[2].first == 2);
                DLIB_TEST(sv3[3].first == 3);

                assign(sv3, sv);
                DLIB_TEST(sv3.size() == 2);
                DLIB_TEST(sv3[0].second == 1);
                DLIB_TEST(sv3[1].second == 1);
                DLIB_TEST(sv3[0].first == 0);
                DLIB_TEST(sv3[1].first == 3);

                sv.clear();
                assign(sv, sv3);
                DLIB_TEST(sv.size() == 2);
                DLIB_TEST(sv[0] == 1);
                DLIB_TEST(sv[1] == 0);
                DLIB_TEST(sv[2] == 0);
                DLIB_TEST(sv[3] == 1);

            }
        }
    } a;

}


/*

	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;

}


#include "hexadecimal.h"
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(hex_1_is_decimal_1)
{
    BOOST_REQUIRE_EQUAL(0x1, hexadecimal::convert("1"));
}

#if defined(EXERCISM_RUN_ALL_TESTS)
BOOST_AUTO_TEST_CASE(hex_c_is_decimal_12)
{
    BOOST_REQUIRE_EQUAL(0xc, hexadecimal::convert("c"));
}

BOOST_AUTO_TEST_CASE(hex_10_is_decimal_16)
{
    BOOST_REQUIRE_EQUAL(0x10, hexadecimal::convert("10"));
}

BOOST_AUTO_TEST_CASE(hex_af_is_decimal_175)
{
    BOOST_REQUIRE_EQUAL(0xaf, hexadecimal::convert("af"));
}

BOOST_AUTO_TEST_CASE(hex_100_is_decimal_256)
{
    BOOST_REQUIRE_EQUAL(0x100, hexadecimal::convert("100"));
}

BOOST_AUTO_TEST_CASE(hex_19ace_is_decimal_105166)
{
    BOOST_REQUIRE_EQUAL(0x19ace, hexadecimal::convert("19ace"));
}

BOOST_AUTO_TEST_CASE(invalid_hex_is_decimal_0)
{
    BOOST_REQUIRE_EQUAL(0, hexadecimal::convert("carrot"));
}

BOOST_AUTO_TEST_CASE(black)
{
    BOOST_REQUIRE_EQUAL(0x000000, hexadecimal::convert("000000"));
}

BOOST_AUTO_TEST_CASE(white)
{
    BOOST_REQUIRE_EQUAL(0xffffff, hexadecimal::convert("ffffff"));
}

BOOST_AUTO_TEST_CASE(yellow)
{
    BOOST_REQUIRE_EQUAL(0xffff00, hexadecimal::convert("ffff00"));
}
#endif


#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


#include <fmt/format.h>
#include <fmt/printf.h>

#include <catch2/catch.hpp>

#include <boost/filesystem.hpp>

#include "test_config.hpp"
#include "embedded_python/functions.hpp"

TEST_CASE( "Python function with one parameter and returning a pair can be called from c++"
           , "[embedde_python]" ) {

  std::string const module_path( "../tests/data/embedded_python/script.py" );
  std::string const function_name( "function_1" );
  CppPy::FunctionP1R2 function;
  REQUIRE_NOTHROW( function.Load( module_path, function_name ) );

  double x{ 2.0 };
  double x2{ 0.0 };
  double x3{ 0.0 };
  REQUIRE( true == function.Evaluate( x, x2, x3 ) );
  REQUIRE( x*x == x2 );
  REQUIRE( x*x2 == x3 );

  if( TestConfig::verbose ) {
    fmt::print( "\nIf x = {} then x^2 = {} and x^3 = {}\n", x, x2, x3 );
  }

}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/list/instance.hpp>
#include <boost/hana/maybe.hpp>
using namespace boost::hana;

int main() {
    //! [list]
    BOOST_HANA_CONSTEXPR_LAMBDA auto half = [](auto x) {
        return if_(x % int_<2> == int_<0>,
            just(x / int_<2>),
            nothing
        );
    };

    BOOST_HANA_CONSTANT_ASSERT(
        traverse<Maybe>(half, list(int_<2>, int_<4>, int_<6>))
        ==
        just(list(int_<1>, int_<2>, int_<3>))
    );

    BOOST_HANA_CONSTANT_ASSERT(
        traverse<Maybe>(half, list(int_<2>, int_<3>, int_<6>))
        ==
        nothing
    );
    //! [list]

    //! [maybe]
    BOOST_HANA_CONSTEXPR_LAMBDA auto twice = [](auto x) {
        return list(x, x);
    };

    BOOST_HANA_CONSTEXPR_ASSERT(
        traverse<List>(twice, just('x')) == list(just('x'), just('x'))
    );

    BOOST_HANA_CONSTANT_ASSERT(
        traverse<List>(twice, nothing) == list(nothing)
    );
    //! [maybe]
}


// BLAS level 2
// symmetric & hermitian matrices

//#define BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <stddef.h>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/atlas/cblas1.hpp>
#include <boost/numeric/bindings/atlas/cblas2.hpp>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/traits/ublas_symmetric.hpp>
#include <boost/numeric/bindings/traits/ublas_hermitian.hpp>
#include "utils.h"

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

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

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

typedef ublas::vector<real_t> vct_t;
typedef ublas::vector<cmplx_t> cvct_t;

typedef ublas::symmetric_matrix<
  real_t, ublas::upper, ublas::column_major
> ucsymm_t;
typedef ublas::symmetric_matrix<
  real_t, ublas::lower, ublas::column_major
> lcsymm_t;
typedef ublas::symmetric_matrix<
  real_t, ublas::upper, ublas::row_major
> ursymm_t;
typedef ublas::symmetric_matrix<
  real_t, ublas::lower, ublas::row_major
> lrsymm_t;

typedef ublas::hermitian_matrix<
  cmplx_t, ublas::upper, ublas::column_major
> ucherm_t;
typedef ublas::hermitian_matrix<
  cmplx_t, ublas::lower, ublas::column_major
> lcherm_t;
typedef ublas::hermitian_matrix<
  cmplx_t, ublas::upper, ublas::row_major
> urherm_t;
typedef ublas::hermitian_matrix<
  cmplx_t, ublas::lower, ublas::row_major
> lrherm_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> ucha_t;
typedef ublas::hermitian_adaptor<cm_t, ublas::lower> lcha_t;
typedef ublas::hermitian_adaptor<rm_t, ublas::upper> urha_t;
typedef ublas::hermitian_adaptor<rm_t, ublas::lower> lrha_t;

int main() {

  cout << endl;

  cout << "symmetric matrix" << endl << endl;
  size_t n;
  cout << "n -> ";
  cin >> n;
  cout << endl;

  vct_t vx (n), vy (n);
  atlas::set (1., vx);
  print_v (vx, "vx");

  ucsymm_t ucs (n, n);
  lcsymm_t lcs (n, n);
  ursymm_t urs (n, n);
  lrsymm_t lrs (n, n);

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

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

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

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

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

  // vy = symm vx
  atlas::spmv (ucs, vx, vy);
  print_v (vy, "vy = ucs vx");
  cout << endl;
  atlas::spmv (1, lcs, vx, 0, vy);
  print_v (vy, "vy = lcs vx");
  cout << endl;
  atlas::spmv (1., urs, vx, 0., vy);
  print_v (vy, "vy = urs vx");
  cout << endl;
  atlas::spmv (1.0f, lrs, vx, 0.0f, vy);
  print_v (vy, "vy = lrs vx");
  cout << endl;

#ifdef F_COMPILATION_FAILURE

  atlas::symv (ucs, vx, vy);
  print_v (vy, "vy = ucs vx");
  cout << endl;

  atlas::hpmv (lcs, vx, vy);
  print_v (vy, "vy = lcs vx");
  cout << endl;

  atlas::gemv (urs, vx, vy);
  print_v (vy, "vy = urs vx");
  cout << endl;

  atlas::spmv (cmplx_t (1., 0.), lrs, vx, cmplx_t (0., 0.), vy);
  print_v (vy, "vy = lrs vx");
  cout << endl;

#endif

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

  cout << endl << "hermitian matrix" << endl << endl;

  size_t n2 = 3;

  cvct_t cvx (n2), cvy (n2);
  atlas::set (1., cvx);
  print_v (cvx, "cvx");
  cout << endl;

  ucherm_t uch (n2, n2);
  uch (0, 0) = cmplx_t (3, 0);
  uch (0, 1) = cmplx_t (2, -2);
  uch (1, 1) = cmplx_t (3, 0);
  uch (0, 2) = cmplx_t (1, -1);
  uch (1, 2) = cmplx_t (2, 2);
  uch (2, 2) = cmplx_t (3, 0);
  print_m (uch, "uch");
  cout << endl;
  print_m_data (uch, "uch");
  cout << endl;

  lcherm_t lch (n2, n2);
  lch = uch;
  print_m (lch, "lch");
  cout << endl;
  print_m_data (lch, "lch");
  cout << endl;

  urherm_t urh (uch);
  print_m (urh, "urh");
  cout << endl;
  print_m_data (urh, "urh");
  cout << endl;

  lrherm_t lrh (uch);
  print_m (lrh, "lrh");
  cout << endl;
  print_m_data (lrh, "lrh");
  cout << endl;

  // cvy = herm cvx
  atlas::hpmv (uch, cvx, cvy);
  print_v (cvy, "cvy = uch cvx");
  cout << endl;
  atlas::hpmv (1, lch, cvx, 0, cvy);
  print_v (cvy, "cvy = lch cvx");
  cout << endl;
  atlas::hpmv (1., urh, cvx, 0., cvy);
  print_v (cvy, "cvy = urh cvx");
  cout << endl;
  atlas::hpmv (cmplx_t (1., 0.), lrh, cvx, cmplx_t (0., 0.), cvy);
  print_v (cvy, "cvy = lrh cvx");
  cout << endl;

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

  cout << endl << "hermitian adaptor" << endl << endl;

  cm_t cm1 (n2, n2);
  ucha_t ucha (cm1);
  ucha = uch;
  print_m (ucha, "ucha");
  cout << endl;
  print_m_data (ucha, "ucha");
  cout << endl;

  cm_t cm2 (n2, n2);
  lcha_t lcha (cm2);
  lcha = uch;
  print_m (lcha, "lcha");
  cout << endl;
  print_m_data (lcha, "lcha");
  cout << endl;

  rm_t rm1 (n2, n2);
  urha_t urha (rm1);
  urha = uch;
  print_m (urha, "urha");
  cout << endl;
  print_m_data (urha, "urha");
  cout << endl;

  rm_t rm2 (n2, n2);
  lrha_t lrha (rm2);
  lrha = uch;
  print_m (lrha, "lrha");
  cout << endl;
  print_m_data (lrha, "lrha");
  cout << endl;

  // cvy = herma cvx
  atlas::hemv (ucha, cvx, cvy);
  print_v (cvy, "cvy = uch cvx");
  cout << endl;
  atlas::hemv (1, lcha, cvx, 0, cvy);
  print_v (cvy, "cvy = lch cvx");
  cout << endl;
  atlas::hemv (1., urha, cvx, 0., cvy);
  print_v (cvy, "cvy = urh cvx");
  cout << endl;
  atlas::hemv (cmplx_t (1., 0.), lrha, cvx, cmplx_t (0., 0.), cvy);
  print_v (cvy, "cvy = lrh cvx");
  cout << endl;

}


#include <ext/iterator/zip_iterator.hpp>

#include <algorithm>
#include <iostream>
#include <iomanip>
#include <random>
#include <boost/range/algorithm.hpp>
#include <boost/range/algorithm_ext.hpp>
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(zip_iterator_sort_test)
{
	using namespace std;
	vector<int> data = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
	vector<short> short_data {data.begin(), data.end()};
	boost::reverse(data);

	auto first = ext::make_zip_iterator(data.begin(), short_data.begin());
	auto last = ext::make_zip_iterator(data.end(), short_data.end());

	typedef decltype(first) iter_type;
	typedef std::tuple<const int &, const short &> ref;

	std::stable_sort(first, last, [](const ref & r1, const ref & r2) { return std::get<0>(r1) < std::get<0>(r2); });
	bool reversed = boost::is_sorted(short_data, std::greater<> {});
	BOOST_CHECK(reversed);
}

BOOST_AUTO_TEST_CASE(zip_iterator_partition_test)
{
	using namespace std;
	vector<int> data = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
	vector<short> short_data {data.begin(), data.end()};

	auto expected_data = data;
	auto expected_short_data = short_data;

	// partition by odd
	boost::stable_partition(expected_data, [](int i) { return i % 1; });
	boost::stable_partition(expected_short_data, [](short i) { return i % 1; });

	auto first = ext::make_zip_iterator(data.begin(), short_data.begin());
	auto last = ext::make_zip_iterator(data.end(), short_data.end());

	//partition by odd on first array
	typedef std::tuple<const int &, const short &> ref;
	std::stable_partition(first, last, [](const ref & r) { return std::get<0>(r) % 1; });

	BOOST_CHECK(expected_data == data);
	BOOST_CHECK(expected_short_data == short_data);
}