#ifndef VEC3F_HXX
#define VEC3F_HXX

#include <iostream>

#include "eigen.hxx"
#include <eigen3/Eigen/Geometry>
#include <eigen3/Eigen/LU>
// WARNING IMPORTANT!!!!!
// READ https://eigen.tuxfamily.org/dox/group__TopicStlContainers.html

#include <boost/optional/optional.hpp>

// Uh ... in global namespace. I should probably change that ...
constexpr auto Epsilon = std::numeric_limits<double>::epsilon();
constexpr auto EpsilonFloat = std::numeric_limits<float>::epsilon();
constexpr auto Pi      = double(3.14159265358979323846264338327950288419716939937510);
constexpr auto PiFloat      = float(3.14159265358979323846264338327950288419716939937510);
constexpr auto EulerNumber = double(2.7182818284590452353602874713527);
constexpr auto Infinity= std::numeric_limits<double>::infinity();
constexpr auto InfinityFloat = std::numeric_limits<float>::infinity();
constexpr auto LargeNumber = std::numeric_limits<double>::max()/16;
constexpr auto LargeFloat  = std::numeric_limits<float>::max()/16;
constexpr auto NaN = std::numeric_limits<double>::quiet_NaN();
constexpr auto UnitSphereSurfaceArea = 4.*Pi;
constexpr auto UnitSphereVolume = 4./3.*Pi;
constexpr auto UnitHalfSphereSurfaceArea = 2.*Pi;
constexpr auto UnitDiscSurfaceArea = Pi;

template<class T, int d>
class Vec : public Eigen::Matrix<T, d, 1>
{
  public:
    typedef Eigen::Matrix<T, d, 1> Base;
    typedef T value_type;

    /*-------------------------------------------
     * ctors
     * -----------------------------------------*/

    Vec() noexcept : Base(Base::Constant(0.)) {}

    explicit Vec(const T &v) noexcept : Base() { this->setConstant(d, v); }

    Vec(const T &a, const T &b) noexcept : Base(a, b) {}
    Vec(const T &a, const T &b, const T &c) noexcept: Base(a, b, c) {}

    template<typename OtherDerived >
    Vec (const Eigen::MatrixBase< OtherDerived > &other) noexcept : Base(other) {}
};

#if 0
typedef Eigen::Matrix<int, 3, 1> Int3;
typedef Eigen::Matrix<int, 2, 1> Int2;
typedef Eigen::Matrix<double, 3, 1> Double3;
typedef Eigen::Matrix<double, 2, 1> Double2;
typedef Eigen::Matrix<double, 3, 1> Float3;
typedef Eigen::Matrix<double, 2, 1> Float2;
#define VECDARG class T,int d
#define VECD Eigen::Matrix<T, d, 1>
#else
typedef Vec<double, 3> Double3;
typedef Vec<double, 2> Double2;
typedef Vec<float, 3> Float3;
typedef Vec<float, 2> Float2;
#define VECDARG class T,int d
#define VECD Vec<T, d>
#endif

using Int2 = Eigen::Array2i;
using UInt3 = Eigen::Array<unsigned int, 3, 1>;

using Index3 = Eigen::Array<int, 3, 1>;

template<typename T>
using Matrix33 = Eigen::Matrix<T, 3, 3>;

template<class T>
constexpr int static_size()
{
  return 0;
}

template<class Derived>
inline std::ostream& operator<<(std::ostream &os, const Eigen::MatrixBase<Derived> &v)
{
  if (v.cols() > 1)
  {
    os << "<";
    for (int i=0; i<v.rows(); ++i)
    {
      os << "<";
      for (int j=0; j<v.cols(); ++j)
      {
        os << v(i,j);
        if (j < v.cols()-1) os << ", ";
      }
      os << ">";
      if (i < v.rows()-1) os << ",\n ";
    }
    os << ">";
  }
  else
  {
    os << "<";
    for (int j=0; j<v.rows(); ++j)
    {
      os << v(j,0);
      if (j < v.rows()-1) os << ", ";
    }
    os << ">T";
  }
  return os;
}

template<class Derived>
inline std::ostream& operator<<(std::ostream &os, const Eigen::ArrayBase<Derived> &v)
{
  if (v.cols() > 1)
  {
    os << "[";
    for (int i=0; i<v.rows(); ++i)
    {
      os << "[";
      for (int j=0; j<v.cols(); ++j)
      {
        os << v(i,j);
        if (j < v.cols()-1) os << ", ";
      }
      os << "]";
      if (i < v.rows()-1) os << ",\n";
    }
    os << "]";
  }
  else
  {
    os << "[";
    for (int j=0; j<v.rows(); ++j)
    {
      os << v(j,0);
      if (j < v.rows()-1) os << ", ";
    }
    os << "]T";
  }
  return os;
}

template<class T>
inline T Cross( const Eigen::Matrix<T,2,1>& u, const Eigen::Matrix<T,2,1> &v )
{
  return u.x()*v.y()-u.y()*v.x();
}

template<class Derived1, class Derived2>
auto Cross( const Eigen::MatrixBase<Derived1>& u, const Eigen::MatrixBase<Derived2> &v )
{
  return u.cross(v);
}

template<class Derived1, class Derived2>
auto Product( const Eigen::MatrixBase<Derived1>& u, const Eigen::MatrixBase<Derived2> &v )
{
  return u.cwiseProduct(v);
}

template<class Derived1, class Derived2>
auto Dot(const Eigen::MatrixBase<Derived1> &u, const Eigen::MatrixBase<Derived2> &v )
{
  return u.dot(v);
}

template<class Derived1, class Derived2>
auto DotAbs(const Eigen::MatrixBase<Derived1> &u, const Eigen::MatrixBase<Derived2> &v )
{
  return (u.array()*v.array()).abs().sum();
}

template<class Derived>
auto Length(const Eigen::MatrixBase<Derived> &a)
{
  return a.norm();
}

template<class Derived>
auto LengthSqr(const Eigen::MatrixBase<Derived> &a)
{
  return a.squaredNorm();
}

template<class Derived>
inline void Normalize(Eigen::MatrixBase<Derived>& u)
{
  return u.normalize();
}

template<class Derived>
auto Normalized(const Eigen::MatrixBase<Derived>& u)
{
  return u.normalized();
}

template<class Derived1, class Derived2>
inline auto AbsDot(const Eigen::MatrixBase<Derived1> &u, const Eigen::MatrixBase<Derived2> &v )
{
  return std::abs(u.dot(v));
}

template<class Derived1, class Derived2>
inline auto ClampDot(const Eigen::MatrixBase<Derived1> &u, const Eigen::MatrixBase<Derived2> &v )
{
  using Scalar = decltype(u.dot(v));
  return std::max(Scalar(0), u.dot(v));
}

#ifndef NDEBUG
// Quite high tolerance because if the vector stems from 32 bit calculations it won't be much better.
#define ASSERT_NORMALIZED(v) assert(std::abs(LengthSqr(v) - 1.) < 1.e-5)
#define ASSERT_NORMALIZED_F32(v) assert(std::abs(LengthSqr(v) - 1.f) < 0.01f)
#else
#define ASSERT_NORMALIZED(v) ((void)0)
#define ASSERT_NORMALIZED_F32(v) ((void)0)
#endif

// template<class Scalar>
// inline Scalar Clip(Scalar x, Scalar a,Scalar b) {
//   // TODO: Change this so that it preserve NaN input in x;
//   if(x>b) x = b;
//   else if(x<a) x = a;
//   return x;
// }

template<class Derived>
Eigen::Matrix<typename Eigen::internal::traits<Derived>::Scalar, 3,3>
inline OrthogonalSystemZAligned(const Eigen::MatrixBase<Derived> &_Z)
{
  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
  Eigen::Matrix<Scalar, 3 ,3> m;
  auto Z = m.col(2);
  auto X = m.col(0);
  auto Y = m.col(1);
  Z = _Z;
  ASSERT_NORMALIZED_F32(Z);
  // Listing 3 in Duff et al. (2017) "Building an Orthonormal Basis, Revisited".
  Scalar sign = std::copysign(Scalar(1.0f), Z[2]);
  const Scalar a = Scalar(-1.0) / (sign + Z[2]);
  const Scalar b = Z[0] * Z[1] * a;
  X = Eigen::Matrix<Scalar,3,1>(Scalar(1.0f) + sign * Z[0] * Z[0] * a, sign * b, -sign * Z[0]);
  Y = Eigen::Matrix<Scalar,3,1>(b, sign + Z[1] * Z[1] * a, -Z[1]);
  return m;
}

template<class Derived, int M>
inline auto Take(const Eigen::ArrayBase<Derived>& u, const Eigen::Array<int, M, 1> &indices)
{
  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
  Eigen::Array<Scalar,M,1> ret;
  for (decltype(indices.size()) i=0; i<indices.size(); ++i)
  {
    assert(indices[i] >= 0 && indices[i]<u.size());
    ret[i] = u[indices[i]];
  }
  return ret;
}

namespace Projections
{

template<class T>
inline auto UvToSpherical(const Eigen::Matrix<T,2,1> &uv)
{
  using Scalar = T;
  const Scalar theta = uv[1]*Scalar(Pi);
  const Scalar phi   = uv[0]*Scalar(2.*Pi);
  return Eigen::Matrix<T,2,1>{phi, theta};
}

template<class T>
inline auto SphericalToUv(const Eigen::Matrix<T,2,1> &angles)
{
  using Scalar = T;
  const Scalar theta = angles[1];
  const Scalar phi = angles[0];
  assert(theta >= 0 && theta <= Pi*1.00001);
  const Scalar u = phi/Scalar(2.*Pi);
  const Scalar v = theta/Scalar(Pi);
  return Eigen::Matrix<Scalar,2,1>{u, v};
}

// From Cartesian to spherical coordinates. Z is up.
template<class Derived>
inline auto KartesianToSpherical(const Eigen::MatrixBase<Derived> &xyz)
{
  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
  const Scalar x = xyz[0];
  const Scalar z = xyz[1];
  const Scalar y = xyz[2];  // Because I'm dump, here, y is up.
  const Scalar ax = std::abs(x);
  const Scalar az = std::abs(z);
  const Scalar r = xyz.norm();
  Scalar theta = std::acos(y/r);
  Scalar phi;
  if (ax > az)
  {
    phi = std::atan2(z,ax);
    phi = (x > Scalar(0)) ? phi : Scalar(Pi) - phi;
  }
  else
  {
    phi = std::atan2(x,az);
    phi = (z > Scalar(0)) ? Scalar(Pi/2.)-phi : Scalar(3./2.*Pi )+ phi;
  }
  return Eigen::Matrix<Scalar,2,1>{phi, theta};
}

template<class Derived>
inline auto SphericalToUnitKartesian(const Eigen::MatrixBase<Derived> &angles)
{
  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
  const Scalar theta = angles[1];
  const Scalar phi   = angles[0];
  const Scalar z = std::cos(theta);
  Scalar r = std::sqrt(Scalar(1)-z*z);
  r = std::isnan(r) ? Scalar(0) : r;
  const Scalar x = r*std::cos(phi);
  const Scalar y = r*std::sin(phi);
  return Eigen::Matrix<Scalar,3,1>{x,y,z};
}

}

#endif


#include <opencv2/imgproc/imgproc.hpp>
#include <boost/foreach.hpp>

#include "detection.hpp"

#define foreach BOOST_FOREACH

namespace artelab
{

    cv::Rect contour_bounding_box(std::vector<cv::Point> contour)
    {
        int top = std::numeric_limits<int>::max();
        int bot = -1;
        int left = top;
        int right = -1;
        foreach(cv::Point p, contour)
        {
            if(p.x < left) left = p.x;
            if(p.x > right) right = p.x;
            if(p.y < top) top = p.y;
            if(p.y > bot) bot = p.y;
        }
        return cv::Rect(left, top, right-left, bot-top);
    }

    std::vector<cv::Rect> object_rectangles(cv::Mat feature_image, int thresh)
    {
        using std::vector;

        vector<cv::Rect> rectangles;
        cv::Size imgsize = feature_image.size();
        const double smaller_side = 18;
        double scalefactor = (imgsize.width > imgsize.height? imgsize.height : imgsize.width) / smaller_side;

        int min_size_rect = int(scalefactor * smaller_side / 6 + 0.5);

        cv::Mat img_thresh;
        cv::threshold(feature_image, img_thresh, thresh, 255, cv::THRESH_BINARY);

        vector<vector<cv::Point> > contours;
        vector<cv::Vec4i> hierarchy;
        cv::findContours(img_thresh, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0,0));

        foreach(vector<cv::Point> cont, contours)
        {
            cv::Rect bb = contour_bounding_box(cont);

            if(bb.width > min_size_rect || bb.height > min_size_rect)
                rectangles.push_back(bb);
        }
        return rectangles;
    }

}


#include "dot-editor-widget.h"
#include "math-dsp.h"
#include <QPainter>
#include <QMouseEvent>
#include <QDebug>
#include <boost/optional.hpp>
#include <vector>
#include <cmath>

struct BasicDotEditorWidget::Impl {
  Impl(int xdots, int ydots) : xdots(xdots), ydots(ydots) {
    dotdata.resize(xdots);
    for (int i = 0; i < xdots; ++i) {
      double x = double(i) / (xdots - 1);
      dotdata[i] = std::sin(2.0 * M_PI * x);
    }
  }

  const int xdots {};
  const int ydots {};
  std::vector<double> dotdata;
  boost::optional<QPoint> mousepos;
};

BasicDotEditorWidget::BasicDotEditorWidget(int xdots, int ydots, QWidget *parent)
  : QWidget(parent), P(new Impl(xdots, ydots)) {
  this->setMouseTracking(true);
  this->setPrecisionCursor();
}

BasicDotEditorWidget::~BasicDotEditorWidget() {
}

void BasicDotEditorWidget::initialize() {
  QSize size = this->sizeHint();
  this->setMinimumSize(size);
  this->setMaximumSize(size);
}

void BasicDotEditorWidget::smooth(double param) {
  if (param <= 0.0 || param > 1.0)
    return;

  double adj = 100.0;  // adjust cutoff
  double s = std::exp(-1.0 / (adj * (1.0 - param)));

  for (int i = 0; i < P->xdots; ++i) {
    double input = P->dotdata[i];
    double nextinput = (i + 1 < P->xdots) ? P->dotdata[i + 1] : input;
    P->dotdata[i] = nextinput * (1.0 - s) + input * s;
  }

  this->update();
}

void BasicDotEditorWidget::window(double param) {
  for (int i = 0; i < P->xdots; ++i) {
    double x = double(i) / (P->xdots - 1);
    double w = tukey_window(param, x);
    P->dotdata[i] *= w;
  }

  this->update();
}

void BasicDotEditorWidget::shiftData(int off) {
  std::vector<double> newdata(P->xdots);
  for (int x = 0; x < P->xdots; ++x) {
    int newx = x - off;
    double s;
    if (newx < 0)
      s = P->dotdata[0];
    else if (newx >= P->xdots)
      s = P->dotdata[P->xdots - 1];
    else
      s = P->dotdata[newx];
    newdata[x] = s;
  }
  std::copy(newdata.begin(), newdata.end(), P->dotdata.begin());
  this->update();
}

void BasicDotEditorWidget::invert(int sides) {
  int xmid = P->xdots / 2;

  if (sides & LeftSide) {
    for (int i = 0; i < xmid; ++i)
      P->dotdata[i] = -P->dotdata[i];
  }
  if (sides & RightSide) {
    for (int i = xmid; i < P->xdots; ++i)
      P->dotdata[i] = -P->dotdata[i];
  }

  this->update();
}

void BasicDotEditorWidget::mirror(MirrorDir mirrordir) {
  switch (mirrordir) {
    case MirLeftToRight:
      for (int i = 0; i < P->xdots / 2; ++i)
        P->dotdata[P->xdots - i - 1] = P->dotdata[i];
      this->update();
      break;
    case MirRightToLeft:
      for (int i = 0; i < P->xdots / 2; ++i)
        P->dotdata[i] = P->dotdata[P->xdots - i - 1];
      this->update();
      break;
  }
}

const std::vector<double> &BasicDotEditorWidget::dotData() const {
  return P->dotdata;
}

std::vector<double> &BasicDotEditorWidget::dotData() {
  return P->dotdata;
}

bool BasicDotEditorWidget::inGridBounds(QPoint gridpos) const {
  int x = gridpos.x(), y = gridpos.y();
  return x >= 0 && y >= 0 && x < P->xdots && y < P->ydots;
}

void BasicDotEditorWidget::paintEvent(QPaintEvent *event) {
  QPainter painter(this);

  painter.fillRect(this->rect(), Qt::black);

  this->paintGrid(painter);
  this->paintAxes(painter);
  this->paintDots(painter);
  this->paintMouseIndicator(painter);
}

void BasicDotEditorWidget::mousePressEvent(QMouseEvent *event) {
  QPoint pos = event->pos();
  QPoint gridpos = this->toGridCoord(pos);
  bool inbounds = inGridBounds(gridpos);

  if (!inbounds) {
    P->mousepos = boost::optional<QPoint>();
    return;
  }

  if (event->buttons() & Qt::LeftButton) {
    // qDebug() << "paint dot by click" << pos;
    this->activateDot(gridpos);
    P->mousepos = pos;
    this->update();
  }
}

void BasicDotEditorWidget::mouseReleaseEvent(QMouseEvent *event) {
  P->mousepos = boost::optional<QPoint>();
}

void BasicDotEditorWidget::mouseMoveEvent(QMouseEvent *event) {
  QPoint pos = event->pos();
  QPoint gridpos = this->toGridCoord(pos);
  bool inbounds = inGridBounds(gridpos);

  if (inbounds)
    emit hoveredGridCoord(gridpos);

  if (event->buttons() & Qt::LeftButton) {
    if (P->mousepos) {
      // qDebug() << "connect dots by move" << *P->mousepos << pos;
      this->connectDots(*P->mousepos, pos);
    } else {
      // qDebug() << "paint dot by move" << pos;
      this->activateDot(gridpos);
    }
    this->update();
  }

  if (!inbounds) {
    P->mousepos = boost::optional<QPoint>();
  } else {
    P->mousepos = pos;
  }
}

void BasicDotEditorWidget::activateDot(QPoint gridpos) {
  if (!inGridBounds(gridpos))
    return;
  double val = (1.0 - double(gridpos.y()) / (P->ydots - 1)) * 2.0 - 1.0;
  P->dotdata[gridpos.x()] = val;
}

void BasicDotEditorWidget::deactivateDot(int gridx) {
  if (!inGridBounds(QPoint(gridx, 0)))
    return;
  P->dotdata[gridx] = 0.0;
}

void BasicDotEditorWidget::connectDots(QPoint mousesrc, QPoint mousedst) {
  int x0 = mousesrc.x(), x1 = mousedst.x();
  int y0 = mousesrc.y(), y1 = mousedst.y();

  if (x0 == x1) {
    if (y0 > y1)
      std::swap(y0, y1);
    for (int y = y0; y <= y1; ++y)
      this->activateDot(this->toGridCoord(QPoint(x0, y)));
    return;
  }

  if (x0 > x1) {
    std::swap(x0, x1);
    std::swap(y0, y1);
  }

  int yinc = (y0 < y1) ? +1 : -1;
  int xmax = x1 + 1;
  int ymax = y1 + yinc;

  double deltax = xmax - x0;
  double deltay = ymax - y0;
  double error = -1.0;
  double deltaerr = std::fabs(deltay / deltax);

  for (int x = x0, y = y0; x < xmax; ++x) {
    this->activateDot(this->toGridCoord(QPoint(x, y)));
    error += deltaerr;
    if (error >= 0.0) {
      y += yinc;
      error -= 1.0;
    }
  }
}

void BasicDotEditorWidget::setPrecisionCursor() {
  QPixmap cursorPic(32, 32);
  cursorPic.fill(Qt::transparent);

  QPainter painter(&cursorPic);
  painter.setPen(Qt::white);
  painter.drawLine(16, 0, 16, 31);
  painter.drawLine(0, 16, 31, 16);
  painter.end();

  this->setCursor(cursorPic);
}

void BasicDotEditorWidget::paintDots(QPainter &painter) const {
  double oldval = 0.0;
  for (int x = 0; x < P->xdots; ++x) {
    double val = P->dotdata[x];
    val = (1.0 - val) * 0.5;
    val = (val < 0.0) ? 0.0 : (val > +1.0) ? +1.0 : val;
    // val normalized to [0,1]
    int y = val * (P->ydots - 1);
    int oldy = oldval * (P->ydots - 1);

    for (int y1 = (oldy < y) ? oldy : y,
             y2 = (oldy < y) ? y : oldy;
         y1 < y2; ++y1)
      this->paintDot(x - 1, y1, Qt::red, painter);

    this->paintDot(x, y, Qt::red, painter);
    oldval = val;
  }
}

///
DotEditorWidget::DotEditorWidget(int xdots, int ydots, int dotsize, QWidget *parent)
  : BasicDotEditorWidget(xdots, ydots, parent)
  , dotsize((dotsize > 2) ? dotsize : 2) {
}

QSize DotEditorWidget::sizeHint() const {
  const QSize size(dotsize * P->xdots + 1, dotsize * P->ydots + 1);
  return size;
}

void DotEditorWidget::paintGrid(QPainter &painter) const {
  const QSize size = this->sizeHint();
  painter.setPen(Qt::gray);
  for (int x = 0; x < P->xdots; ++x)
    painter.drawLine(QLine(dotsize * x, 0, dotsize * x, size.height() - 1));
  for (int y = 0; y < P->ydots; ++y)
    painter.drawLine(QLine(0, dotsize * y, size.width() - 1, dotsize * y));
}

void DotEditorWidget::paintAxes(QPainter &painter) const {
  const QSize size = this->sizeHint();
  painter.setPen(Qt::lightGray);
  int xcenter = size.width() / 2;
  int ycenter = size.height() / 2;
  painter.drawLine(xcenter, 0, xcenter, size.height() - 1);
  painter.drawLine(0, ycenter, size.width() - 1, ycenter);
}

void DotEditorWidget::paintDot(int x, int y, QColor color, QPainter &painter) const {
  QPoint origin(x * dotsize + 1, y * dotsize + 1);
  QRect rect(origin, QSize(dotsize - 1, dotsize - 1));
  painter.fillRect(rect, color);
}

void DotEditorWidget::paintMouseIndicator(QPainter &painter) const {
  if (!P->mousepos)
    return;
  QPoint gridpos = this->toGridCoord(*P->mousepos);
  ///
}

QPoint DotEditorWidget::toGridCoord(QPoint pos) const {
  int x = pos.x() / dotsize;
  int y = pos.y() / dotsize;
  x = (x < 0) ? 0 : (x >= P->xdots) ? (P->xdots - 1) : x;
  y = (y < 0) ? 0 : (y >= P->ydots) ? (P->ydots - 1) : y;
  return QPoint(x, y);
}

///
CompactDotEditorWidget::CompactDotEditorWidget(int xdots, int ydots, QWidget *parent)
  : BasicDotEditorWidget(xdots, ydots, parent) {
}

QSize CompactDotEditorWidget::sizeHint() const {
  return QSize(P->xdots, P->ydots);
}

void CompactDotEditorWidget::paintGrid(QPainter &painter) const {
  const QSize size = this->sizeHint();
  painter.setPen(Qt::gray);
  for (int x = 0; x < P->xdots; x += 2) {
    for (int y = 0; y < P->ydots; y += 2) {
      painter.drawPoint(x, y);
    }
  }
}

void CompactDotEditorWidget::paintAxes(QPainter &painter) const {
  const QSize size = this->sizeHint();
  painter.setPen(Qt::lightGray);
  int xcenter = size.width() / 2;
  int ycenter = size.height() / 2;
  painter.drawLine(xcenter, 0, xcenter, size.height() - 1);
  painter.drawLine(0, ycenter, size.width() - 1, ycenter);
}

void CompactDotEditorWidget::paintDot(int x, int y, QColor color, QPainter &painter) const {
  painter.setPen(color);
  painter.drawPoint(x, y);
}

void CompactDotEditorWidget::paintMouseIndicator(QPainter &painter) const {
  if (!P->mousepos)
    return;
  QPoint gridpos = this->toGridCoord(*P->mousepos);
  ///
}

QPoint CompactDotEditorWidget::toGridCoord(QPoint pos) const {
  return QPoint(pos.x(), pos.y());
}


#include <Eigen/Dense>
#include "volume/Edge.hpp"
#include "utils/LOG.hpp"

const double bs_margin = 1e-7;

void Edge::initWithObs(Eigen::Vector2d obs) {
    LOG_CHECK("Init with obs, this address is: %x, size is %lu", this, this->size());
    Eigen::Vector2d front_beam = this->front().block<2, 1>(0, 0) - obs, back_beam = this->back().block<2, 1>(0, 0) - obs;
    int max_size = static_cast<int>(this->size()) - 1;
    min_dist = std::min(front_beam.norm(), back_beam.norm());
    for (int i = 1; i < max_size; i++) {
        Eigen::Vector2d vec = this->at(i).block<2, 1>(0, 0) - obs;
        min_dist = std::min(vec.norm(), min_dist);
    }
    valid = true;
}

int Edge::rotatedBinarySearch(double angle) const{
    int s = 0, e = static_cast<int>(size()) - 1, m = static_cast<int>((s + e) / 2);
    double start_angle = front().z(), mid_angle = at(m).z();
    if (front().z() < back().z())
        return binarySearch(std::make_pair(0, e), angle);
    while (s < e) {
        if (mid_angle > start_angle) {
            s = m + 1;
            start_angle = mid_angle;
        } else if (mid_angle < start_angle){
            e = m;
        } else {
            if (at(e).z() < mid_angle)
                m = e;
            else m = s;
            break;
        }
        m = static_cast<int>((s + e) / 2);
        mid_angle = at(m).z();
    }
    if (angle >= front().z() - bs_margin) {
        int id = binarySearch(std::make_pair(0, m - 1), angle);
        if (id == -2) return m;
        return id;
    }
    int id = binarySearch(std::make_pair(m, static_cast<int>(size()) - 1), angle);
    if (id == -1) return m;
    return id;
}
int Edge::binarySearch(std::pair<int, int> range, double angle) const{
    int s = range.first, e = range.second, m = static_cast<int>((s + e) / 2);
    if (angle < at(s).z() - bs_margin) return -1;
    else if (angle > at(e).z() + bs_margin) return -2;
    int range_min = std::max(1, range.first);
    double mid_angle = at(m).z();
    while (s < e) {
        if (mid_angle - bs_margin > angle) {
            e = m;
        } else if (mid_angle + bs_margin < angle) {
            s = m + 1;
        } else {
            return std::max(range_min, m);
        }
        m = static_cast<int>((s + e) / 2);
        mid_angle = at(m).z();
    }
    return std::max(range_min, s);
}

Eigen::Vector3d getIntersection(
    const Eigen::Vector2d& vec,
    const Eigen::Vector3d& _p1,
    const Eigen::Vector3d& _p2,
    const Eigen::Vector2d& obs
) {
    const Eigen::Vector2d p1 = _p1.block<2, 1>(0, 0), p2 = _p2.block<2, 1>(0, 0);
    const Eigen::Vector2d vec_line = p2 - p1;
    Eigen::Matrix2d A = Eigen::Matrix2d::Zero();
    A << -vec(1), vec(0), -vec_line(1), vec_line(0);
    double b1 = Eigen::RowVector2d(-vec(1), vec(0)) * obs;
    double b2 = Eigen::RowVector2d(-vec_line(1), vec_line(0)) * p1;
    const Eigen::Vector2d b(b1, b2);
    double det = A(0, 0) * A(1, 1) - A(0, 1) * A(1, 0);
    if (std::abs(det) < 1e-5)
        return _p1;
    const Eigen::Vector2d pt = A.inverse() * b, new_vec = pt - obs;
    double angle = atan2(new_vec(1), new_vec(0));
    Eigen::Vector3d result;
    result << pt, angle;
    return result;                   // 解交点
}

Eigen::Vector2d Edge::getRayIntersect(const Eigen::Vector3d& ray, const Eigen::Vector2d& obs) const {
    int id = rotatedBinarySearch(ray.z());
    if (id < 0) throw "Something went wrong with id < 0, which should not happen at all.\n";
    const Eigen::Vector2d p1 = at(id - 1).block<2, 1>(0, 0), p2 = at(id).block<2, 1>(0, 0);
    const Eigen::Vector2d vec_line = p2 - p1, vec = ray.block<2, 1>(0, 0);
    Eigen::Matrix2d A = Eigen::Matrix2d::Zero();
    A << -vec(1), vec(0), -vec_line(1), vec_line(0);
    double b1 = Eigen::RowVector2d(-vec(1), vec(0)) * obs;
    double b2 = Eigen::RowVector2d(-vec_line(1), vec_line(0)) * p1;
    const Eigen::Vector2d b(b1, b2);
    double det = A(0, 0) * A(1, 1) - A(0, 1) * A(1, 0);
    if (std::abs(det) < 1e-5)
        return p1 - obs;
    return A.inverse() * b - obs;
}


// flexible random number generator providing multiple distributions.
//
//  Copyright Steven Ross 2009-2014.
//
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org/libs/sort for library home page.

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <stdio.h>
#include "stdlib.h"
#include <fstream>
#include <iostream>
using namespace boost;

int main(int argc, const char ** argv) {
  random::mt19937 generator;
  random::uniform_int_distribution<unsigned> distribution;
  //defaults
  unsigned high_shift = 16;
  unsigned low_shift = 16;
  unsigned count = 1000000;
  //Reading in user arguments
  if (argc > 1)
    high_shift = atoi(argv[1]);
  if (argc > 2)
    low_shift = atoi(argv[2]);
  if (argc > 3)
    count = atoi(argv[3]);
  if (high_shift > 16)
    high_shift = 16;
  if (low_shift > 16)
    low_shift = 16;
  std::ofstream ofile;
  ofile.open("input.txt", std::ios_base::out | std::ios_base::binary |
             std::ios_base::trunc);
  if (ofile.bad()) {
    printf("could not open input.txt for writing!\n");
    return 1;
  }
  //buffering file output for speed
  unsigned uDivideFactor = 1000;
  //Skipping buffering for small files
  if (count < uDivideFactor * 100)
    uDivideFactor = count;
  unsigned * pNumbers = static_cast<unsigned *>(malloc(uDivideFactor *
                                                       sizeof(unsigned)));
  //Generating semirandom numbers
  unsigned mask = 0;
  unsigned one = 1;
  for (unsigned u = 0; u < low_shift; ++u) {
    mask += one << u;
  }
  for (unsigned u = 0; u < high_shift; ++u) {
    mask += one << (16 + u);
  }
  for (unsigned u = 0; u < count/uDivideFactor; ++u) {
    unsigned i = 0;
    for (; i< uDivideFactor; ++i) {
      pNumbers[i] = distribution(generator) & mask;
    }
    ofile.write(reinterpret_cast<char *>(pNumbers), uDivideFactor * 4 );
  }
  ofile.close();
  return 0;
}


//
// Created by kellerberrin on 19/4/21.
//

#include "kol_OntologyTypes.h"
#include "kol_SimilarityMatrix.h"

#include <fstream>
#include <boost/tokenizer.hpp>

namespace kol = kellerberrin::ontology;

//! A method for calculating term-to-term similarity for GO terms using a precomputed similarity matrix.
/*!
  This method returns the term similarity as defined by the matrix.
*/
double kol::SimilarityMatrix::calculateTermSimilarity(const std::string &goTermA, const std::string &goTermB) const {

  if (hasTerm(goTermA) && hasTerm(goTermB)) {

    auto const&[aterm, row] = *(_termToIndex.find(goTermA));
    auto const&[bterm, column] = *(_termToIndex.find(goTermB));

    return _matrix[row][column];

  } else {

    return 0.0;

  }

}

//! This method projects a set of terms into it the kernel space
/*!
  This method treats the term similarity matrix as a kernel
   and projects a set of terms into it.
*/
std::vector<double> kol::SimilarityMatrix::projectTermSet(const std::vector<std::string> &terms) const {

  std::vector<double> projectedVec(_matrix.size(), 0.0);

  for (std::size_t col = 0; col < _matrix.size(); ++col) {
    double sum = 0.0;

    for (auto const &prot : terms) {

      if (_termToIndex.find(prot) != _termToIndex.end()) {

        auto const&[term, row] = *(_termToIndex.find(prot));
        sum += _matrix.at(row).at(col);

      }

    }

    projectedVec[col] = sum;

  }
  return projectedVec;
}

void kol::SimilarityMatrix::readSimilarityMatrix(const std::string &matrix_file) {

  std::ifstream in(matrix_file.c_str());

  // Check the file stream.
  if (not in.good()) {

    return;

  }
  //Tokenizer type
  typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
  boost::char_separator<char> tab_sep("\t", "", boost::keep_empty_tokens);
  tokenizer::iterator it;
  std::string line;

  std::size_t line_count = 0;

  //first line
  getline(in, line);
  tokenizer tokens(line, tab_sep);
  it = tokens.begin();

  std::string firstTerm = *it;
  _termToIndex[firstTerm] = line_count;
  ++line_count;
  ++it;

  std::vector<double> firstRow;
  while (it != tokens.end()) {

    std::string strval = *it;
    double val = atof(strval.c_str());
    firstRow.push_back(val);
    ++it;

  }

  std::size_t nRows = firstRow.size();
  _matrix.reserve(nRows);
  _matrix.push_back(firstRow);

  while (in.good()) {

    getline(in, line);
    tokenizer tokens(line, tab_sep);
    it = tokens.begin();
    if (it == tokens.end()) { continue; }

    std::string term = *it;
    _termToIndex[term] = line_count;
    ++line_count;
    ++it;

    std::vector<double> row;
    row.reserve(nRows);
    while (it != tokens.end()) {

      std::string strval = *it;
      double val = atof(strval.c_str());
      row.push_back(val);
      ++it;

    }

    _matrix.push_back(row);

  }
  //std::cout << "rows " << cache_matrix_.size() << std::endl;
  //std::cout << "cols " << cache_matrix_.at(0).size() << std::endl;
  //std::cout << "terms " << term_to_index_.size() << std::endl;
}

bool kol::SimilarityMatrix::isFileGood(const std::string &filename) const {

  std::ifstream in(filename.c_str());
  if (not in.good()) {

    return false;

  }

  typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
  boost::char_separator<char> tab_sep("\t", "", boost::keep_empty_tokens);
  tokenizer::iterator it;
  std::string line;

  std::size_t line_count = 0;

  //first line
  getline(in, line);
  tokenizer tokens(line, tab_sep);
  it = tokens.begin();

  std::string firstTerm = *it;
  ++line_count;
  ++it;

  std::vector<double> firstRow;
  while (it != tokens.end()) {
    std::string strval = *it;
    double val = atof(strval.c_str());
    firstRow.push_back(val);
    ++it;
  }

  try {

    while (in.good() && line_count < 5) {

      getline(in, line);
      tokenizer tokens(line, tab_sep);
      it = tokens.begin();
      if (it == tokens.end()) { continue; }

      std::string term = *it;
      ++line_count;
      ++it;

      std::vector<double> row;
      row.reserve(firstRow.size());

      while (it != tokens.end()) {
        std::string strval = *it;
        double val = atof(strval.c_str());
        row.push_back(val);
        ++it;
      }

      if (row.size() != firstRow.size()) {

        return false;

      }

    }

  }
  catch (std::exception &e) {

    return false;

  }

  return true;
}


/*
 * Copyright (c) 2009-2021, Albertas Vyšniauskas
 * All rights reserved.
 * Copyright (c) 2012, David Gowers (Portions regarding adaptation of GammaModification.(cpp|h) to quantise colors instead.)
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
 *     * Neither the name of the software author nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
 * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 * OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "Quantization.h"
#include "dynv/Map.h"
#include "uiUtilities.h"
#include "I18N.h"
#include <gtk/gtk.h>
#include <math.h>
#include <string.h>
#include <boost/math/special_functions/round.hpp>
namespace transformation {
static const char *transformationId = "quantization";
const char *Quantization::getId() {
	return transformationId;
}
const char *Quantization::getName() {
	return _("Quantization");
}
void Quantization::apply(Color *input, Color *output) {
	if (clip_top) {
		float max_intensity = (value - 1) / value;
		output->rgb.red = MIN(max_intensity, boost::math::round(input->rgb.red * value) / value);
		output->rgb.green = MIN(max_intensity, boost::math::round(input->rgb.green * value) / value);
		output->rgb.blue = MIN(max_intensity, boost::math::round(input->rgb.blue * value) / value);
	} else {
		float actualmax = value - 1;
		output->rgb.red = boost::math::round(input->rgb.red * actualmax) / actualmax;
		output->rgb.green = boost::math::round(input->rgb.green * actualmax) / actualmax;
		output->rgb.blue = boost::math::round(input->rgb.blue * actualmax) / actualmax;
	}
}
Quantization::Quantization():
	Transformation(transformationId, getName()) {
	value = 16;
}
Quantization::Quantization(float value_):
	Transformation(transformationId, getName()) {
	value = value_;
}
Quantization::~Quantization() {
}
void Quantization::serialize(dynv::Map &system) {
	system.set("value", value);
	system.set("clip-top", clip_top);
	Transformation::serialize(system);
}
void Quantization::deserialize(const dynv::Map &system) {
	value = system.getFloat("value", 16);
	clip_top = system.getBool("clip-top", false);
}
std::unique_ptr<IConfiguration> Quantization::getConfiguration() {
	return std::make_unique<Configuration>(*this);
}
Quantization::Configuration::Configuration(Quantization &transformation) {
	GtkWidget *table = gtk_table_new(2, 3, false);
	GtkWidget *widget;
	int table_y = 0;
	gtk_table_attach(GTK_TABLE(table), gtk_label_aligned_new(_("Value:"), 0, 0.5, 0, 0), 0, 1, table_y, table_y + 1, GTK_FILL, GTK_FILL, 5, 5);
	value = widget = gtk_spin_button_new_with_range(2, 256, 1);
	gtk_spin_button_set_value(GTK_SPIN_BUTTON(widget), transformation.value);
	gtk_table_attach(GTK_TABLE(table), widget, 1, 2, table_y, table_y + 1, GtkAttachOptions(GTK_FILL | GTK_EXPAND), GTK_FILL, 5, 0);
	table_y++;
	clip_top = widget = gtk_check_button_new_with_label(_("Clip top-end"));
	gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(widget), transformation.clip_top);
	gtk_table_attach(GTK_TABLE(table), widget, 1, 2, table_y, table_y + 1, GtkAttachOptions(GTK_FILL | GTK_EXPAND), GTK_FILL, 5, 0);
	main = table;
	gtk_widget_show_all(main);
	g_object_ref(main);
}
Quantization::Configuration::~Configuration() {
	g_object_unref(main);
}
GtkWidget *Quantization::Configuration::getWidget() {
	return main;
}
void Quantization::Configuration::apply(dynv::Map &system) {
	system.set<float>("value", static_cast<float>(gtk_spin_button_get_value(GTK_SPIN_BUTTON(value))));
	system.set<bool>("clip-top", gtk_toggle_button_get_active(GTK_TOGGLE_BUTTON(clip_top)));
}
}


/*****************************************************************************
*
*     Program: EPTlib
*     Author: Alessandro Arduino <a.arduino@inrim.it>
*
*  MIT License
*
*  Copyright (c) 2020-2021  Alessandro Arduino
*  Istituto Nazionale di Ricerca Metrologica (INRiM)
*  Strada delle cacce 91, 10135 Torino
*  ITALY
*
*  Permission is hereby granted, free of charge, to any person obtaining a copy
*  of this software and associated documentation files (the "Software"), to deal
*  in the Software without restriction, including without limitation the rights
*  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
*  copies of the Software, and to permit persons to whom the Software is
*  furnished to do so, subject to the following conditions:
*
*  The above copyright notice and this permission notice shall be included in all
*  copies or substantial portions of the Software.
*
*  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
*  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
*  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
*  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
*  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
*  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
*  SOFTWARE.
*
*****************************************************************************/

#include "eptlib/ept_gradient.h"

#include <cmath>
#include <limits>
#include <iostream>

#include <Eigen/Dense>
#include <Eigen/Sparse>

using namespace eptlib;

namespace {
	static double nand = std::numeric_limits<double>::quiet_NaN();
	static std::complex<double> nancd = std::complex<double>(nand,nand);
}

// EPTGradient constructor
EPTGradient::
EPTGradient(const double freq, const std::array<int,NDIM> &nn,
	const std::array<double,NDIM> &dd, const int tx_ch,
	const Shape &shape, const bool is_2d) :
	EPTInterface(freq,nn,dd,tx_ch, 1),
	is_2d_(is_2d), use_seed_points_(false),
	plane_idx_(nn[2]/2), lambda_(0.0), gradient_tolerance_(0.0),
	mask_(0), seed_points_(0), fd_filter_(shape),
	epsc_(0), g_plus_(0), g_z_(0), cost_functional_(0.0),
	cost_regularization_(0.0), thereis_epsc_(false),
	run_mode_(EPTGradientRun::FULL) {
	if (tx_ch_<5) {
		throw std::runtime_error("Impossible to solve the linear system: at least 5 transmit channels are needed.");
	}
	return;
}

// EPTGradient destructor
EPTGradient::
~EPTGradient() {
	return;
}

// EPTGradient run
EPTlibError EPTGradient::
Run() {
	// Check the run mode conditions
	if (run_mode_==EPTGradientRun::GRADIENT && !thereis_epsc_) {
		return EPTlibError::MissingData;
	}
	// Set-up the output
	int n_out = is_2d_?nn_[0]*nn_[1]:n_vox_;
	if (thereis_tx_sens_.all() && thereis_trx_phase_.all()) {
		thereis_epsr_ = true;
		thereis_sigma_ = true;
		if (is_2d_) {
			epsr_ = Image<double>(nn_[0],nn_[1]);
			sigma_ = Image<double>(nn_[0],nn_[1]);
		} else {
			epsr_ = Image<double>(nn_[0],nn_[1],nn_[2]);
			sigma_ = Image<double>(nn_[0],nn_[1],nn_[2]);
		}
	} else {
		return EPTlibError::MissingData;
	}
	// Solution of the local systems
	if (run_mode_==EPTGradientRun::FULL || run_mode_==EPTGradientRun::LOCAL) {
		thereis_epsc_ = true;
		// Clear and re-allocate the intermediate results
		epsc_.clear();
		epsc_.resize(n_out,0.0);
		g_plus_.clear();
		g_plus_.resize(n_out,0.0);
		g_z_.clear();
		if (!is_2d_) {
			g_z_.resize(n_out,0.0);
		}
		// For each transmit channel as reference...
		int r2 = fd_filter_.GetShape().GetSize()[2];
		int n_int = is_2d_?nn_[0]*nn_[1]*r2:n_vox_;
		std::vector<double> totweight(n_out,0.0);
		for (int iref = 0; iref<tx_ch_; ++iref) {
			// ...allocate memory for auxiliary quantities
			std::array<std::vector<double>,NDIM> grad_phi0;
			for (int d = 0; d<NDIM; ++d) {
				grad_phi0[d].resize(n_int,::nand);
			}
			std::vector<std::complex<double> > g_plus(n_int,::nancd);
			std::vector<std::complex<double> > g_z(n_int,::nancd);
			std::vector<std::complex<double> > theta(n_int,::nancd);
			// ...perform the local recovery
			LocalRecovery(&grad_phi0,&g_plus,&g_z,&theta,iref);
			// ...estimate the complex permittivity from theta
			EPTlibError error = Theta2Epsc(&theta,grad_phi0,g_plus,g_z);
			if (error!=EPTlibError::Success) {
				return error;
			}
			// ...contribute in averaging
			for (int idx = 0; idx<n_out; ++idx) {
				int idx_tx = is_2d_?(idx+nn_[0]*nn_[1]*plane_idx_):idx;
				int idx_int = is_2d_?(idx+nn_[0]*nn_[1]*(r2/2)):idx;
				double weight = (*tx_sens_[iref])[idx_tx];
				epsc_[idx] += theta[idx_int]*weight;
				g_plus_[idx] += g_plus[idx_int]*weight;
				if (!is_2d_) {
					g_z_[idx] += g_z[idx_int]*weight;
				}
				totweight[idx] += weight;
			}
		}
		// Average the epsc, g_plus and g_z quantities
		for (int idx = 0; idx<n_out; ++idx) {
			epsc_[idx] /= totweight[idx];
			g_plus_[idx] /= totweight[idx];
			if (!is_2d_) {
				g_z_[idx] /= totweight[idx];
			}
		}
	}
	// Optimise the estimate using the gradient information
	if (run_mode_==EPTGradientRun::FULL || run_mode_==EPTGradientRun::GRADIENT) {
		GlobalMinimisation();
	}
	// Extract the electric properties
	ExtractElectricProperties();
	return EPTlibError::Success;
}

void EPTGradient::
SetRunMode(EPTGradientRun run_mode) {
	run_mode_ = run_mode;
	return;
}
EPTGradientRun EPTGradient::
GetRunMode() {
	return run_mode_;
}
bool EPTGradient::
ToggleSeedPoints() {
	use_seed_points_ = !use_seed_points_;
	return use_seed_points_;
}
void EPTGradient::
AddSeedPoint(const SeedPoint seed_point) {
	seed_points_.push_back(seed_point);
	return;
}
EPTlibError EPTGradient::
SelectSlice(const int slice_idx) {
	int r2 = fd_filter_.GetShape().GetSize()[2]/2;
	if (slice_idx<2*r2||slice_idx>nn_[2]-1-2*r2) {
		return EPTlibError::WrongDataFormat;
	}
    plane_idx_ = slice_idx;
    return EPTlibError::Success;
}
EPTlibError EPTGradient::
SetRegularizationCoefficient(const double lambda) {
	if (lambda<0.0) {
		return EPTlibError::WrongDataFormat;
	}
	lambda_ = lambda;
	return EPTlibError::Success;
}
EPTlibError EPTGradient::
SetGradientTolerance(const double gradient_tolerance) {
	if (gradient_tolerance<0.0 || gradient_tolerance>1.0) {
		return EPTlibError::WrongDataFormat;
	}
	gradient_tolerance_ = gradient_tolerance;
	return EPTlibError::Success;
}

bool EPTGradient::
SeedPointsAreUsed() {
	return use_seed_points_;
}

const boost::dynamic_bitset<>& EPTGradient::
GetMask() {
	return mask_;
}
double EPTGradient::
GetCostFunctional() {
	return cost_functional_;
}
double EPTGradient::
GetCostRegularization() {
	return cost_regularization_;
}
EPTlibError EPTGradient::
GetEpsC(std::vector<std::complex<double> > *epsc) {
	if (!thereis_epsc_) {
		return EPTlibError::MissingData;
	}
	*epsc = epsc_;
	return EPTlibError::Success;
}
EPTlibError EPTGradient::
GetGPlus(std::vector<std::complex<double> > *g_plus) {
	if (!thereis_epsc_) {
		return EPTlibError::MissingData;
	}
	*g_plus = g_plus_;
	return EPTlibError::Success;
}
EPTlibError EPTGradient::
GetGZ(std::vector<std::complex<double> > *g_z) {
	if (!thereis_epsc_ || is_2d_) {
		return EPTlibError::MissingData;
	}
	*g_z = g_z_;
	return EPTlibError::Success;
}

/////////////////////////////////////////////////////////////////////////////
// EPTGradient LOCAL RECOVERY ///////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
namespace { //details
	static constexpr int col_theta = 0;
	static constexpr int col_gplus = 2;
	static constexpr int col_phi = 4;
	static constexpr int col_gz = 7;

	// Fill the local matrix for pixel-by-pixel recovery.
	void FillLocalMatrix(Eigen::MatrixXd *A, Eigen::VectorXd *b,
		const std::vector<std::vector<std::complex<double> > > &field_crop,
		const FDSavitzkyGolayFilter &fd_filter,
		const std::array<double,NDIM> &dd, const int tx_ch, const bool is_2d) {
		int mid_vox = fd_filter.GetShape().GetBoxVolume()/2;
		int n_col = is_2d?6:9;
		*A = Eigen::MatrixXd::Zero(2*tx_ch,n_col);
		*b = Eigen::VectorXd::Zero(2*tx_ch);
		// for each transmit channel...
		for (int itx = 0; itx<tx_ch; ++itx) {
			int row = 2*itx;
			// ...laplacian
			std::complex<double> tmp = fd_filter.Laplacian(field_crop[itx],dd);
			(*b)[row] = tmp.real();
			(*b)[row+1] = tmp.imag();
			// ...grad_x
			tmp = fd_filter.FirstOrder(0,field_crop[itx],dd);
			(*A)(row,col_phi) = 2.0*tmp.imag();
			(*A)(row+1,col_phi) = -2.0*tmp.real();
			(*A)(row,col_gplus) = tmp.real();
			(*A)(row+1,col_gplus) = tmp.imag();
			(*A)(row,col_gplus+1) = -tmp.imag();
			(*A)(row+1,col_gplus+1) = tmp.real();
			// ...grad_y
			tmp = fd_filter.FirstOrder(1,field_crop[itx],dd);
			(*A)(row,col_phi+1) = 2.0*tmp.imag();
			(*A)(row+1,col_phi+1) = -2.0*tmp.real();
			(*A)(row,col_gplus) += tmp.imag();
			(*A)(row+1,col_gplus) += -tmp.real();
			(*A)(row,col_gplus+1) += tmp.real();
			(*A)(row+1,col_gplus+1) += tmp.imag();
			// ...grad_z
			if (!is_2d) {
				tmp = fd_filter.FirstOrder(2,field_crop[itx],dd);
				(*A)(row,col_phi+2) = 2.0*tmp.imag();
				(*A)(row+1,col_phi+2) = -2.0*tmp.real();
				(*A)(row,col_gz) = tmp.real();
				(*A)(row+1,col_gz) = tmp.imag();
				(*A)(row,col_gz+1) = -tmp.imag();
				(*A)(row+1,col_gz+1) = tmp.real();
			}
			// ...value
			tmp = field_crop[itx][mid_vox];
			(*A)(row,col_theta) = tmp.real();
			(*A)(row+1,col_theta) = tmp.imag();
			(*A)(row,col_theta+1) = -tmp.imag();
			(*A)(row+1,col_theta+1) = tmp.real();
		}
		return;
	}
	// Solve the local system for pixel-by-pixel recovery.
	void SolveLocalSystem(double *gradx_phi0, double *grady_phi0,
		double *gradz_phi0, std::complex<double> *g_plus,
		std::complex<double> *g_z, std::complex<double> *theta,
		Eigen::MatrixXd A, Eigen::VectorXd b, const bool is_2d) {
		// solve the system
		b = A.transpose()*b;
		A = A.transpose()*A;
		Eigen::VectorXd x = A.ldlt().solve(b);
		// distribute the result
		*theta = std::complex<double>(x[col_theta],x[col_theta+1]);
		*g_plus = std::complex<double>(x[col_gplus],x[col_gplus+1]);
		*gradx_phi0 = x[col_phi];
		*grady_phi0 = x[col_phi+1];
		if (!is_2d) {
			*gradz_phi0 = x[col_phi+2];
			*g_z = std::complex<double>(x[col_gz],x[col_gz+1]);
		}
		return;
	}
}  // namespace
// Perform the pixel-by-pixel recovery
void EPTGradient::
LocalRecovery(std::array<std::vector<double>,NDIM> *grad_phi0,
	std::vector<std::complex<double> > *g_plus,
	std::vector<std::complex<double> > *g_z,
	std::vector<std::complex<double> > *theta,
	const int iref) {
	int r2 = fd_filter_.GetShape().GetSize()[2]/2;
	if (is_2d_) {
		for (int i2 = plane_idx_-r2; i2<=plane_idx_+r2; ++i2) {
			LocalRecoverySlice(grad_phi0,g_plus,g_z,theta,iref,i2);
		}
	} else {
		for (int i2 = r2; i2<nn_[2]-r2; ++i2) {
			LocalRecoverySlice(grad_phi0,g_plus,g_z,theta,iref,i2);
		}
	}
	return;
}
// Perform pixel-by-pixel recovery in a slice.
void EPTGradient::
LocalRecoverySlice(std::array<std::vector<double>,NDIM> *grad_phi0,
	std::vector<std::complex<double> > *g_plus,
	std::vector<std::complex<double> > *g_z,
	std::vector<std::complex<double> > *theta,
	const int iref, const int i2) {
	std::array<int,NDIM> rr = fd_filter_.GetShape().GetSize();
	for (int d = 0; d<NDIM; ++d) {
		rr[d] /= 2;
	}
	std::array<int,NDIM> ii;
	std::array<int,NDIM> m_inc;
	int m_vox = fd_filter_.GetShape().GetBoxVolume();
	m_inc[0] = 1;
	m_inc[1] = nn_[0]-fd_filter_.GetShape().GetSize()[0];
	m_inc[2] = nn_[0]*(nn_[1]-fd_filter_.GetShape().GetSize()[1]);
	ii[2] = i2;
	for (ii[1] = rr[1]; ii[1]<nn_[1]-rr[1]; ++ii[1]) {
		for (ii[0] = rr[0]; ii[0]<nn_[0]-rr[0]; ++ii[0]) {
			std::array<int,NDIM> ii_l;
			std::vector<std::vector<std::complex<double> > > field_crop(tx_ch_);
			for (int itx = 0; itx<tx_ch_; ++itx) {
				field_crop[itx].resize(m_vox,0.0);
			}
			// inner loop over the kernel voxels
			int idx_l = 0;
			int idx_g = ii[0]-rr[0] + nn_[0]*(ii[1]-rr[1] + nn_[1]*(ii[2]-rr[2]));
			for (ii_l[2] = -rr[2]; ii_l[2]<=rr[2]; ++ii_l[2]) {
				for (ii_l[1] = -rr[1]; ii_l[1]<=rr[1]; ++ii_l[1]) {
					for (ii_l[0] = -rr[0]; ii_l[0]<=rr[0]; ++ii_l[0]) {
						if (fd_filter_.GetShape()[idx_l]) {
							// set the field value
							for (int itx = 0; itx<tx_ch_; ++itx) {
								field_crop[itx][idx_l] = (*tx_sens_[itx])[idx_g]*
									std::exp(std::complex<double>(0.0,(*trx_phase_[itx])[idx_g]-
									(*trx_phase_[iref])[idx_g]));
							}
						}
						++idx_l;
						idx_g += m_inc[0];
					}
					idx_g += m_inc[1];
				}
				idx_g += m_inc[2];
			}
			// fill the matrix
			Eigen::MatrixXd A;
			Eigen::VectorXd b;
			::FillLocalMatrix(&A,&b, field_crop,fd_filter_,dd_,tx_ch_,is_2d_);
			// solve the system
			int idx = ii[0] + nn_[0]*ii[1];
			if (is_2d_) {
				idx += nn_[0]*nn_[1]*(ii[2]-plane_idx_+rr[2]);
			} else {
				idx += nn_[0]*nn_[1]*ii[2];
			}
			::SolveLocalSystem(&((*grad_phi0)[0][idx]),&((*grad_phi0)[1][idx]),
				&((*grad_phi0)[2][idx]),&((*g_plus)[idx]),&((*g_z)[idx]),
				&((*theta)[idx]), A,b,is_2d_);
		}
	}
	return;
}

/////////////////////////////////////////////////////////////////////////////
// EPTGradient THETA TO COMPLEX PERMITTIVITY ////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
// Estimate the complex permittivity from theta local recovery.
EPTlibError EPTGradient::
Theta2Epsc(std::vector<std::complex<double> > *theta,
	const std::array<std::vector<double>,NDIM> &grad_phi0,
	const std::vector<std::complex<double> > &g_plus,
	const std::vector<std::complex<double> > &g_z) {
	int n_dim = is_2d_?2:3;
	int r2 = fd_filter_.GetShape().GetSize()[2];
	std::array<int,NDIM> nn{nn_[0],nn_[1],is_2d_?r2:nn_[2]};
	int n_vox = nn[0]*nn[1]*nn[2];
	int offset = is_2d_?nn[0]*nn[1]*(r2/2):0;
	// grad_phi0 laplacian...
	for (int d = 0; d<n_dim; ++d) {
		std::vector<double> lapl_phi0(n_vox);
		DifferentialOperator diff_op = static_cast<DifferentialOperator>(d);
		EPTlibError error = fd_filter_.Apply(diff_op,lapl_phi0.data(),
			grad_phi0[d].data(),nn,dd_);
		if (error!=EPTlibError::Success) {
			return error;
		}
		std::transform(theta->begin()+offset,
			theta->end()-offset,
			lapl_phi0.begin()+offset,
			theta->begin()+offset,
			[](const std::complex<double> &a,const double &b)->std::complex<double>{
				return a + std::complex<double>(0.0,b);
			});
	}
	// grad_phi0 magnitude...
	for (int d = 0; d<n_dim; ++d) {
		std::vector<double> tmp(n_vox);
		std::transform(grad_phi0[d].begin()+offset,
			grad_phi0[d].end()-offset,
			tmp.begin()+offset,
			[](const double &a)->double{
				return a*a;
			});
		std::transform(theta->begin()+offset,
			theta->end()-offset,
			tmp.begin()+offset,
			theta->begin()+offset,
			[](const std::complex<double> &a,const double &b)->std::complex<double>{
				return a-b;
			});
	}
	// grad_phi0, g inner product...
	std::vector<std::complex<double> > tmp(n_vox);
	// ...component 0
	std::transform(grad_phi0[0].begin()+offset,
		grad_phi0[0].end()-offset,
		g_plus.begin()+offset,
		tmp.begin()+offset,
		[](const double &a,const std::complex<double> &b)->std::complex<double>{
			return std::complex<double>(0.0,a)*b;
		});
	std::transform(theta->begin()+offset,
		theta->end()-offset,
		tmp.begin()+offset,
		theta->begin()+offset,
		std::minus<std::complex<double> >());
	// ...component 1
	std::transform(grad_phi0[1].begin()+offset,
		grad_phi0[1].end()-offset,
		g_plus.begin()+offset,
		tmp.begin()+offset,
		[](const double &a,const std::complex<double> &b)->std::complex<double>{
			return a*b;
		});
	std::transform(theta->begin()+offset,
		theta->end()-offset,
		tmp.begin()+offset,
		theta->begin()+offset,
		std::minus<std::complex<double> >());
	if (!is_2d_) {
		// ...component 2
		std::transform(grad_phi0[2].begin()+offset,
			grad_phi0[2].end()-offset,
			g_z.begin()+offset,
			tmp.begin()+offset,
			[](const double &a,const std::complex<double> &b)->std::complex<double>{
				return std::complex<double>(0.0,a)*b;
			});
		std::transform(theta->begin()+offset,
			theta->end()-offset,
			tmp.begin()+offset,
			theta->begin()+offset,
			std::minus<std::complex<double> >());
	}
	// multiplicative factor...
	std::transform(theta->begin()+offset,
		theta->end()-offset,
		theta->begin()+offset,
		[this](const std::complex<double> &a)->std::complex<double>{
			return -a/omega_/omega_/MU0;
		});
	return EPTlibError::Success;
}

/////////////////////////////////////////////////////////////////////////////
// EPTGradient GLOBAL OPTIMISATION //////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
namespace { //details
	static constexpr int vox_nodes = 8;
	static constexpr int quad_nodes = 4;

	// Select the NaN-free elements.
	void NaNFreeEle(std::vector<int> *ele, int *idx_ele, const int i2,
		const std::array<int,NDIM> &nn, const std::vector<int> &locstep,
		const std::vector<int> &dof, const bool is_2d) {
		int ele_nodes = is_2d?quad_nodes:vox_nodes;
		int nxy = nn[0]*nn[1];
		int idx_out = is_2d?0:nxy*i2;
		for (int i1 = 0; i1<nn[1]-1; ++i1) {
			for (int i0 = 0; i0<nn[0]-1; ++i0) {
				bool thereis_nan = false;
				for (int locid = 0; locid<ele_nodes; ++locid) {
					if (dof[idx_out+locstep[locid]]<0) {
						thereis_nan = true;
						break;
					}
				}
				if (thereis_nan) {
					(*ele)[(*idx_ele)++] = -1;
				} else {
					(*ele)[(*idx_ele)++] = idx_out;
				}
				++idx_out;
			}
			++idx_out;
		}
		return;
	}
	// Clean dof by counting to how many elements they belong.
	void CleanDoF(std::vector<int> *dof, int *n_dof, int *n_dop,
		const std::vector<int> &ele, const std::vector<int> &locstep,
		const bool is_2d) {
		int ele_nodes = is_2d?quad_nodes:vox_nodes;
		// count the elements
		std::vector<int> howmany_ele(dof->size(),0);
		for (int idx_ele = 0; idx_ele<ele.size(); ++idx_ele) {
			int idx = ele[idx_ele];
			if (idx>=0) {
				for (int locid = 0; locid<ele_nodes; ++locid) {
					++howmany_ele[idx+locstep[locid]];
				}
			}
		}
		// clean the dof
		*n_dof = 0;
		*n_dop = 0;
		for (int idx = 0; idx<dof->size(); ++idx) {
			if ((*dof)[idx]>0 && howmany_ele[idx]>0) {
				(*dof)[idx] = ++(*n_dof);
			} else {
				(*dof)[idx] = --(*n_dop);
			}
		}
		return;
	}

	// Associate each seed point to its dof.
	void SeedPointsToDoF(std::vector<std::complex<double> > *dir,
		std::vector<int> *dof, int *n_dof,
		const std::vector<SeedPoint> &seed_points,
		const std::array<int,NDIM> &nn, const int i2, const double omega,
		const bool is_2d) {
		// for each seed find the corresponding idx
		auto n_out = dof->size();
		std::vector<int> idx2sp(n_out,-1);
		for (int idx_sp = 0; idx_sp<seed_points.size(); ++idx_sp) {
			SeedPoint sp = seed_points[idx_sp];
			int idx = sp.ijk[0] + nn[0]*sp.ijk[1];
			if (!is_2d) {
				idx += nn[0]*nn[1]*sp.ijk[2];
			}
			idx2sp[idx] = idx_sp;
			(*dir)[idx_sp] = std::log(std::complex<double>(sp.epsr*EPS0,
				-sp.sigma/omega));
		}
		// correct the dof
		*n_dof = 0;
		for (int idx = 0; idx<n_out; ++idx) {
			if ((*dof)[idx]>0) {
				if (idx2sp[idx]<0) {
					(*dof)[idx] = ++(*n_dof);
				} else {
					(*dof)[idx] = -idx2sp[idx]-1;
				}
			} else {
				(*dof)[idx] = 0;
			}
		}
		return;
	}

	// Fill the global matrix for minimisation.
	void FillGlobalMatrix(Eigen::SparseMatrix<std::complex<double> > *A,
		Eigen::SparseMatrix<std::complex<double> > *B,
		Eigen::SparseMatrix<std::complex<double> > *Bout, Eigen::VectorXcd *b,
		Eigen::VectorXcd *b_dir, Eigen::VectorXcd *l, const std::vector<int> &dof,
		const int n_dof, const std::vector<int> &ele,
		const std::vector<int> &locstep,
		const std::vector<std::complex<double> > &epsc,
		const std::vector<std::complex<double> > &g_plus,
		const std::vector<std::complex<double> > &g_z,
		const std::array<double,NDIM> &dd, const bool is_2d,
		const bool use_seed_points_,
		const std::vector<std::complex<double> > &dir,
		const boost::dynamic_bitset<> &mask) {
		const int ele_nodes = is_2d?quad_nodes:vox_nodes;
		const int n_dim = is_2d?2:3;
		std::vector<Eigen::Triplet<std::complex<double> > > A_trip(0);
		std::vector<Eigen::Triplet<std::complex<double> > > B_trip(0);
		std::vector<Eigen::Triplet<std::complex<double> > > Bout_trip(0);
		*b = Eigen::VectorXcd::Zero(n_dof);
		*b_dir = Eigen::VectorXcd::Zero(n_dof);
		*l = Eigen::VectorXcd::Zero(n_dof);
		// loop the elements
		for (int idx_ele = 0; idx_ele<ele.size(); ++idx_ele) {
			int idx = ele[idx_ele];
			if (idx<0) {
				continue;
			}
			// loop the equation index
			for (int i = 0; i<ele_nodes; ++i) {
				int iidx = idx+locstep[i];
				int ii = dof[iidx];
				if (ii>0) {
					--ii;
					// loop the unknown index
					for (int j = 0; j<ele_nodes; ++j) {
						int jidx = idx+locstep[j];
						int jj = dof[jidx];
						std::array<bool,NDIM> same_side;
						for (int d = 0; d<NDIM; ++d) {
							same_side[d] = (i&(1<<d))==(j&(1<<d));
						}
						// matrix B and vector l
						std::complex<double> Bij = 1.0;
						for (int d = 0; d<n_dim; ++d) {
							Bij *= same_side[d] ? dd[d]/3.0 : dd[d]/6.0;
						}
						if (use_seed_points_ || !mask[idx_ele]) {
							if (jj>0) {
								Bout_trip.push_back(Eigen::Triplet<std::complex<double> >(ii,jj-1,Bij));
							}
						} else {
							B_trip.push_back(Eigen::Triplet<std::complex<double> >(ii,jj-1,Bij));
							(*l)[ii] += Bij*std::log(epsc[jidx]);
						}
						// matrix A
						std::complex<double> Aij = 0.0;
						for (int d1 = 0; d1<n_dim; ++d1) {
							double tmp = 1.0;
							for (int d = 0; d<n_dim; ++d) {
								if (d==d1) {
									tmp *= same_side[d] ? 1.0/dd[d] : -1.0/dd[d];
								} else {
									tmp *= same_side[d] ? dd[d]/3.0 : dd[d]/6.0;
								}
							}
							Aij += tmp;
						}
						{
							double tmp = 1.0;
							tmp *= i&(1<<0) ? 0.5 : -0.5;
							tmp *= j&(1<<1) ? 0.5 : -0.5;
							if (!is_2d) {
								tmp *= same_side[2] ? dd[2]/3.0 : dd[2]/6.0;
							}
							Aij += std::complex<double>(0.0,tmp);
						}
						{
							double tmp = -1.0;
							tmp *= j&(1<<0) ? 0.5 : -0.5;
							tmp *= i&(1<<1) ? 0.5 : -0.5;
							if (!is_2d) {
								tmp *= same_side[2] ? dd[2]/3.0 : dd[2]/6.0;
							}
							Aij += std::complex<double>(0.0,tmp);
						}
						if (jj>0) {
							A_trip.push_back(Eigen::Triplet<std::complex<double> >(ii,jj-1,Aij));
						} else {
							(*b_dir)[ii] += -Aij*dir[-jj-1];
						}
						// vector b
						for (int d1 = 0; d1<n_dim; ++d1) {
							double tmp = 1.0;
							for (int d = 0; d<n_dim; ++d) {
								if (d==d1) {
									tmp *= i&(1<<d) ? 0.5 : -0.5;
								} else {
									tmp *= same_side[d] ? dd[d]/3.0 : dd[d]/6.0;
								}
							}
							if (d1==0) {
								(*b)[ii] += tmp*g_plus[jidx];
							} else if (d1==1) {
								(*b)[ii] += -std::complex<double>(0.0,tmp)*g_plus[jidx];
							} else {
								(*b)[ii] += tmp*g_z[jidx];
							}
						}
					}
				}
			}
		}
		(*A).setFromTriplets(A_trip.begin(),A_trip.end());
		(*A).makeCompressed();
		(*B).setFromTriplets(B_trip.begin(),B_trip.end());
		(*B).makeCompressed();
		(*Bout).setFromTriplets(Bout_trip.begin(),Bout_trip.end());
		(*Bout).makeCompressed();
		return;
	}
	// Solve the global system for minimisation.
	void SolveGlobalSystem(Eigen::VectorXcd *x,
		const Eigen::SparseMatrix<std::complex<double> > &A,
		const Eigen::VectorXcd &b) {
		Eigen::ConjugateGradient<Eigen::SparseMatrix<std::complex<double> > > solver;
		solver.compute(A);
		*x = solver.solve(b);
		return;
	}

	// Set a gradient threshold.
	double SetGradientThreshold(const double relative_threshold,
		const std::vector<std::complex<double> > &g_plus,
		const std::vector<std::complex<double> > &g_z, const bool is_2d) {
		double Gmax = 0;
		// loop the g_plus values
		for (int idx = 0; idx<g_plus.size(); ++idx) {
			if (g_plus[idx]==g_plus[idx]) {
				double G = std::norm(g_plus[idx]);
				if (!is_2d) {
					G += std::norm(g_z[idx]);
				}
				if (G>Gmax) {
					Gmax = G;
				}
			}
		}
		return relative_threshold*Gmax;
	}
	// Fill the gradient mask.
	void FillGradientMask(boost::dynamic_bitset<> *mask,
		const double threshold, const std::vector<int> &ele,
		const std::vector<int> &locstep,
		const std::vector<std::complex<double> > &g_plus,
		const std::vector<std::complex<double> > &g_z, const bool is_2d) {
		const int ele_nodes = is_2d?quad_nodes:vox_nodes;
		const int n_dim = is_2d?2:3;
		// loop the elements
		for (int idx_ele = 0; idx_ele<ele.size(); ++idx_ele) {
			int idx = ele[idx_ele];
			if (idx<0) {
				continue;
			}
			mask->set(idx_ele,true);
			// loop the equation index
			for (int i = 0; i<ele_nodes; ++i) {
				int iidx = idx+locstep[i];
				double G = std::norm(g_plus[iidx]);
				if (!is_2d) {
					G += std::norm(g_z[iidx]);
				}
				if (G>threshold) {
					mask->set(idx_ele,false);
					break;
				}
			}
		}
		return;
	}
	// Evaluate the costs functional terms.
	void EvaluateCostFunctional(double *cost_functional,
		double *cost_regularization, const Eigen::VectorXcd &x,
		const Eigen::SparseMatrix<std::complex<double> > &A,
		const Eigen::SparseMatrix<std::complex<double> > &B,
		const Eigen::SparseMatrix<std::complex<double> > &Bout,
		const Eigen::VectorXcd &b, const std::vector<int> &dof,
		const int n_dof, const std::vector<std::complex<double> > &epsc,
		const std::vector<std::complex<double> > &g_plus,
		const std::vector<std::complex<double> > &g_z, const bool is_2d) {
		const auto n_vox = dof.size();
		Eigen::VectorXcd gp(n_dof);
		Eigen::VectorXcd gz(is_2d?0:n_dof);
		Eigen::VectorXcd x0(n_dof);
		for (int idx = 0; idx<n_vox; ++idx) {
			if (dof[idx]>0) {
				gp[dof[idx]-1] = g_plus[idx];
				if (!is_2d) {
					gz[dof[idx]-1] = g_z[idx];
				}
				x0[dof[idx]-1] = std::log(epsc[idx]);
			}
		}
		double norm2_Dpu = std::real(std::complex<double>(x.adjoint()*(A*x)));
		double norm2_gp = std::real(std::complex<double>(gp.adjoint()*((B+Bout)*gp)));
		*cost_functional = norm2_Dpu + norm2_gp - 2.0*std::real(std::complex<double>(x.adjoint()*b));
		if (!is_2d) {
			*cost_functional += std::real(std::complex<double>(gz.adjoint()*((B+Bout)*gz)));
		}
		*cost_regularization = std::real(std::complex<double>(x.adjoint()*(B*(x-2.0*x0)))+std::complex<double>(x0.adjoint()*(B*x0)));
		return;
	}
}  // namespace
// Select the degree of freedom.
void EPTGradient::
SelectDoF(std::vector<int> *dof, std::vector<int> *ele, int *n_dof,
	const std::vector<int> &locstep) {
	// select the dof
	int n_dop = 0;
	*n_dof = 0;
	for (int idx = 0; idx<dof->size(); ++idx) {
		if (epsc_[idx]==epsc_[idx]) {
			(*dof)[idx] = ++(*n_dof);
		} else {
			(*dof)[idx] = --n_dop;
		}
	}
	// determine the nan-free elements
	int idx_ele = 0;
	if (is_2d_) {
		::NaNFreeEle(ele,&idx_ele, 0,nn_,locstep,*dof,is_2d_);
	} else {
		for (int i2 = 0; i2<nn_[2]-1; ++i2) {
			::NaNFreeEle(ele,&idx_ele, i2,nn_,locstep,*dof,is_2d_);
		}
	}
	// clean the dof by counting to how many elements they belong
	::CleanDoF(dof,n_dof,&n_dop, *ele,locstep,is_2d_);
	return;
}
// Solve the global minimisation problem.
void EPTGradient::
GlobalMinimisation() {
	const int ele_nodes = is_2d_?quad_nodes:vox_nodes;
	const int n_dim = is_2d_?2:3;
	const std::array<int,NDIM> step{1,nn_[0],nn_[0]*nn_[1]};
	const int n_out = is_2d_?step[2]:n_vox_;
	// define the computational elements
	std::vector<int> locstep(ele_nodes,0);
	for (int locid = 0; locid<ele_nodes; ++locid) {
		for (int d = 0; d<n_dim; ++d) {
			locstep[locid] += locid&(1<<d) ? step[d] : 0;
		}
	}
	// select the degrees of freedom
	int n_ele = (nn_[0]-1)*(nn_[1]-1);
	if (!is_2d_) {
		n_ele *= (nn_[2]-1);
	}
	std::vector<int> dof(n_out);
	std::vector<int> ele(n_ele);
	int ndof;
	SelectDoF(&dof,&ele,&ndof,locstep);
	// set the dirichlet values in case of seed points...
	std::vector<std::complex<double> > dir(seed_points_.size());
	if (use_seed_points_) {
		::SeedPointsToDoF(&dir,&dof,&ndof, seed_points_,nn_,plane_idx_,omega_,is_2d_);
	}
	// otherwise set the mask
	if (!use_seed_points_) {
		mask_.resize(ele.size(),false);
		double threshold = ::SetGradientThreshold(gradient_tolerance_,g_plus_,g_z_,is_2d_);
		::FillGradientMask(&mask_, threshold,ele,locstep,g_plus_,g_z_,is_2d_);
	}
	// create the matrix of coefficients and the forcing term
	Eigen::SparseMatrix<std::complex<double> > A(ndof,ndof);
	Eigen::SparseMatrix<std::complex<double> > B(ndof,ndof);
	Eigen::SparseMatrix<std::complex<double> > Bout(ndof,ndof); // Bout accounts for the masked out mass contributions
	Eigen::VectorXcd b;
	Eigen::VectorXcd b_dir;
	Eigen::VectorXcd l;
	::FillGlobalMatrix(&A,&B,&Bout,&b,&b_dir,&l, dof,ndof,ele,locstep,epsc_,g_plus_,g_z_,
		dd_,is_2d_, use_seed_points_,dir,mask_);
	// solve the linear system
	Eigen::VectorXcd x(ndof);
	if (!use_seed_points_) {
		::SolveGlobalSystem(&x, A+lambda_*B,b+lambda_*l);
	} else {
		::SolveGlobalSystem(&x, A,b+b_dir);
	}
	// evaluate the cost functional terms
	::EvaluateCostFunctional(&cost_functional_,&cost_regularization_, x,A,B,Bout,b,
		dof,ndof,epsc_,g_plus_,g_z_,is_2d_);
	// extract electric properties
	for (int idx = 0; idx<n_out; ++idx) {
		if (dof[idx]>0) {
			epsc_[idx] = std::exp(x[dof[idx]-1]);
		} else {
			if (use_seed_points_ && dof[idx]<0) {
				epsc_[idx] = std::exp(dir[-dof[idx]-1]);
			} else {
				epsc_[idx] = ::nancd;
			}
		}
	}
	return;
}

// Extract the electric properties of a slice.
void EPTGradient::
ExtractElectricProperties() {
	for (int idx = 0; idx<epsc_.size(); ++idx) {
		epsr_[idx] = epsc_[idx].real()/EPS0;
		sigma_[idx] = -epsc_[idx].imag()*omega_;
	}
	return;
}


/**
 * @file RtMatrix.hpp
 * @author Alexander Herzog
 * @author Brahayam Ponton (brahayam.ponton@tuebingen.mpg.de)
 * @license License BSD-3-Clause
 * @copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft.
 * @date 2019-10-06
 */

#pragma once

#include <Eigen/Dense>

namespace rt_solver {

  // Class for creating a dynamic matrix with a pre-allocated maximum size
  template<int _MaxRows, int _MaxCols>
  struct RtMatrix
  {
    #ifdef RTEIG_DYN_ALLOCS
      typedef Eigen::MatrixXd d;
    #else
      static const int min_possible_elements_ = 2;
      typedef Eigen::Matrix<double, Eigen::Dynamic,
                            Eigen::Dynamic, Eigen::AutoAlign,
                            (_MaxRows>=min_possible_elements_)?_MaxRows:min_possible_elements_,
                            (_MaxCols>=min_possible_elements_)?_MaxCols:min_possible_elements_> d;
      typedef Eigen::Matrix<float, Eigen::Dynamic,
                            Eigen::Dynamic, Eigen::AutoAlign,
                            (_MaxRows>=min_possible_elements_)?_MaxRows:min_possible_elements_,
                            (_MaxCols>=min_possible_elements_)?_MaxCols:min_possible_elements_> f;
      typedef Eigen::Matrix<int, Eigen::Dynamic,
                            Eigen::Dynamic, Eigen::AutoAlign,
                            (_MaxRows>=min_possible_elements_)?_MaxRows:min_possible_elements_,
                            (_MaxCols>=min_possible_elements_)?_MaxCols:min_possible_elements_> i;
    #endif
  };

  template<int _MaxRows, int _MaxCols>
  const int RtMatrix<_MaxRows, _MaxCols>::min_possible_elements_;

  // Class for creating a dynamic vector with a pre-allocated maximum size
  template<int _MaxRows>
  struct RtVector
  {
    #ifdef RTEIG_DYN_ALLOCS
      typedef Eigen::VectorXd d;
    #else
      static const int min_possible_elements_ = 2;
      typedef Eigen::Matrix<double, Eigen::Dynamic, 1, Eigen::AutoAlign,
                            (_MaxRows>=min_possible_elements_)?_MaxRows:min_possible_elements_, 1> d;
      typedef Eigen::Matrix<float, Eigen::Dynamic, 1, Eigen::AutoAlign,
                            (_MaxRows>=min_possible_elements_)?_MaxRows:min_possible_elements_, 1> f;
      typedef Eigen::Matrix<int, Eigen::Dynamic, 1, Eigen::AutoAlign,
                            (_MaxRows>=min_possible_elements_)?_MaxRows:min_possible_elements_, 1> i;
      typedef Eigen::Matrix<bool, Eigen::Dynamic, 1, Eigen::AutoAlign,
                            (_MaxRows>=min_possible_elements_)?_MaxRows:min_possible_elements_, 1> b;
    #endif
  };

  template<int _MaxRows>
  const int RtVector<_MaxRows>::min_possible_elements_;

  // Class with utilities for Rt elements
  class RtMatrixUtils
  {
    private:
      RtMatrixUtils(){}
      virtual ~RtMatrixUtils(){}

    public:

      // creates a selector matrix based on entries of a selector vector which are more than specified threshold
      template<int MaxRows, int MaxCols>
      static inline void createSelectorMat(typename RtMatrix<MaxRows, MaxCols>::d& selector_mat,
                                           const typename RtVector<MaxRows>::d& row_selector, int num_valids, double threshold)
      {
        RtMatrixUtils::setZero(selector_mat, num_valids, row_selector.size());
        int row = 0;
        for (int col=0; col<row_selector.size(); col++)
          if (row_selector[col] > threshold)
            selector_mat(row++,col) = 1.0;
      }

      // pruning rows of matrix which are close to zero (squared norm row < threshold)
      template<int MaxRows, int MaxCols>
      static inline void removeZeroRows(typename RtMatrix<MaxRows, MaxCols>::d& mat, double threshold = remove_zero_thresh_)
      {
        if(mat.rows() == 0)
          return;

        typename RtVector<MaxRows>::d squared_norms;
        squared_norms = mat.rowwise().squaredNorm();
        const double normed_threash = squared_norms.size() * threshold * threshold;
        int num_valids = 0;
        for (int row_id=1; row_id<squared_norms.size(); row_id++)
          if(squared_norms[row_id] > normed_threash)
            ++num_valids;

        if(num_valids == mat.rows()) { return; }
        else if (num_valids == 0) { RtMatrixUtils::resize(mat, 0, mat.cols()); }
        else
        {
          typename RtMatrix<MaxRows, MaxCols>::d selector_mat;
          createSelectorMat(selector_mat, squared_norms, num_valids, normed_threash); // create selector matrix to prune zero rows
          mat = selector_mat * mat;
        }
      }

      /**!
       * Resize matrix by keeping data untouched without memory allocation.
       * Asserts that maximum size is not exceeded.
       * @param mat
       * @param rows
       * @param cols
       */
      template <typename Derived>
      static inline void conservativeResize(Eigen::MatrixBase<Derived>& mat, int rows, int cols)
      {
        #ifndef RTEIG_NO_ASSERTS
          assert(rows <= mat.MaxRowsAtCompileTime && cols <= mat.MaxColsAtCompileTime);
        #endif
        mat.derived().conservativeResize(rows, cols);
      }

      /**!
       * Resize matrix without memory allocation. Asserts that maximum size is not exceeded.
       * @param mat
       * @param rows
       * @param cols
       */
      template <typename Derived>
      static inline void resize(Eigen::MatrixBase<Derived>& mat, int rows, int cols)
      {
        #ifndef RTEIG_NO_ASSERTS
          assert(rows <= mat.MaxRowsAtCompileTime && cols <= mat.MaxColsAtCompileTime);
        #endif
        mat.derived().resize(rows, cols);
      }

      template <typename Derived>
      static inline void setZero(Eigen::MatrixBase<Derived>& mat)
      {
        #ifdef RTEIG_DYN_ALLOCS
          resize(mat,1, 1);
        #else
          resize(mat, mat.MaxRowsAtCompileTime, mat.MaxColsAtCompileTime);
        #endif
        mat.derived().setZero();
      }

      template <typename Derived>
      static inline void setZero(Eigen::MatrixBase<Derived>& mat, int rows, int cols)
      {
        resize(mat, rows, cols);
        mat.derived().setZero();
      }

      template <typename Derived>
      static inline void setOnes(Eigen::MatrixBase<Derived>& mat)
      {
        #ifdef RTEIG_DYN_ALLOCS
          resize(mat,1, 1);
        #else
          resize(mat, mat.MaxRowsAtCompileTime, mat.MaxColsAtCompileTime);
        #endif
        mat.derived().setOnes();
      }

      template <typename Derived>
      static inline void setOnes(Eigen::MatrixBase<Derived>& mat, int rows, int cols)
      {
        resize(mat, rows, cols);
        mat.derived().setOnes();
      }

      template <typename Derived>
      static inline void setConstant(Eigen::MatrixBase<Derived>& mat, double scalar)
      {
        #ifdef RTEIG_DYN_ALLOCS
          resize(mat,1, 1);
        #else
          resize(mat, mat.MaxRowsAtCompileTime, mat.MaxColsAtCompileTime);
        #endif
        mat.derived().setConstant(scalar);
      }

      template <typename Derived>
      static inline void setConstant(Eigen::MatrixBase<Derived>& mat, int rows, int cols, double scalar)
      {
        resize(mat, rows, cols);
        mat.derived().setConstant(scalar);
      }

      template <typename Derived>
      static inline void setIdentity(Eigen::MatrixBase<Derived>& mat)
      {
        #ifndef RTEIG_NO_ASSERTS
          assert(mat.MaxRowsAtCompileTime == mat.MaxColsAtCompileTime);
        #endif
        #ifdef RTEIG_DYN_ALLOCS
          resize(mat,1, 1);
        #else
          resize(mat, mat.MaxRowsAtCompileTime, mat.MaxRowsAtCompileTime);
        #endif
        mat.derived().setIdentity();
      }

      template <typename Derived>
      static inline void setIdentity(Eigen::MatrixBase<Derived>& mat, int rows)
      {
        resize(mat, rows, rows);
        mat.derived().setIdentity();
      }

      // Append a matrix to another one
      template <typename AffMatDerived, typename SubstMatDerived>
      static inline void append(Eigen::MatrixBase<AffMatDerived>& aff_mat, const Eigen::MatrixBase<SubstMatDerived>& subst_mat)
      {
        #ifndef RTEIG_NO_ASSERTS
          assert(subst_mat.cols() == aff_mat.cols() && aff_mat.rows() + subst_mat.rows() <= aff_mat.MaxRowsAtCompileTime);
        #endif
        const int last_row = aff_mat.rows();
        if (last_row == 0) { aff_mat = subst_mat; }
        else {
          RtMatrixUtils::conservativeResize(aff_mat, aff_mat.rows() + subst_mat.rows(), aff_mat.cols());
          aff_mat.block(last_row, 0, subst_mat.rows(), subst_mat.cols()) = subst_mat;
        }
      }

      /**!
       * Append a matrix at the end of another one starting from a certain column.
       * AffMat and SubstMat are not required to have the same number of columns.
       */
      template <typename AffMatDerived, typename SubstMatDerived>
      static inline void appendAtColumn(Eigen::MatrixBase<AffMatDerived>& aff_mat, const Eigen::MatrixBase<SubstMatDerived>& subst_mat, int starting_column)
      {
        #ifndef RTEIG_NO_ASSERTS
          assert(subst_mat.cols() + starting_column <= aff_mat.cols() && aff_mat.rows() + subst_mat.rows() <= aff_mat.MaxRowsAtCompileTime);
        #endif
        const int last_row = aff_mat.rows();
        if (last_row == 0) { RtMatrixUtils::resize(aff_mat, aff_mat.rows() + subst_mat.rows(), aff_mat.cols()); }
        else { RtMatrixUtils::conservativeResize(aff_mat, aff_mat.rows() + subst_mat.rows(), aff_mat.cols()); }
        aff_mat.block(last_row, 0, subst_mat.rows(), aff_mat.cols()).setZero();
        aff_mat.block(last_row, starting_column, subst_mat.rows(), subst_mat.cols()) = subst_mat;
      }

      template<typename sigmaPinvDer, typename svdMatDer>
      static void computeSigmaPinv(Eigen::MatrixBase<sigmaPinvDer>& sigma_pinv_vec, const Eigen::JacobiSVD<svdMatDer>& mat_svd);

      template<int _mat_rows, int _mat_cols, int _mat_align, int max_mat_rows, int max_mat_cols, typename nullDer>
      static int computeNullspaceMap(
        const Eigen::Matrix<double, _mat_rows, _mat_cols, _mat_align, max_mat_rows, max_mat_cols>& mat, Eigen::MatrixBase<nullDer>& null_map,
        const Eigen::JacobiSVD<typename RtMatrix<max_mat_rows, max_mat_cols>::d >& mat_svd, const double mat_cond_threas = mat_condition_thresh_);

      template<typename Mat>
      static inline double conditionNumber(const Mat& mat)
      {
        Eigen::JacobiSVD<Mat> mat_svd;
        mat_svd.compute(mat);
        return conditionNumberFromSvd(mat_svd);
      }

      template<typename Mat, int QRPreconditioner>
      static inline double conditionNumberFromSvd(const Eigen::JacobiSVD<Mat, QRPreconditioner>& mat_svd)
      { return mat_svd.singularValues()[0]/mat_svd.singularValues()[mat_svd.singularValues().size()-1]; }

      static const double remove_zero_thresh_;
      static const double mat_condition_thresh_;

  };

  template<typename sigmaPinvDer, typename svdMatDer>
  void RtMatrixUtils::computeSigmaPinv(Eigen::MatrixBase<sigmaPinvDer>& sigma_pinv_vec, const Eigen::JacobiSVD<svdMatDer>& mat_svd)
  {
    #ifndef RTEIG_NO_ASSERTS
      assert(sigma_pinv_vec.rows() == mat_svd.singularValues().size() && sigma_pinv_vec.cols() == 1 && "sigma_pinv_vec wrongly sized");
    #endif

    const double min_sing_val = mat_svd.singularValues()[0]/mat_condition_thresh_;
    int id;
    for (id=0; id<mat_svd.singularValues().size(); id++)
      if (mat_svd.singularValues()[id]>min_sing_val) { sigma_pinv_vec[id] = 1.0/mat_svd.singularValues()[id]; }
      else                                           { sigma_pinv_vec[id] = 0.0; break; }
    sigma_pinv_vec.segment(id+1, sigma_pinv_vec.size()-id);
  }

  template<int _mat_rows, int _mat_cols, int _mat_align, int max_mat_rows, int max_mat_cols, typename nullDer>
  int RtMatrixUtils::computeNullspaceMap(
    const Eigen::Matrix<double, _mat_rows, _mat_cols, _mat_align, max_mat_rows, max_mat_cols>& mat, Eigen::MatrixBase<nullDer>& null_map,
    const Eigen::JacobiSVD<typename RtMatrix<max_mat_rows, max_mat_cols>::d >& mat_svd, const double mat_cond_threas)
  {
    int dim_range = 0;

    const double first_sv = mat_svd.singularValues()[0];
    for(dim_range=0; dim_range<mat_svd.singularValues().size(); ++dim_range)
      if (mat_svd.singularValues()[dim_range] <= 0.0 || first_sv / mat_svd.singularValues()[dim_range] > mat_cond_threas)
        break;

    const int dim_null = mat.cols() - dim_range;
    RtMatrixUtils::resize(null_map, mat.cols(), dim_null);  //asserts that null_map has enough space

    if (null_map.cols() > 0)
      null_map = mat_svd.matrixV().rightCols(dim_null);

    #ifndef RTEIG_NO_ASSERTS
      assert((mat * null_map).norm() < 0.0001 && "Nullspace map is not correctly computed");
      typename RtMatrix<max_mat_cols, max_mat_cols>::d kernel_id;
      RtMatrixUtils::setIdentity(kernel_id, dim_null);
      assert((null_map.transpose() * null_map - kernel_id).norm() < 0.0001 && "Nullspace map is not correctly computed");
    #endif
    return dim_range;
  }

  class RtVectorUtils
  {
    private:
      RtVectorUtils(){}
      virtual ~RtVectorUtils(){}

    public:
      template <typename Derived>
      static inline void conservativeResize(Eigen::MatrixBase<Derived>& vec, int rows)
      {
        #ifndef RTEIG_NO_ASSERTS
          assert(rows <= vec.MaxSizeAtCompileTime);
        #endif
        vec.derived().conservativeResize(rows);
      }

      template <typename Derived>
      static inline void resize(Eigen::MatrixBase<Derived>& vec, int rows)
      {
        #ifndef RTEIG_NO_ASSERTS
          assert(rows <= vec.MaxSizeAtCompileTime);
        #endif
        vec.derived().resize(rows);
      }

      template <typename Derived>
      static inline void setZero(Eigen::MatrixBase<Derived>& vec)
      {
        #ifdef RTEIG_DYN_ALLOCS
          resize(vec, 1);
        #else
          resize(vec, vec.MaxSizeAtCompileTime);
        #endif
        vec.derived().setZero();
      }

      template <typename Derived>
      static inline void setZero(Eigen::MatrixBase<Derived>& vec, int size)
      {
        resize(vec, size);
        vec.derived().setZero();
      }

      template <typename Derived>
      static inline void setOnes(Eigen::MatrixBase<Derived>& vec)
      {
        #ifdef RTEIG_DYN_ALLOCS
          resize(vec, 1);
        #else
          resize(vec, vec.MaxSizeAtCompileTime);
        #endif
        vec.derived().setOnes();
      }

      template <typename Derived>
      static inline void setOnes(Eigen::MatrixBase<Derived>& vec, int size)
      {
        resize(vec, size);
        vec.derived().setOnes();
      }

      template <typename Derived>
      static inline void setConstant(Eigen::MatrixBase<Derived>& vec, double scalar)
      {
        #ifdef RTEIG_DYN_ALLOCS
          resize(vec, 1);
        #else
          resize(vec, vec.MaxSizeAtCompileTime);
        #endif
        vec.derived().setConstant(scalar);
      }

      template <typename Derived>
      static inline void setConstant(Eigen::MatrixBase<Derived>& vec, int size, double scalar)
      {
        resize(vec, size);
        vec.derived().setConstant(scalar);
      }

      // Append a vector to another one
      template <typename AffVecDerived, typename SubstVecDerived>
      static inline void append(Eigen::MatrixBase<AffVecDerived>& aff_vec, const Eigen::MatrixBase<SubstVecDerived>& subst_vec)
      {
        #ifndef RTEIG_NO_ASSERTS
          assert(aff_vec.rows() + subst_vec.rows() <= aff_vec.MaxSizeAtCompileTime);
        #endif
        const int last_row = aff_vec.rows();
        RtVectorUtils::conservativeResize(aff_vec, aff_vec.rows() + subst_vec.rows());
        aff_vec.block(last_row, 0, subst_vec.rows(), 1) = subst_vec;
      }
  };

  class RtAffineUtils
  {
    private:
      RtAffineUtils(){}
      virtual ~ RtAffineUtils(){}

    public:

      // Append an affine mapping at the end of another one
      template <typename AffMatDerived, typename AffVecDerived, typename SubstMatDerived>
      static inline void append(Eigen::MatrixBase<AffMatDerived>& aff_mat,
        Eigen::MatrixBase<AffVecDerived>& aff_vec, const Eigen::MatrixBase<SubstMatDerived>& subst_mat)
      {
        #ifndef RTEIG_NO_ASSERTS
          assertMatchingDimensions(aff_mat, aff_vec);
          assert(aff_vec.rows() + subst_mat.rows() <= aff_vec.MaxSizeAtCompileTime);
        #endif
        const int last_row = aff_vec.rows();
        RtVectorUtils::conservativeResize(aff_vec, aff_vec.rows() + subst_mat.rows());
        RtMatrixUtils::append(aff_mat, subst_mat);
        aff_vec.block(last_row, 0, subst_mat.rows(),1).setZero();
      }

      template <typename AffMatDerived, typename AffVecDerived, typename SubstMatDerived, typename SubstVecDerived>
      static inline void append(Eigen::MatrixBase<AffMatDerived>& aff_mat, Eigen::MatrixBase<AffVecDerived>& aff_vec,
        const Eigen::MatrixBase<SubstMatDerived>& subst_mat, const Eigen::MatrixBase<SubstVecDerived>& subst_vec)
      {
        #ifndef RTEIG_NO_ASSERTS
          assertMatchingDimensions(aff_mat, aff_vec);
          assertMatchingDimensions(subst_mat, subst_vec);
        #endif

        const int last_row = aff_vec.rows();
        append(aff_mat, aff_vec, subst_mat);
        aff_vec.block(last_row, 0, subst_mat.rows(),1) = subst_vec;
      }

      /**!
       * Append an affine mapping at the end of another one starting from a certain column.
       * AffMat and SubstMat are not required to have the same number of columns.
       */
      template <typename AffMatDerived, typename AffVecDerived, typename SubstMatDerived>
      static inline void appendAtColumn(Eigen::MatrixBase<AffMatDerived>& aff_mat, Eigen::MatrixBase<AffVecDerived>& aff_vec,
        const Eigen::MatrixBase<SubstMatDerived>& subst_mat, int starting_column)
      {
        #ifndef RTEIG_NO_ASSERTS
          assertMatchingDimensions(aff_mat, aff_vec);
          assert(aff_vec.rows() + subst_mat.rows() <= aff_vec.MaxSizeAtCompileTime);
        #endif
        const int last_row = aff_vec.rows();
        RtVectorUtils::conservativeResize(aff_vec, aff_vec.rows() + subst_mat.rows());
        RtMatrixUtils::appendAtColumn(aff_mat, subst_mat, starting_column);
        aff_vec.block(last_row, 0, subst_mat.rows(),1).setZero();
      }

      template <typename AffMatDerived, typename AffVecDerived, typename SubstMatDerived, typename SubstVecDerived>
      static inline void appendAtColumn(Eigen::MatrixBase<AffMatDerived>& aff_mat, Eigen::MatrixBase<AffVecDerived>& aff_vec,
        const Eigen::MatrixBase<SubstMatDerived>& subst_mat, const Eigen::MatrixBase<SubstVecDerived>& subst_vec, int starting_column)
      {
        #ifndef RTEIG_NO_ASSERTS
          assertMatchingDimensions(aff_mat, aff_vec);
          assertMatchingDimensions(subst_mat, subst_vec);
        #endif

        const int last_row = aff_vec.rows();
        appendAtColumn(aff_mat, aff_vec, subst_mat, starting_column);
        aff_vec.block(last_row, 0, subst_mat.rows(),1) = subst_vec;
      }

      template<int MaxRows, int MaxCols>
      static inline void removeZeroRows(typename RtMatrix<MaxRows, MaxCols>::d& mat,
        typename RtVector<MaxRows>::d& vec, double threshold = RtMatrixUtils::remove_zero_thresh_)
      {
        if (mat.rows() == 0) { return; }

        typename RtVector<MaxRows>::d squared_norms;
        squared_norms = mat.rowwise().squaredNorm();
        const double normed_thresh = std::sqrt(double(squared_norms.size())) * threshold * threshold;
        int num_valids = 0;
        for (int row_id=0; row_id<squared_norms.size(); row_id++)
          if(squared_norms[row_id] > normed_thresh)
            ++num_valids;

        if (num_valids == 0) {
          RtVectorUtils::resize(vec, 0);
          RtMatrixUtils::resize(mat, 0, mat.cols());
        } else {
          typename RtMatrix<MaxRows, MaxCols>::d selector_mat;
          RtMatrixUtils::createSelectorMat<MaxRows, MaxCols>(selector_mat, squared_norms, num_valids, normed_thresh);
          mat = selector_mat * mat;
          vec = selector_mat * vec;
        }
      }

      template <typename Mat, typename Vec>
      static inline void removeZeroRows(Eigen::EigenBase<Mat>& mat, Eigen::EigenBase<Vec>& vec)
      {
        const double length_thresh = 1.e-12;
        int first_empty_i=0;
        for (; first_empty_i < mat.rows(); ++first_empty_i)
          if (mat.derived().block(first_empty_i, 0, 1, mat.cols()).array().abs().sum() < length_thresh)
            break;

        // found an empty row, now find the next nonempty row
        int block_start_i = first_empty_i+1;
        while (true) {
          for (; block_start_i < mat.rows(); ++block_start_i)
            if (mat.derived().block(block_start_i, 0, 1, mat.cols()).array().abs().sum() > length_thresh)
              break;

          // if the rest of the matrix is zero, stop
          if (block_start_i >= mat.rows())
            break;

          // now find the end of the block
          int block_end_i=block_start_i+1;
          for (; block_end_i < mat.rows(); ++block_end_i)
            if (mat.derived().block(block_end_i, 0, 1, mat.cols()).array().abs().sum() < length_thresh)
              break;

          //now shift the block left
          vec.derived().block(first_empty_i, 0, block_end_i-block_start_i, 1) =
          vec.derived().block(block_start_i, 0, block_end_i-block_start_i,1);
          mat.derived().block(first_empty_i, 0, block_end_i-block_start_i, mat.cols()) =
          mat.derived().block(block_start_i, 0, block_end_i-block_start_i, mat.cols());

          first_empty_i += block_end_i-block_start_i;
          block_start_i = block_end_i;
        }
        RtVectorUtils::conservativeResize(vec.derived(), first_empty_i);
        RtMatrixUtils::conservativeResize(mat.derived(), first_empty_i, mat.cols());
      }

      /**!
       * Given aff_mat*x + aff_vec, substitute x = subst_mat * y + subst_vec into x.
       */
      template <typename AffMatDerived, typename AffVecDerived, typename SubstMatDerived, typename SubstVecDerived>
      static inline void substitute(Eigen::MatrixBase<AffMatDerived>& aff_mat, Eigen::MatrixBase<AffVecDerived>& aff_vec,
        const Eigen::MatrixBase<SubstMatDerived>& subst_mat, const Eigen::MatrixBase<SubstVecDerived>& subst_vec)
      {
        #ifndef RTEIG_NO_ASSERTS
          assertMatchingDimensions(aff_mat, aff_vec);
          assertMatchingDimensions(subst_mat, subst_vec);
        #endif
        aff_vec += aff_mat*subst_vec;
        substitute(aff_mat, subst_mat);
      }

      template <typename AffMatDerived, typename SubstMatDerived>
      static inline void substitute(Eigen::MatrixBase<AffMatDerived>& aff_mat, const Eigen::MatrixBase<SubstMatDerived>& subst_mat)
      {
        #ifndef RTEIG_NO_ASSERTS
          assert(aff_mat.rows()*subst_mat.cols() <= aff_mat.MaxSizeAtCompileTime);
        #endif
        aff_mat = aff_mat*subst_mat;
      }

      #ifndef RTEIG_NO_ASSERTS
        template <typename AffMatDerived, typename AffVecDerived>
        static inline void assertMatchingDimensions(const Eigen::MatrixBase<AffMatDerived>& aff_mat, const Eigen::MatrixBase<AffVecDerived>& aff_vec)
        {
          assert(aff_mat.rows() == aff_vec.rows());
          assert(aff_vec.cols() == 1);
        }
      #endif
  };

  class RtQuadraticUtils
  {
    private:
      RtQuadraticUtils(){}
      virtual ~ RtQuadraticUtils(){}

    public:
      // CARE with const correctness: quad_mat is NOT const! const will be casted away!
      template <typename QuadMatDerived, typename AffMatDerived>
      static inline void addFromNorm(const Eigen::MatrixBase<QuadMatDerived>& quad_mat, const Eigen::MatrixBase<AffMatDerived>& aff_mat)
      {
        const_cast<Eigen::MatrixBase<QuadMatDerived>&>(quad_mat) += aff_mat.transpose() * aff_mat;
      }

      // CARE with const correctness: quad_mat is NOT const! const will be casted away!
      template <typename QuadMatDerived, typename QuadVecDerived, typename AffMatDerived, typename AffVecDerived>
      static inline void addFromNorm(const Eigen::MatrixBase<QuadMatDerived>& quad_mat, const Eigen::MatrixBase<QuadVecDerived>& quad_vec,
        const Eigen::MatrixBase<AffMatDerived>& aff_mat, const Eigen::MatrixBase<AffVecDerived>& aff_vec)
      {
        #ifndef RTEIG_NO_ASSERTS
          assertMatchingDimensions(quad_mat, quad_vec);
          RtAffineUtils::assertMatchingDimensions(aff_mat, aff_vec);
        #endif
        addFromNorm(quad_mat, aff_mat);
        const_cast<Eigen::MatrixBase<QuadVecDerived>&>(quad_vec) += aff_mat.transpose() * aff_vec;
      }

      template <typename QuadMatDerived, typename AffMatDerived, typename WeightMatDerived>
      static inline void addFromWeightedNorm(Eigen::MatrixBase<QuadMatDerived>& quad_mat,
        const Eigen::MatrixBase<AffMatDerived>& aff_mat, const Eigen::MatrixBase<WeightMatDerived>& weight_mat)
      {
        quad_mat += aff_mat.transpose() * weight_mat * aff_mat;
      }

      template <typename QuadMatDerived, typename QuadVecDerived, typename AffMatDerived, typename AffVecDerived, typename WeightMatDerived>
      static inline void addFromWeightedNorm(Eigen::MatrixBase<QuadMatDerived>& quad_mat,
        Eigen::MatrixBase<QuadVecDerived>& quad_vec, const Eigen::MatrixBase<AffMatDerived>& aff_mat,
        const Eigen::MatrixBase<AffVecDerived>& aff_vec, const Eigen::MatrixBase<WeightMatDerived>& weight_mat)
      {
        #ifndef RTEIG_NO_ASSERTS
          assertMatchingDimensions(quad_mat, quad_vec);
          RtAffineUtils::assertMatchingDimensions(aff_mat, aff_vec);
        #endif
        addFromWeightedNorm(quad_mat, aff_mat, weight_mat);
        quad_vec += aff_mat.transpose() * weight_mat * aff_vec;
      }

      template <typename QuadMatDerived, typename SubstMatDerived>
      static inline void substitute(Eigen::MatrixBase<QuadMatDerived>& quad_mat, const Eigen::MatrixBase<SubstMatDerived>& subst_mat)
      {
        #ifndef RTEIG_NO_ASSERTS
          assert(subst_mat.cols()*subst_mat.cols() <= quad_mat.MaxSizeAtCompileTime);
        #endif
        quad_mat = subst_mat.transpose() * quad_mat *subst_mat;
      }

      template <typename QuadMatDerived, typename QuadVecDerived, typename SubstMatDerived, typename SubstVecDerived>
      static inline void substitute(Eigen::MatrixBase<QuadMatDerived>& quad_mat, Eigen::MatrixBase<QuadVecDerived>& quad_vec,
        const Eigen::MatrixBase<SubstMatDerived>& subst_mat, const Eigen::MatrixBase<SubstVecDerived>& subst_vec)
      {
        #ifndef RTEIG_NO_ASSERTS
          assertMatchingDimensions(quad_mat, quad_vec);
          RtAffineUtils::assertMatchingDimensions(subst_mat, subst_vec);
          assert(subst_mat.cols() <= quad_vec.MaxSizeAtCompileTime);
        #endif
        quad_vec += quad_mat*subst_vec;
        quad_vec = subst_mat.transpose() * quad_vec;
        substitute(quad_mat, subst_mat);
      }

      #ifndef RTEIG_NO_ASSERTS
        template <typename QuadMatDerived, typename QuadVecDerived>
        static inline void assertMatchingDimensions(const Eigen::MatrixBase<QuadMatDerived>& quad_mat, const Eigen::MatrixBase<QuadVecDerived>& quad_vec)
        {
          assert(quad_mat.rows() == quad_mat.cols() && quad_mat.cols() == quad_vec.rows());
          assert(quad_vec.cols() == 1);
        }
      #endif

  };

}