// -*- compile-command: "c++ -std=c++14 -O3 -DNDEBUG -ffast-math -march=native lcp.cpp -o lcp `pkg-config --cflags eigen3` -lstdc++" -*-

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <iostream>
#include <array>

template<class U>
struct lcp {
  using real = U;

  template<int N>
  using vector = Eigen::Matrix<real, N, 1>;

  template<int N>
  using indices = Eigen::Matrix<Eigen::Index, N, 1>;

  template<int N>
  using matrix = Eigen::Matrix<real, N, N>;

  static void solve(vector<1>& x, const matrix<1>& M, const vector<1>& q) {
    assert(M(0, 0) > 0);
    x(0) = std::max<real>(0, -q(0) / M(0, 0));
  }

  template<int N>
  static void solve(vector<N>& x, matrix<N> M, vector<N> q) {
    static_assert(N > 1, "size error");
    indices<N - 1> sub;
    vector<N> w;

    std::array<bool, N> active;

    for(std::size_t i = 0; i < N; ++i) {
      // filter all but ith component
      auto ptr = sub.data();
      for(std::size_t j = 0; (j += (j == i)) < N; ++j, ++ptr) {
        *ptr = j;
      }

      vector<N - 1> qq = sub.unaryExpr(q);
      matrix<N - 1> MM;
      for(int j = 0; j < N - 1; ++j) {
        MM.col(j) = sub.unaryExpr(M.col(sub(j)));
      }

      vector<N - 1> xx;
      solve(xx, MM, qq);

      w(i) = std::max<real>(sub.unaryExpr(M.col(i)).dot(xx) + q(i), 0);
      active[i] = w(i) == 0;
    }

    for(int i = 0; i < N; ++i) {
      if(!active[i]) {
        M.row(i) = vector<N>::Zero().transpose();
        M.col(i) = vector<N>::Zero();
        M(i, i) = 1;
        q(i) = w(i);
      }
    }

    // TODO use LLT when N > 4?
    x.noalias() = M.inverse() * (w - q);
  }
};

template<class U>
struct closest {
  using real = U;
  using vec3 = Eigen::Matrix<real, 3, 1>;
  using vec2 = Eigen::Matrix<real, 2, 1>;

  using mat3x3 = Eigen::Matrix<real, 3, 3>;
  using mat2x2 = Eigen::Matrix<real, 2, 2>;

  static double clamp(double x) {
    return x;
  }

  // static real clamp(real x) {
  //   return std::max<real>(x, 0);
  // }

  static void fast_lcp(vec3& x, mat3x3 M, vec3 q) {
    const real x1[3] = {
      clamp(-q(0) / M(0, 0)),
      clamp(-q(1) / M(1, 1)),
      clamp(-q(2) / M(2, 2)),
    };

    using vec2i = Eigen::Matrix<Eigen::Index, 2, 1>;
    // TODO make this constexpr?
    const vec2i ind[3] =  {{1, 2}, {0, 2}, {0, 1}};

    mat2x2 M2inv[3];

    vec2 x2[3];
    vec3 w;
    int skip = 0;
    for(int i = 0; i < 3; ++i) {
      mat2x2 sub;               // TODO optimize (symmetric)
      for(int j = 0; j < 2; ++j) {
        sub.col(j) = ind[i].unaryExpr(M.col(ind[i][j]));
      }

      vec2 w2;
      for(int j = 0; j < 2; ++j) {
        const int other = (j + 1) % 2;
        // w2(j) = clamp(sub(j, other) * x1[ind[i][other]] + q(ind[i][j]));
        w2(j) = clamp(M(ind[i][j], ind[i][other]) * x1[ind[i][other]] + q(ind[i][j]));
      }

      // TODO optimize?
      M2inv[i] = sub.inverse();
      x2[i].noalias() = M2inv[i] * (w2 - ind[i].unaryExpr(q));

      w[i] = clamp(ind[i].unaryExpr(M.col(i)).dot(x2[i]) + q(i));

      if(w[i] > 0) {
        skip = i;
      }

    }

    const vec2 res = M2inv[skip] * ind[skip].unaryExpr(w - q);
    x[ind[skip][0]] = res[0];
    x[ind[skip][1]] = res[1];
    x[skip] = 0;
  }

  static vec3 project_triangle(vec3 a, vec3 b, vec3 c, vec3 p) {
    const vec3* points[3] = {&a, &b, &c};

    const vec3 edges[3] = {
      b - a,
      c - b,
      a - c
    };

    const vec3 n = edges[0].cross(edges[1]);

    mat3x3 JT;
    for(int i = 0; i < 3; ++i) {
      JT.col(i) = n.cross(edges[i]);
    }

    vec3 bounds;

    for(int i = 0; i < 3; ++i) {
      bounds(i) = JT.col(i).dot(*points[i]);
    }

    const mat3x3 M = JT.transpose() * JT;
    const vec3 q = (JT.transpose() * p - bounds);

    vec3 lambda;
    // lcp<real>::solve(lambda, M, q);
    fast_lcp(lambda, M, q);

    // std::clog << lambda.transpose() << std::endl;
    // std::clog << (M * lambda + q).transpose() << std::endl;
    // std::clog << lambda.dot(M * lambda + q) << std::endl;

    const vec3 origin = a;
    const vec3 delta = p - origin;
    const vec3 proj = p - n * n.dot(delta) / n.dot(n);

    return JT * lambda + proj;
  }

  struct projector_ref {
    vec3 p1, p2, p3, p1p2, p1p3;
    real distp1p2;
    real fa, fb, fc;
    real fdet;

    projector_ref(vec3 p1, vec3 p2, vec3 p3):
        p1(p1),
        p2(p2),
        p3(p3),
        p1p2(p2 - p1),
        p1p3(p3 - p1),
        distp1p2(p1p2.dot(p1p2)),
        fa(distp1p2),
        fb(p1p2.dot(p1p3)),
        fc(p1p3.dot(p1p3)),
        fdet(fa * fc - fb * fb) {

    }

    vec3 operator()(vec3 p) const {
      const vec3 pp1 = p1 - p;
      const real fd = p1p2.dot(pp1), fe = p1p3.dot(pp1);
      real fs = fb * fe - fc * fd, ft = fb * fd - fa * fe;

      if(fs + ft <= fdet) {
        if(fs < 0) {
          if(ft < 0) {
            // region 4
            if(fd < 0) {
              ft = 0;
              if(-fd >= fa) {
                fs = 1;
              } else {
                fs = -fd / fa;
              }
            } else {
              fs = 0;
              if(fe >= 0) {
                ft = 0;
              } else if(-fe >= fc) {
                ft = 1;
              } else {
                ft = -fe / fc;
              }
            }
          } else {
            // region 3
            fs = 0;
            if(fe >= 0) {
              ft = 0;
            } else if(-fe >= fc) {
              ft = 1;
            } else {
              ft = -fe / fc;
            }
          }
        } else if(ft < 0) {
          // region 5
          ft = 0;
          if(fd >= 0) {
            fs = 0;
          } else if(-fd >= fa) {
            fs = 1;
          } else {
            fs = -fd / fa;
          }

        } else {
          // region 0
          // minimum at interior point
          fs /= fdet;
          ft /= fdet;
        }
      } else {
        real ftmp0, ftmp1, fNumer, fDenom;

        if(fs < 0) {
          // region 2
          ftmp0 = fb + fd;
          ftmp1 = fc + fe;
          if(ftmp1 > ftmp0) {
            fNumer = ftmp1 - ftmp0;
            fDenom = fa - 2 * fb + fc;
            if(fNumer >= fDenom) {
              fs = 1;
              ft = 0;
            } else {
              fs = fNumer / fDenom;
              ft = 1 - fs;
            }
          } else {
            fs = 0;
            if(ftmp1 <= 0) {
              ft = 1;
            } else if(fe >= 0) {
              ft = 0;
            } else {
              ft = -fe / fc;
            }
          }
        } else if(ft < 0) {
          // region 6
          ftmp0 = fb + fe;
          ftmp1 = fa + fd;
          if(ftmp1 > ftmp0) {
            fNumer = ftmp1 - ftmp0;
            fDenom = fa - 2 * fb + fc;
            if(fNumer >= fDenom) {
              ft = 1;
              fs = 0;
            } else {
              ft = fNumer / fDenom;
              fs = 1 - ft;
            }
          } else {
            ft = 0;
            if(ftmp1 <= 0) {
              fs = 1;
            } else if(fd >= 0) {
              fs = 0;
            } else {
              fs = -fd / fa;
            }
          }
        } else {
          // region 1
          fNumer = fc + fe - fb - fd;
          if(fNumer <= 0) {
            fs = 0;
            ft = 1;
          } else {
            fDenom = fa - 2 * fb + fc;
            if(fNumer >= fDenom) {
              fs = 1;
              ft = 0;
            } else {
              fs = fNumer / fDenom;
              ft = 1 - fs;
            }
          }
        }
      }

      return (1 - fs - ft) * p1 + fs * p2 + ft * p3;
    }

  };

  static vec3 project_triangle_ref(vec3 p1, vec3 p2, vec3 p3, vec3 p) {
    const vec3 p1p2 = p2 - p1, p1p3 = p3 - p1, pp1 = p1 - p;
    const real distp1p2 = p1p2.dot(p1p2) // distp2p3 = (p3 - p2).dot(p3 - p2),
               // distp3p1 = p1p3.dot(p1p3)
      ;

    const real fa = distp1p2, fb = p1p2.dot(p1p3), fc = p1p3.dot(p1p3), fd = p1p2.dot(pp1),
      fe = p1p3.dot(pp1);

    const real fdet = fa * fc - fb * fb;
    real fs = fb * fe - fc * fd, ft = fb * fd - fa * fe;

    if(fs + ft <= fdet) {
      if(fs < 0) {
        if(ft < 0) {
          // region 4
          if(fd < 0) {
            ft = 0;
            if(-fd >= fa) {
              fs = 1;
            } else {
              fs = -fd / fa;
            }
          } else {
            fs = 0;
            if(fe >= 0) {
              ft = 0;
            } else if(-fe >= fc) {
              ft = 1;
            } else {
              ft = -fe / fc;
            }
          }
        } else {
          // region 3
          fs = 0;
          if(fe >= 0) {
            ft = 0;
          } else if(-fe >= fc) {
            ft = 1;
          } else {
            ft = -fe / fc;
          }
        }
      } else if(ft < 0) {
        // region 5
        ft = 0;
        if(fd >= 0) {
          fs = 0;
        } else if(-fd >= fa) {
          fs = 1;
        } else {
          fs = -fd / fa;
        }

      } else
      {
        // region 0
        // minimum at interior point
        fs /= fdet;
        ft /= fdet;
      }
    } else {
      real ftmp0, ftmp1, fNumer, fDenom;

      if(fs < 0)
        {
          // region 2
          ftmp0 = fb + fd;
          ftmp1 = fc + fe;
          if(ftmp1 > ftmp0) {
            fNumer = ftmp1 - ftmp0;
            fDenom = fa - 2 * fb + fc;
            if(fNumer >= fDenom) {
              fs = 1;
              ft = 0;
            } else {
              fs = fNumer / fDenom;
              ft = 1 - fs;
            }
          } else {
            fs = 0;
            if(ftmp1 <= 0) {
              ft = 1;
            } else if(fe >= 0) {
              ft = 0;
            } else {
              ft = -fe / fc;
            }
          }
        } else if(ft < 0)
        {
          // region 6
          ftmp0 = fb + fe;
          ftmp1 = fa + fd;
          if(ftmp1 > ftmp0) {
            fNumer = ftmp1 - ftmp0;
            fDenom = fa - 2 * fb + fc;
            if(fNumer >= fDenom) {
              ft = 1;
              fs = 0;
            } else {
              ft = fNumer / fDenom;
              fs = 1 - ft;
            }
          } else {
            ft = 0;
            if(ftmp1 <= 0) {
              fs = 1;
            } else if(fd >= 0) {
              fs = 0;
            } else {
              fs = -fd / fa;
            }
          }
        } else
        {
          // region 1
          fNumer = fc + fe - fb - fd;
          if(fNumer <= 0) {
            fs = 0;
            ft = 1;
          } else {
            fDenom = fa - 2 * fb + fc;
            if(fNumer >= fDenom) {
              fs = 1;
              ft = 0;
            } else {
              fs = fNumer / fDenom;
              ft = 1 - fs;
            }
          }
        }
    }

    return (1 - fs - ft) * p1 + fs * p2 + ft * p3;
  }

  struct projector_alt {
    mat3x3 points;
    mat3x3 edges;
    mat3x3 points_inv;
    vec3 n;
    real n2;

    projector_alt(vec3 a, vec3 b, vec3 c) {
      points << a, b, c;
      edges << c - b, a - c, b - a;

      points_inv = points.inverse();
      n = edges.col(0).cross(edges.col(1));
      n2 = n.dot(n);

    }

    vec3 operator()(vec3 q) const {
      const vec3 delta = q - points.col(0);
      const vec3 proj = q - n * n.dot(delta) / n2;
      const vec3 coords = points_inv * proj;

      int negative = 0;

      // negative, positive
      int last_index[2];
      for(int i = 0; i < 3; ++i) {
        const auto neg = coords[i] < 0;
        negative += neg;
        last_index[neg] = i;
      }

      switch(negative) {
      case 0: return proj;
      case 1: {
        const vec3 e = edges.col(last_index[0]);
        const vec3 origin = points.col(last_index[1]);
        const vec3 delta = q - origin;

        real alpha = e.dot(delta) / e.dot(e);

        // TODO branchless
        // alpha = (alpha > 0) * alpha - (alpha > 1) * (alpha - 1);
        alpha = std::max<real>(0, alpha);
        alpha = std::min<real>(1, alpha);

        return origin + e * alpha;
      }
      case 2: return points.col(last_index[1]);
      default:
        throw std::logic_error("unreachable");
      }
    }
  };

  static vec3 project_triangle_alt(vec3 a, vec3 b, vec3 c, vec3 q) {
    mat3x3 points;
    points << a, b, c;

    mat3x3 edges;
    edges << c - b, a - c, b - a;

    const vec3 n = edges.col(0).cross(edges.col(1));

    const vec3 origin = a;
    const vec3 delta = q - origin;
    const vec3 proj = q - n * n.dot(delta) / n.dot(n);

    const vec3 coords = points.inverse() * proj;

    int negative = 0;

    // negative, positive
    int last_index[2];
    for(int i = 0; i < 3; ++i) {
      const auto neg = coords[i] < 0;
      negative += neg;
      last_index[neg] = i;
    }

    switch(negative) {
    case 0: return proj;
    case 1: {
      const vec3 e = edges.col(last_index[0]);
      const vec3 origin = points.col(last_index[1]);
      const vec3 delta = q - origin;

      real alpha = e.dot(delta) / e.dot(e);
      // alpha = (alpha > 0) * alpha - (alpha > 1) * (alpha - 1);
      alpha = std::max<real>(0, alpha);
      alpha = std::min<real>(1, alpha);

      return origin + e * alpha;
    }
    case 2: return points.col(last_index[1]);
    default:
      throw std::logic_error("unreachable");
    }

  }
};

#include "timer.hpp"
#include <bitset>

Eigen::Matrix<double, 3, 1> solution;

int main(int argc, char** argv) {
  std::srand(time(0));

  {
    using lcp = struct lcp<double>;

    static constexpr int N = 3;

    lcp::matrix<N> M;
    lcp::vector<N> q, x, w;

    M.setRandom();
    M = M.transpose() * M;

    q.setRandom();
    lcp::solve(x, M, q);

    w.noalias() = M * x + q;

    std::cout << "x: " << x.transpose() << std::endl;
    std::cout << "w: " << w.transpose() << std::endl;
    std::cout << "xw: " << x.dot(w) << std::endl;
  }

  {
    using closest = struct closest<double>;

    std::clog << "=================" << std::endl;

    static const auto make_problem = [](auto k) {
      closest::vec3 a, b, c, p;
      a.setRandom();
      b.setRandom();
      c.setRandom();

      p.setRandom();

      return k(a, b, c, p);
    };

    static const int n = 1000000;

    const auto lcp_duration = with_time([] {
      for(int i = 0; i < n; ++i) {
        solution = make_problem([](auto a, auto b, auto c, auto p) {
          return closest::project_triangle(a, b, c, p);
        });
      }
    });

    const auto alt_duration = with_time([] {
      for(int i = 0; i < n; ++i) {
        solution = make_problem([](auto a, auto b, auto c, auto p) {
          return closest::project_triangle_alt(a, b, c, p);
        });
      }
    });

    const auto ref_duration = with_time([] {
      for(int i = 0; i < n; ++i) {
        solution = make_problem([](auto a, auto b, auto c, auto p) {
          return closest::project_triangle_ref(a, b, c, p);
        });
      }
    });

    std::clog << "lcp: " << lcp_duration << std::endl;
    std::clog << "alt: " << alt_duration << std::endl;
    std::clog << "ref: " << ref_duration << std::endl;

    ////////////////////////////////////////////////////////////////////////////////
    const int m = 100;
    const auto proj_alt_duration = with_time([] {
      std::vector<closest::projector_alt> projs;

      for(int i = 0; i < n; ++i) {
        projs.emplace_back(closest::vec3::Random(),
                           closest::vec3::Random(),
                           closest::vec3::Random());
      }

      for(auto& proj: projs) {
        for(int i = 0; i < m; ++i) {
          solution = proj(closest::vec3::Random());
        }
      }
    });

    const auto proj_ref_duration = with_time([] {
      std::vector<closest::projector_ref> projs;

      for(int i = 0; i < n; ++i) {
        projs.emplace_back(closest::vec3::Random(),
                           closest::vec3::Random(),
                           closest::vec3::Random());
      }

      for(auto& proj: projs) {
        for(int i = 0; i < m; ++i) {
          solution = proj(closest::vec3::Random());
        }
      }
    });

    std::clog << "proj alt: " << proj_alt_duration << std::endl;
    std::clog << "proj ref: " << proj_ref_duration << std::endl;

    // closest::mat3x3 B;
    // B.col(0) = a;
    // B.col(1) = b;
    // B.col(2) = c;

    // const closest::vec3 coords = B.inverse() * x;
    // std::clog << "coords sum: " << coords.sum() << std::endl;
    // std::clog << "coords: " << coords.transpose() << std::endl;
  }

  return 0;
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2016 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#define FUSION_MAX_VECTOR_SIZE 25  // increase maximal vector size for boost/fusion vectors
#include <iostream>
#include <algorithm>  // min, max

#include <boost/timer/timer.hpp>

#include "dune/grid/config.h"
#include "dune/grid/uggrid.hh"
#include "dune/istl/solvers.hh"

#include "fem/assemble.hh"
#include "fem/istlinterface.hh"
#include "fem/gridmanager.hh"
#include "fem/lagrangespace.hh"
//#include "fem/hierarchicspace.hh"   // ContinuousHierarchicMapper
#include "fem/norms.hh"
#include "linalg/iluprecond.hh"
#include "io/vtk.hh"
//#include "io/amira.hh"
#include "utilities/kaskopt.hh"
#include "utilities/gridGeneration.hh" //  createUnitSquare

using namespace Kaskade;
#include "sst.hh"

struct InitialValue
{
  using Scalar = double;
  static constexpr int components = 1;
  using ValueType = Dune::FieldVector<Scalar,components>;

  InitialValue(int c): component(c) {}

  template <class Cell> int order(Cell const&) const { return std::numeric_limits<int>::max(); }
  template <class Cell>
  ValueType value(Cell const& cell,
          Dune::FieldVector<typename Cell::Geometry::ctype,Cell::Geometry::coorddimension> const& localCoordinate) const
  {
  Dune::FieldVector<typename Cell::Geometry::ctype,Cell::Geometry::coorddimension> x = cell.geometry().global(localCoordinate);
  if (component==0)
    return 1.306028e6;
  else if (component==1)
    return 1.076508e12;
  else if (component==2)
    return 6.457715e10;
  else if (component==3)
    return 3.542285e10;
  else
    assert("wrong index!\n"==0);
  return 0;

  }

private:
  int component;
};

int main(int argc, char *argv[])
{
  using Scalar = double;
  using namespace boost::fusion;

  std::cout << "Start sst transfer tutorial program with ordinary Newton iteration" << std::endl;

  boost::timer::cpu_timer totalTimer;

  int verbosityOpt = 1;
  bool dump = true;
  std::unique_ptr<boost::property_tree::ptree> pt = getKaskadeOptions(argc, argv, verbosityOpt, dump);

  int  direct,
     refinements = getParameter(pt, "refinement", 5),
     order =  getParameter(pt, "order", 2),
     verbosity   = getParameter(pt, "verbosity", 0);
  Scalar tol = getParameter(pt, "tolerance", 1.0e-10);
  //   IterateType iterateType = IterateType::CG;
  //   PrecondType precondType = PrecondType::NONE;
  Scalar fsign = static_cast<Scalar>(getParameter(pt, "sign", 0.0));
  std::string empty;

  std::cout << "refinements of original mesh   : " << refinements << std::endl;
  std::cout << "discretization order           : " << order << std::endl;
  std::cout << "tolerance for Newton iteration : " << tol << std::endl;

  std::string s("names.type.");
  s += getParameter(pt, "solver.type", empty);
  direct = getParameter(pt, s, 0);

  //   s = "names.iterate." + getParameter(pt, "solver.iterate", empty);
  //   iterateType = static_cast<IterateType>(getParameter(pt, s, 0));
  //   s = "names.preconditioner." + getParameter(pt, "solver.preconditioner", empty);
  //   precondType = static_cast<PrecondType>(getParameter(pt, s, 0));

  constexpr int dim=2;
  using Grid = Dune::UGGrid<dim>;
  using LeafView = Grid::LeafGridView;
  using H1Space = FEFunctionSpace<ContinuousLagrangeMapper<Scalar,LeafView> >;
// using H1Space = FEFunctionSpace<ContinuousHierarchicMapper<Scalar,LeafView> >;
  using Spaces = boost::fusion::vector<H1Space const*>;
  using VariableDescriptions = boost::fusion::vector<Variable<SpaceIndex<0>,Components<1>,VariableId<0> >,
                               Variable<SpaceIndex<0>,Components<1>,VariableId<1> >,
                               Variable<SpaceIndex<0>,Components<1>,VariableId<2> >,
                               Variable<SpaceIndex<0>,Components<1>,VariableId<3> > >;
  using VariableSet = VariableSetDescription<Spaces,VariableDescriptions>;
  using Functional = SSTFunctional<Scalar,VariableSet>;
  using Assembler = VariationalFunctionalAssembler<LinearizationAt<Functional> >;
  constexpr int neq = SSTFunctional<Scalar,VariableSet>::TestVars::noOfVariables;
  using CoefficientVectors = VariableSet::CoefficientVectorRepresentation<0,neq>::type;
  using LinearSpace = VariableSet::CoefficientVectorRepresentation<0,neq>::type;

  GridManager<Grid> gridManager( createUnitSquare<Grid>() );
  gridManager.globalRefine(refinements);
  std::cout << std::endl << "Grid: " << gridManager.grid().size(0) << " triangles, " << std::endl;
  std::cout << "      " << gridManager.grid().size(1) << " edges, " << std::endl;
  std::cout << "      " << gridManager.grid().size(2) << " points" << std::endl;

  // construction of finite element space for the scalar solution T
  H1Space temperatureSpace(gridManager,gridManager.grid().leafGridView(),
               order);
  Spaces spaces(&temperatureSpace);
  // VariableDescription<int spaceId, int components, int Id>
  // spaceId: number of associated FEFunctionSpace
  // components: number of components in this variable
  // Id: number of this variable
  std::string varNames[4] = { "u0", "u1", "u2", "u3" };
  VariableSet variableSet(spaces,varNames);

  Functional F;
  Assembler assembler(gridManager,spaces);
  VariableSet::VariableSet x(variableSet);
  VariableSet::VariableSet newtonCorr(variableSet);
  VariableSet::VariableSet tmp(variableSet);

  constexpr int nvars = SSTFunctional<Scalar,VariableSet>::AnsatzVars::noOfVariables;
  size_t  nnz = assembler.nnz(0,neq,0,nvars,false);
  size_t  size = variableSet.degreesOfFreedom(0,nvars);
  std::cout << "nvars=" << nvars << ", neq=" << neq << ", size=" << size << ", nnz=" << nnz << "\n";

  std::vector<Scalar> xdata(size), scal(size), dxdata(size);

  int k=0;
  L2Norm l2Norm;
  Scalar norm_dx, norm_rhs;
  std::vector<Scalar> norm_dx_comp(4), norm_rhs_comp(4);

  F.scaleInitialValue<0>(InitialValue(0),x);
  F.scaleInitialValue<1>(InitialValue(1),x);
  F.scaleInitialValue<2>(InitialValue(2),x);
  F.scaleInitialValue<3>(InitialValue(3),x);
  x.write(xdata.begin());
  for (int i=0;i<size;i++) scal[i]=std::max(std::abs(xdata[i]),1.0);

  CoefficientVectors solution(VariableSet::CoefficientVectorRepresentation<0,neq>::init(spaces));

  writeVTKFile(x,"graph/sst_start",IoOptions().setOrder(std::min(order,2)).setPrecision(7));
  gridManager.enforceConcurrentReads(false);
  Dune::InverseOperatorResult res;

  std::cout << std::endl << "Newton iteration starts:" << std::endl <<
            "iter  scaled ||corr||            ||F||    itsol: eps     result  steps      rate        time"
            << std::endl;

// begin of ordinary Newton iteration loop
  do {
    boost::timer::cpu_timer assembTimer;
    assembler.assemble(linearization(F,x));
    size_t  nnz = assembler.nnz(0,neq,0,nvars,false);
    AssembledGalerkinOperator<Assembler,0,neq,0,neq> A(assembler);
    CoefficientVectors rhs(assembler.rhs());

    tmp.data = rhs.data;
    norm_rhs_comp[0]=l2Norm(boost::fusion::at_c<0>(tmp.data));
    norm_rhs_comp[1]=l2Norm(boost::fusion::at_c<1>(tmp.data));
    norm_rhs_comp[2]=l2Norm(boost::fusion::at_c<2>(tmp.data));
    norm_rhs_comp[3]=l2Norm(boost::fusion::at_c<3>(tmp.data));
    norm_rhs = sqrt(norm_rhs_comp[0]*norm_rhs_comp[0]+norm_rhs_comp[1]*norm_rhs_comp[1]+
                    norm_rhs_comp[2]*norm_rhs_comp[2]+norm_rhs_comp[3]*norm_rhs_comp[3]);

    boost::timer::cpu_timer iteTimer;
    int iteSteps = getParameter(pt, "solver.iteMax", 1000);
    Scalar iteEps = getParameter(pt, "solver.iteEps", 1.0e-12);
    int fill_lev = getParameter(pt, "solver.ILUK.fill_lev", 0);
    ILUKPreconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > iluk(A,fill_lev,verbosity);
    Dune::BiCGSTABSolver<LinearSpace> cg(A,iluk,iteEps,iteSteps,verbosity);
    solution = 0;
    cg.apply(solution,rhs,res);
    newtonCorr.data = solution.data;
    newtonCorr *= -1;
    // add Newton-correction
    x += newtonCorr;

    newtonCorr.write(dxdata.begin());
    for (int i=0;i<size;i++) dxdata[i] /= scal[i];
    tmp.read(dxdata.begin());
    norm_dx_comp[0]=l2Norm(boost::fusion::at_c<0>(tmp.data));
    norm_dx_comp[1]=l2Norm(boost::fusion::at_c<1>(tmp.data));
    norm_dx_comp[2]=l2Norm(boost::fusion::at_c<2>(tmp.data));
    norm_dx_comp[3]=l2Norm(boost::fusion::at_c<3>(tmp.data));
    norm_dx = sqrt(norm_dx_comp[0]*norm_dx_comp[0]+norm_dx_comp[1]*norm_dx_comp[1]+
                   norm_dx_comp[2]*norm_dx_comp[2]+norm_dx_comp[3]*norm_dx_comp[3]);

    x.write(xdata.begin());
    for (int i=0;i<size;i++) scal[i]=std::max(std::abs(xdata[i]),1.0);

    std::cout << std::setw(4) << k+1 << "  "
              << std::setw(15) << std::setprecision(5) << std::scientific << norm_dx << "  "
              << std::setw(15) << norm_rhs << "  " << std::setprecision(6);
    std::cout.unsetf(std::ios::fixed | std::ios::scientific);
    std::cout << std::setw(12) << iteEps << "  "
              << (res.converged?"converged":"failed  ") << "  "
              << std::setw(5) << res.iterations << "  "
              << std::setw(8) << res.conv_rate << "  "
              << std::setw(9) << (Scalar)(iteTimer.elapsed().user)/1e9 << "s " << std::endl;

    // output of solution in VTK format for visualization,
    // the data are written as ascii stream into file temperature.vtu,
    // possible is also binary
    std::ostringstream fname;
    fname << "graph/sst_";
    fname.width(2);
    fname.fill('0');
    fname.setf(std::ios_base::right,std::ios_base::adjustfield);
    fname << k;
    fname.flush();
    writeVTKFile(x,fname.str(),IoOptions().setOrder(std::min(order,2)).setPrecision(7));

    // output of solution for Amira visualization,
    // the data are written in binary format into file temperature.am,
    // possible is also ascii
    //IoOptions options;
    //options.outputType = IoOptions::ascii;
    //LeafView leafGridView = gridManager.grid().leafGridView();
    //writeAMIRAFile(leafGridView,x,"sst",options);
  k ++;
  }
  while ( norm_dx > tol );
 // end of ordinary Newton iteration loop

  std::cout << "total computing time: " << boost::timer::format(totalTimer.elapsed()) << "\n";
  std::cout << "End sst transfer tutorial program" << std::endl;
}


#include <blitz/array.h>
#include <blitz/tinyvec-et.h>

BZ_USING_NAMESPACE(blitz)

/*
 * The current implementation of stencil objects forces these variables
 * to be placed in global scope.  Ugh.  This restriction will be removed
 * eventually.
 */
double rho;                        // Density of fluid
double recip_rho;                  // 1/rho
double eta;                        // Kinematic viscosity
double time_now;                   // Elapsed seconds
double delta_t;                    // Time step
double volume;                     // Volume of a cell
double airPressure;                // Air pressure (Pa)
double spatialStep;                // Grid element size
double gravity;                    // Acceleration due to gravity
double gravityPressureGradient;    // Pressure gradient due to gravity
/*
 * The "geometry" object specifies how an array is mapped into real-world
 * space.  In this case, "UniformCubicGeometry" is used, which means that
 * the real-world grid is orthogonal, regularly spaced, with the same spatial
 * step in each dimension.
 */

UniformCubicGeometry<3> geom;      // Geometry
/*
 * Some typedefs to make life easier.
 */

typedef TinyVector<double,3> vector3d;
typedef Array<vector3d,3> vectorField;
typedef Array<double,3>   scalarField;

/***********          Timestep the velocity field           ************
 * This is a 63-point stencil.  For example, Laplacian3DVec4 turns into
 * a 45-point stencil: each 2nd derivative is a 5-point stencil, and
 * there are 9 of these derivatives to take the Laplacian of a 3D vector
 * field.
 */

BZ_DECLARE_STENCIL5(timestep, V, nextV, P, advect, force)

    nextV = *V + delta_t * ( recip_rho * (
      eta * Laplacian3DVec4(V,geom) - grad3D4(P, geom) + *force) - *advect);

BZ_END_STENCIL
/*
 * Allocate arrays and set their initial state
 */
void setup(const int N, vectorField& V, vectorField& nextV, scalarField& P,
    scalarField& P_rhs, vectorField& advect, vectorField& force)
{
    // A 1m x 1m x 1m domain
    spatialStep = 1.0 / (N - 1);
    geom = UniformCubicGeometry<3>(spatialStep);

    // Allocate arrays
    allocateArrays(shape(N,N,N), advect, V, nextV, force);  // vector fields
    allocateArrays(shape(N,N,N), P, P_rhs);                 // scalar fields

    // Since incompressibility is assumed, pressure only shows up as
    // derivative terms in the equations.  We choose airPressure = 0
    // as an arbitrary datum.

    airPressure = 0;             // Pa
    rho = 1000;                  // density of fluid, kg/m^3
    recip_rho = 1.0 / rho;       // inverse of density
    eta = 1.0e-6;                // kinematic viscosity of fluid, m^2/s
    gravity = 9.81;              // m/s^2
    delta_t = 0.001;             // initial time step, in seconds
    volume = pow3(spatialStep);  // cubic volume associated with grid point

    // Kludge: Set eta high, so that the flow will spread faster.
    // This means the cube is filled with molasses, rather than water.
    eta *= 1000;

    // Initial conditions: quiescent
    V = 0.0;
    P_rhs = 0.0;
    advect = 0.0;
    nextV = 0.0;
    P = 0.0;
    force = 0.0;
}

// Calculate a simple check on a vector field
void record(vectorField& V)
{
    // Calculate the magnitude of a field
    const int x=0, y=1, z=2;
    double magx = sum(pow2(V[x])) / V.numElements();
    double magy = sum(pow2(V[y])) / V.numElements();
    double magz = sum(pow2(V[z])) / V.numElements();

    cout << "norm = [" << magx
        << " " << magy << " " << magz << " ]" << endl;
}

void iterate(vectorField& V, vectorField& nextV, scalarField& P,
    scalarField& P_rhs, vectorField& advect, vectorField& force)
{
    // Time step
    applyStencil(timestep(), V, nextV, P, advect, force);
}

int main()
{
    vectorField V, nextV;        // Velocity fields
    scalarField P, P_rhs;        // Pressure fields
    vectorField advect;          // Advection field
    vectorField force;           // Forcing function

    const int N = 50;            // Arrays are NxNxN

    setup(N, V, nextV, P, P_rhs, advect, force);

    const int nIters = 10;

    for (int i=0; i < nIters; ++i)
    {
        iterate(V, nextV, P, P_rhs, advect, force);
    }

    return 0;
}


#include <iostream>
#include <vector>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>

// Graph Type with nested interior edge properties for flow algorithms
typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
    boost::property<boost::edge_capacity_t, long,
        boost::property<boost::edge_residual_capacity_t, long,
            boost::property<boost::edge_reverse_t, traits::edge_descriptor>>>> graph;

typedef traits::vertex_descriptor vertex_desc;
typedef traits::edge_descriptor edge_desc;

class edge_adder {
  graph &G;

 public:
  explicit edge_adder(graph &G) : G(G) {}

  void add_edge(int from, int to, long capacity) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const auto e = boost::add_edge(from, to, G).first;
    const auto rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  }
};

using namespace std;

int index(int row, int col, bool in, int cols) {
  return 2 * (row * cols + col) + in;
}

void solve() {
  int cols; cin >> cols;
  int rows; cin >> rows;
  int k; cin >> k;
  int c; cin >> c;

  graph G(2 * rows * cols);
  edge_adder adder(G);

  if (cols == 0 || rows == 0 || k == 0 || c == 0) {
    cout << 0 << endl;
    return;
  }

  // Set up graph
  for (int row = 0; row < rows; ++row) {
    for (int col = 0; col < cols; ++col) {
      // In to out vertex
      adder.add_edge(index(row, col, true, cols), index(row, col, false, cols), c);

      // Neighbors
      if (row > 0)
        adder.add_edge(index(row - 1, col, false, cols), index(row, col, true, cols), 1);
      if (row < rows - 1)
        adder.add_edge(index(row + 1, col, false, cols), index(row, col, true, cols), 1);
      if (col > 0)
        adder.add_edge(index(row, col - 1, false, cols), index(row, col, true, cols), 1);
      if (col < cols - 1)
        adder.add_edge(index(row, col + 1, false, cols), index(row, col, true, cols), 1);

    }
  }

  // Set up target
  vertex_desc target = boost::add_vertex(G);
  for (int row = 0; row < rows; ++row) {
    adder.add_edge(index(row, 0, false, cols), target, 1);
    adder.add_edge(index(row, cols - 1, false, cols), target, 1);
  }
  for (int col = 0; col < cols; ++col) {
    adder.add_edge(index(0, col, false, cols), target, 1);
    adder.add_edge(index(rows - 1, col, false, cols), target, 1);
  }

  // Set up source
  vertex_desc source = boost::add_vertex(G);
  int row, col;
  for (int i = 0; i < k; ++i) {
    cin >> col;
    cin >> row;
    adder.add_edge(source, index(row, col, true, cols), 1);
  }

  long flow = boost::push_relabel_max_flow(G, source, target);
  cout << flow << endl;
}

int main() {
  ios_base::sync_with_stdio(false);
  int t; cin >> t;
  for (int i = 0; i < t; ++i) {
    solve();
  }
}


#include <iostream>
#include <cassert>
#include <vector>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>
#include <boost/graph/find_flow_cost.hpp>

typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> GraphTraits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
                              boost::property<boost::edge_capacity_t, long,
                                              boost::property<boost::edge_residual_capacity_t, long,
                                                              boost::property<boost::edge_reverse_t, GraphTraits::edge_descriptor,
                                                                              boost::property<boost::edge_weight_t, long>>>>>
    Graph;

const int debug_level = 0;

#define DEBUG(min_level, x)      \
  if (debug_level >= min_level)  \
  {                              \
    std::cerr << x << std::endl; \
  }

struct BoatSailorPair
{
  int b, s, c;
};

const int max_possible_spectacle = 50;

void testcase()
{
  int b, s, p;
  std::cin >> b >> s >> p;
  assert(b >= 1 && b <= 500 && s >= 1 && s <= 500 && p >= 1 && p <= b * s && p <= 5000);

  std::vector<BoatSailorPair> boat_sailor_pairs(p);
  for (BoatSailorPair &pair : boat_sailor_pairs)
  {
    std::cin >> pair.b >> pair.s >> pair.c;
    assert(pair.b >= 0 && pair.b < b && pair.s >= 0 && pair.s < s && pair.c >= 1 && pair.c <= max_possible_spectacle);
  }

  int next_node_index = 0;
  const int node_source = next_node_index++;
  const int node_target = next_node_index++;
  const auto get_node_boat = [next_node_index, b](int i) {
    assert(i >= 0 && i < b);
    return next_node_index + i;
  };
  next_node_index += b;
  const auto get_node_sailor = [next_node_index, s](int i) {
    assert(i >= 0 && i < s);
    return next_node_index + i;
  };
  next_node_index += s;
  const int num_nodes = next_node_index;
  Graph G(num_nodes);

  const auto add_edge = [&G](int from, int to, long capacity, long cost) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    auto w_map = boost::get(boost::edge_weight, G);
    const Graph::edge_descriptor e = boost::add_edge(from, to, G).first;
    const Graph::edge_descriptor rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0;
    r_map[e] = rev_e;
    r_map[rev_e] = e;
    w_map[e] = cost;
    w_map[rev_e] = -cost;
  };

  for (int i = 0; i < b; i++)
  {
    add_edge(node_source, get_node_boat(i), 1, 0);
    add_edge(get_node_boat(i), node_target, 1, max_possible_spectacle);
  }

  for (int i = 0; i < s; i++)
  {
    add_edge(get_node_sailor(i), node_target, 1, 0);
  }

  for (BoatSailorPair &pair : boat_sailor_pairs)
  {
    add_edge(get_node_boat(pair.b), get_node_sailor(pair.s), 1, max_possible_spectacle - pair.c);
  }

  boost::successive_shortest_path_nonnegative_weights(G, node_source, node_target);
  const int flow_cost = boost::find_flow_cost(G);
  const int spectacle_sum = b * max_possible_spectacle - flow_cost;
  assert(spectacle_sum > 0);
  std::cout << spectacle_sum << "\n";
}

int main()
{
  std::ios_base::sync_with_stdio(false);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
    DEBUG(1, "");
  }

  return 0;
}


#pragma once
#include <Eigen/Core>

#include "DualIntervals.hpp"

using namespace Eigen;

#ifdef USEDOUBLE
    using NType = double;
#else
    using NType = DI;
#endif

typedef Matrix<NType, Dynamic, Dynamic, RowMajor> Mat;
typedef Matrix<NType, 1, Dynamic, RowMajor> RowVec;
typedef Matrix<Interval, Dynamic, Dynamic, RowMajor> IntervalMat;
typedef Matrix<double, Dynamic, Dynamic, RowMajor> DoubleMat;

////////////////////////////////////////////////////////////////////////////////
//
// TENSOR FUNCTIONS
//
////////////////////////////////////////////////////////////////////////////////
// note: tensors are implemented as arrays (3D) or multi-dimensional arrays (4D+) of matrices

/*
 * 2D-Convolution with variable stride, padding, number of channels/kernels, and optional bias
 * Note: Assumes square image and kernel
 * Note: Bias is a tied bias (i.e., each kernel shares the same bias) -- pass in NULL if no bias
 * Note: delete[] must be called on the returned value explicitly
 *
 * img[n] represents an image with n channels (e.g., 3 for RGB), like a 3D-Tensor
 * kernels[i][j] represents i kernels, each with j channels (note: j must equal n), like a 4D-Tensor
 * stride represents how much the kernel is "moved" when doing the convolution
 * padding represents how much img is padded (with zeros) by on all sides
 */
template <int num_channels, int num_kernels>
Mat* Conv2d(Mat* img, Mat** kernels, RowVec* biases, int stride = 1, int padding = 0) {
    int img_length = img[0].rows();            // side length of the square image
    int kernel_length = kernels[0][0].rows();  // side length of the kernel's square sliding window
    int kernel_size = kernel_length * kernel_length;
    int new_img_length = std::floor(((img_length + 2 * padding - kernel_length) / stride) + 1);

    // pad img
    Mat padded_img[num_channels];
    for (int c = 0; c < num_channels; ++c) {
        padded_img[c] = Mat::Zero(img_length + 2 * padding, img_length + 2 * padding);
        padded_img[c].block(padding, padding, img_length, img_length) = img[c];
    }

    // img im2col, see here: https://leonardoaraujosantos.gitbook.io/artificial-inteligence/machine_learning/deep_learning/convolution_layer/making_faster
    // note: implemention is the transpose of what is described in the link above
    Mat resized_img(new_img_length * new_img_length, num_channels * kernel_size);
    for (int c = 0; c < num_channels; ++c) {
        int curr_resized_row = 0;
        for (int row = 0; row + kernel_length <= img_length + 2 * padding; row += stride) {
            for (int col = 0; col + kernel_length <= img_length + 2 * padding; col += stride) {
                Mat b = padded_img[c].block(row, col, kernel_length, kernel_length);
                Map<RowVec> b_flattened(b.data(), 1, kernel_size);
                resized_img.block(curr_resized_row, c * kernel_size, 1, kernel_size) = b_flattened;
                ++curr_resized_row;
            }
        }
    }
    resized_img.transposeInPlace();

    // kernel im2col
    Mat resized_kernel(num_kernels, num_channels * kernel_size);
    for (int k = 0; k < num_kernels; ++k) {
        for (int c = 0; c < num_channels; ++c) {
            Map<RowVec> k_flattened(kernels[k][c].data(), 1, kernel_size);
            resized_kernel.block(k, c * kernel_size, 1, kernel_size) = k_flattened;
        }
    }

    // convolution as big matrix multiplication now!
    Mat res = resized_kernel * resized_img;

    // col2im, reshape back to correct dimensions, and apply bias (if applicable)
    if (biases) {
        res.colwise() += biases->transpose();  // adds bias value to each row of res (corresponding to each kernel)
    }

    Mat* res_resized = new Mat[num_kernels];
    for (int k = 0; k < num_kernels; ++k) {
        res_resized[k] = Map<Mat>(res.row(k).data(), new_img_length, new_img_length);
    }

    return res_resized;
}

/*
 * Max-Pooling with variable stride, padding, and number of channels/kernel sizes
 * Note: Assumes square image and kernel
 * Note: delete[] must be called on the returned value explicitly
 *
 * img[n] represents an image with n channels (e.g., 3 for RGB), like a 3D-Tensor
 * kernels_length represents the side length of the kernel (in the first two dimensions)
 * padding represents how much img is padded (with zeros) by on all sides
 * stride represents how much the kernel is "moved" when doing the pooling
 */
template <int num_channels>
Mat* MaxPool2d(Mat img[num_channels], int kernel_length, int stride = 1, int padding = 0) {
    int img_length = img[0].rows();
    int new_img_length = std::floor(((img_length + 2 * padding - kernel_length) / stride) + 1);

    // pad img
    Mat padded_img[num_channels];
    for (int c = 0; c < num_channels; ++c) {
        padded_img[c] = Mat::Zero(img_length + 2 * padding, img_length + 2 * padding);
        padded_img[c].block(padding, padding, img_length, img_length) = img[c];
    }

    Mat* pooled_img = new Mat[num_channels];
    for (int c = 0; c < num_channels; ++c) {
        int curr_row = 0, curr_col = 0;
        pooled_img[c] = Mat(new_img_length, new_img_length);
        for (int row = 0; row + kernel_length <= img_length + 2 * padding; row += stride) {
            for (int col = 0; col + kernel_length <= img_length + 2 * padding; col += stride) {
                pooled_img[c](curr_row, curr_col) = padded_img[c].block(row, col, kernel_length, kernel_length).maxCoeff();
                ++curr_col;
                if (curr_col >= new_img_length) {
                    curr_col = 0;
                    ++curr_row;
                }
            }
        }
    }

    return pooled_img;
}

/*
 * Average-Pooling with variable stride, padding, and number of channels/kernel sizes
 * Everything the same as max-pooling, except we take the mean of the values in each window
 */
template <int num_channels>
Mat* AvgPool2d(Mat img[num_channels], int kernel_length, int stride = 1, int padding = 0) {
    int img_length = img[0].rows();
    int new_img_length = std::floor(((img_length + 2 * padding - kernel_length) / stride) + 1);

    // pad img
    Mat padded_img[num_channels];
    for (int c = 0; c < num_channels; ++c) {
        padded_img[c] = Mat::Zero(img_length + 2 * padding, img_length + 2 * padding);
        padded_img[c].block(padding, padding, img_length, img_length) = img[c];
    }

    Mat* pooled_img = new Mat[num_channels];
    for (int c = 0; c < num_channels; ++c) {
        int curr_row = 0, curr_col = 0;
        pooled_img[c] = Mat(new_img_length, new_img_length);
        for (int row = 0; row + kernel_length <= img_length + 2 * padding; row += stride) {
            for (int col = 0; col + kernel_length <= img_length + 2 * padding; col += stride) {
                pooled_img[c](curr_row, curr_col) = padded_img[c].block(row, col, kernel_length, kernel_length).mean();
                ++curr_col;
                if (curr_col >= new_img_length) {
                    curr_col = 0;
                    ++curr_row;
                }
            }
        }
    }

    return pooled_img;
}

/*
 * Pad an image by "padding" amount on all sides
 */
template <int num_channels>
Mat* Pad(Mat img[num_channels], int padding) {
    int img_length = img[0].rows();

	Mat* padded_img = new Mat[num_channels];;
    for (int c = 0; c < num_channels; ++c) {
        padded_img[c] = Mat::Zero(img_length + 2 * padding, img_length + 2 * padding);
        padded_img[c].block(padding, padding, img_length, img_length) = img[c];
    }

	return padded_img;
}

/*
 * Flattens a 3D-Tensor into a single row vector, so that it can be fed into a fully-connected layer
 */
template <int num_channels>
RowVec Flatten(Mat img[num_channels]) {
    int img_size = img[0].rows() * img[0].cols();
    RowVec flattened_img(1, num_channels * img_size);
    for (int c = 0; c < num_channels; ++c) {
        flattened_img.block(0, c * img_size, 1, img_size) = Map<RowVec>(img[c].data(), 1, img_size);
    }
    return flattened_img;
}

// IMPORTANT: use as an in-place operation (output will replace input)
template <int num_channels>
Mat* Relu(Mat* x) {
    for (int c = 0; c < num_channels; ++c) {
        #ifdef USEDOUBLE
            x[c] = x[c].unaryExpr([](double a) { return std::max(0., a); });
        #else
            x[c] = x[c].unaryExpr(std::ref(relu));
        #endif
    }
    return x;
}

////////////////////////////////////////////////////////////////////////////////
//
// MATRIX FUNCTIONS
//
////////////////////////////////////////////////////////////////////////////////

Mat Exp(const Mat& x) {
    return ((x.array()).exp()).matrix();
}

Mat Log(const Mat& x) {
    return ((x.array()).log()).matrix();
}

Mat Sqrt(const Mat& x) {
    return ((x.array()).sqrt()).matrix();
}

Mat Sin(const Mat& x) {
    return ((x.array()).sin()).matrix();
}

Mat Cos(const Mat& x) {
    return ((x.array()).cos()).matrix();
}

Mat Tanh(const Mat& x) {
    return ((x.array()).tanh()).matrix();
}

Mat Atan(const Mat& x) {
    return ((x.array()).atan()).matrix();
}

Mat Logistic(const Mat& x) {
    return (((x.array() * 0.5).tanh()) * 0.5 + 0.5).matrix();
}

Mat Softmax(const Mat& x) {
    Mat temp = Exp(x);
    Mat::Scalar total = temp.sum();
    return temp / total;
}

Mat Relu(const Mat& x) {
    #ifdef USEDOUBLE
        return x.unaryExpr([](double a) { return std::max(0., a); });
    #else
        return x.unaryExpr(std::ref(relu));
    #endif
}

Mat Dropout(const Mat& mask, const Mat& x) {
    return (mask.array() * x.array()).matrix();
}

template <class MatType>
double Norm1(const MatType& array) {
    size_t num_rows = array.rows();
    size_t num_cols = array.cols();

    double norm = 0.;
    for (size_t j = 0; j < num_cols; ++j) {
        double sum = 0.;
        for (size_t i = 0; i < num_rows; ++i) {
            #ifdef USEDOUBLE
                sum += std::abs(array(i, j));
            #else
                Interval ij = array(i, j);
                sum += std::max(std::abs(ij.lower()), std::abs(ij.upper()));
            #endif
        }
        norm = std::max(norm, sum);
    }
    return norm;
}

template <class MatType>
double NormInf(const MatType& array) {
    size_t num_rows = array.rows();
    size_t num_cols = array.cols();

    double norm = 0.;
    for (size_t i = 0; i < num_rows; ++i) {
        double sum = 0.;
        for (size_t j = 0; j < num_cols; ++j) {
            #ifdef USEDOUBLE
                sum += std::abs(array(i, j));
            #else
                Interval ij = array(i, j);
                sum += std::max(std::abs(ij.lower()), std::abs(ij.upper()));
            #endif
        }
        norm = std::max(norm, sum);
    }
    return norm;
}

void Print(const Mat& x) {
    for (size_t i = 0; i < x.rows(); i++) {
        for (size_t j = 0; j < x.cols(); j++) {
            std::cout << x(i, j) << ", ";
        }
        std::cout << std::endl;
    }
}


/******************************************************************************
 * Author:   Laurent Kneip                                                    *
 * Contact:  kneip.laurent@gmail.com                                          *
 * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
 *                                                                            *
 * Redistribution and use in source and binary forms, with or without         *
 * modification, are permitted provided that the following conditions         *
 * are met:                                                                   *
 * * Redistributions of source code must retain the above copyright           *
 *   notice, this list of conditions and the following disclaimer.            *
 * * Redistributions in binary form must reproduce the above copyright        *
 *   notice, this list of conditions and the following disclaimer in the      *
 *   documentation and/or other materials provided with the distribution.     *
 * * Neither the name of ANU 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 ANU OR THE CONTRIBUTORS BE LIABLE        *
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT         *
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY  *
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF     *
 * SUCH DAMAGE.                                                               *
 ******************************************************************************/

/**
 * \file Sturm.hpp
 * \brief Class for evaluating Sturm chains on polynomials, and bracketing the
 *        real roots.
 */

#ifndef OPENGV_STURM_HPP_
#define OPENGV_STURM_HPP_

#include <stdlib.h>
#include <vector>
#include <Eigen/Eigen>

/**
 * \brief The namespace of this library.
 */
namespace opengv
{
/**
 * \brief The namespace of the math tools.
 */
namespace math
{

/**
 * Sturm is initialized over polynomials of arbitrary order, and used to compute
 * the real roots of the polynomial.
 */
class Sturm
{

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  /** A pair of values bracketing a real root */
  typedef std::pair<double,double> bracket_t;

  /**
   * \brief Contructor.
   * \param[in] p The polynomial coefficients (poly = p(0,0)*x^n + p(0,1)*x^(n-1) ...).
   */
  Sturm( const Eigen::MatrixXd & p );
  /**
   * \brief Contructor.
   * \param[in] p The polynomial coefficients (poly = p[0]*x^n + p[1]*x^(n-1) ...).
   */
  Sturm( const std::vector<double> & p );
  /**
   * \brief Destructor.
   */
  virtual ~Sturm();

  /**
   * \brief Finds the roots of the polynomial.
   * \return An array with the real roots of the polynomial.
   */
  std::vector<double> findRoots();
  /**
   * \brief Finds brackets for the real roots of the polynomial.
   * \return An array of brackets for the real roots of the polynomial.
   */
  std::vector<bracket_t> bracketRoots();
  /**
   * \brief Evaluates the Sturm chain for a bracket.
   * \param[in] leftBound The left value of the bracket.
   * \param[in] rightBound The right value of the bracket.
   * \return The number of real roots in the given bracket.
   */
  size_t evaluateChain( double leftBound, double rightBound );

private:
  /**
   * \brief Internal function used for composing the Sturm chain
   * \param[in] p1 First polynomial.
   * \param[in] p2 Second polynomial.
   * \param[out] r The negated remainder of the polynomial division p1/p2.
   */
  void computeNegatedRemainder(
      const Eigen::MatrixXd & p1,
      const Eigen::MatrixXd & p2,
      Eigen::MatrixXd & r );
  /**
   * \brief Internal function used for composing an initial bracket for all
   *        the roots of the polynomial.
   * \return The maximum of the absolute values of the bracket-values (That's
   *         what the Lagrangian bound is able to find).
   */
  double computeLagrangianBound();

  /** A matrix containing the coefficients of the Sturm-chain of the polynomial */
  Eigen::MatrixXd _C;
  /** The dimension _C, which corresponds to (polynomial order+1) */
  size_t _dimension;
};

}
}

#endif /* OPENGV_STURM_HPP_ */


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

// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// This file was modified by Oracle on 2018-2020.
// Modifications copyright (c) 2018-2020 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_ALGORITHMS_TRANSFORM_HPP
#define BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_ALGORITHMS_TRANSFORM_HPP

#include <type_traits>

#include <boost/geometry/algorithms/convert.hpp>
#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/core/static_assert.hpp>
#include <boost/geometry/extensions/algebra/algorithms/detail.hpp>
#include <boost/geometry/extensions/algebra/geometries/concepts/vector_concept.hpp>
#include <boost/geometry/extensions/algebra/geometries/concepts/rotation_quaternion_concept.hpp>
#include <boost/geometry/extensions/algebra/geometries/concepts/rotation_matrix_concept.hpp>
#include <boost/geometry/geometries/concepts/check.hpp>

namespace boost { namespace geometry {

namespace detail { namespace transform_geometrically {

template <typename Box, typename Vector, std::size_t Dimension>
struct box_vector_cartesian
{
    BOOST_GEOMETRY_STATIC_ASSERT(
        (Dimension > 0),
        "Dimension has to be greater than 0.",
        Box);

    static inline void apply(Box & box, Vector const& vector)
    {
        box_vector_cartesian<Box, Vector, Dimension-1>::apply(box, vector);
        set<min_corner, Dimension-1>(box, get<min_corner, Dimension-1>(box) + get<Dimension-1>(vector));
        set<max_corner, Dimension-1>(box, get<max_corner, Dimension-1>(box) + get<Dimension-1>(vector));
    }
};

template <typename Box, typename Vector>
struct box_vector_cartesian<Box, Vector, 1>
{
    static inline void apply(Box & box, Vector const& vector)
    {
        set<min_corner, 0>(box, get<min_corner, 0>(box) + get<0>(vector));
        set<max_corner, 0>(box, get<max_corner, 0>(box) + get<0>(vector));
    }
};

}} // namespace detail::transform

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch {

template <typename Geometry, typename Transform,
          typename GTag = typename tag<Geometry>::type,
          typename TTag = typename tag<Transform>::type>
struct transform_geometrically
{
    BOOST_GEOMETRY_STATIC_ASSERT_FALSE(
        "Not implemented for this Geometry.",
        Geometry, Transform);
};

// Point translation by Vector
template <typename Point, typename Vector>
struct transform_geometrically<Point, Vector, point_tag, vector_tag>
{
    BOOST_CONCEPT_ASSERT( (concepts::Point<Point>) );
    BOOST_CONCEPT_ASSERT( (concepts::Vector<Vector>) );

    static inline void apply(Point & point, Vector const& vector)
    {
        typedef std::is_same
            <
                typename traits::coordinate_system<Point>::type,
                cs::cartesian
            > is_cartesian;
        apply(point, vector, is_cartesian());
    }

    static inline void apply(Point & point, Vector const& vector, std::true_type /*is_cartesian*/)
    {
        for_each_coordinate(point, detail::point_operation<Vector, std::plus>(vector));
    }

    static inline void apply(Point & point, Vector const& vector, std::false_type /*is_cartesian*/)
    {
        BOOST_GEOMETRY_STATIC_ASSERT_FALSE(
            "Not implemented for this coordinate system.",
            typename traits::coordinate_system<Point>::type);
    }
};

// Box translation by Vector
template <typename Box, typename Vector>
struct transform_geometrically<Box, Vector, box_tag, vector_tag>
{
    typedef typename traits::point_type<Box>::type point_type;

    BOOST_CONCEPT_ASSERT( (concepts::Point<point_type>) );
    BOOST_CONCEPT_ASSERT( (concepts::Vector<Vector>) );

    static inline void apply(Box & box, Vector const& vector)
    {
        typedef std::is_same
            <
                typename traits::coordinate_system<point_type>::type,
                cs::cartesian
            > is_cartesian;
        apply(box, vector, is_cartesian());
    }

    static inline void apply(Box & box, Vector const& vector, std::true_type /*is_cartesian*/)
    {
        geometry::detail::transform_geometrically::box_vector_cartesian<
            Box, Vector, traits::dimension<point_type>::value
        >::apply(box, vector);
    }

    static inline void apply(Box & box, Vector const& vector, std::false_type /*is_cartesian*/)
    {
        BOOST_GEOMETRY_STATIC_ASSERT_FALSE(
            "Not implemented for this coordinate system.",
            typename traits::coordinate_system<point_type>::type);
    }
};

// Vector rotation by Quaternion
template <typename Vector, typename RotationQuaternion>
struct transform_geometrically<Vector, RotationQuaternion, vector_tag, rotation_quaternion_tag>
{
    static inline void apply(Vector & v, RotationQuaternion const& r)
    {
        concepts::check_concepts_and_equal_dimensions<Vector, RotationQuaternion const>();

        detail::algebra::quaternion_rotate(v, r);
    }
};

// Vector rotation by Matrix
template <typename Vector, typename RotationMatrix>
struct transform_geometrically<Vector, RotationMatrix, vector_tag, rotation_matrix_tag>
{
    static inline void apply(Vector & v, RotationMatrix const& r)
    {
        concepts::check_concepts_and_equal_dimensions<Vector, RotationMatrix const>();

        // TODO vector_type and convert from Vector
        Vector tmp(v);
        detail::algebra::matrix_rotate(r, tmp, v);
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

template <typename Geometry, typename Transformation>
inline void transform_geometrically(Geometry & g, Transformation const& t)
{
    dispatch::transform_geometrically<Geometry, Transformation>::apply(g, t);
}

template <typename GeometrySrc, typename Transformation, typename GeometryDst>
inline void transformed_geometrically(GeometrySrc const& gsrc, Transformation const& t, GeometryDst & gdst)
{
    geometry::convert(gsrc, gdst);
    geometry::transform_geometrically(gdst, t);
}

template <typename GeometryDst, typename GeometrySrc, typename Transformation>
inline GeometryDst return_transformed_geometrically(GeometrySrc const& gsrc, Transformation const& t)
{
    GeometryDst res;
    transformed_geometrically(gsrc, t, res);
    return res;
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_ALGORITHMS_TRANSFORM_HPP


/*
 * Copyright 2018 Pedro Proenza <p.proenca@surrey.ac.uk> (University of Surrey)
 *
 */

#include <iostream>
#include <cstdio>
#define _USE_MATH_DEFINES
#include <math.h>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include "CAPE/CAPE.h"

bool done = false;
float COS_ANGLE_MAX = cos(M_PI/12);
float MAX_MERGE_DIST = 50.0f;
bool cylinder_detection= true;
CAPE * plane_detector;
std::vector<cv::Vec3b> color_code;

bool loadCalibParameters(std::string filepath, cv:: Mat & intrinsics_rgb, cv::Mat & dist_coeffs_rgb, cv:: Mat & intrinsics_ir, cv::Mat & dist_coeffs_ir, cv::Mat & R, cv::Mat & T){

    cv::FileStorage fs(filepath,cv::FileStorage::READ);
    if (fs.isOpened()){
        fs["RGB_intrinsic_params"]        >> intrinsics_rgb;
        fs["RGB_distortion_coefficients"] >> dist_coeffs_rgb;
        fs["IR_intrinsic_params"]         >> intrinsics_ir;
        fs["IR_distortion_coefficients"]  >> dist_coeffs_ir;
        fs["Rotation"]                    >> R;
        fs["Translation"]                 >> T;
        fs.release();
        return true;
    }else{
        std::cerr << "Calibration file missing" << std::endl;
        return false;
    }
}

void projectPointCloud(cv::Mat & X, cv::Mat & Y, cv::Mat & Z, cv::Mat & U, cv::Mat & V, float fx_rgb, float fy_rgb, float cx_rgb, float cy_rgb, double z_min, Eigen::MatrixXf & cloud_array){

    int width = X.cols;
    int height = X.rows;

    // Project to image coordinates
    cv::divide(X,Z,U,1);
    cv::divide(Y,Z,V,1);
    U = U*fx_rgb + cx_rgb;
    V = V*fy_rgb + cy_rgb;
    // Reusing U as cloud index
    //U = V*width + U + 0.5;

    float * sz, * sx, * sy, * u_ptr, * v_ptr, * id_ptr;
    float z, u, v;
    int id;
    for(int r=0; r< height; r++){
        sx = X.ptr<float>(r);
        sy = Y.ptr<float>(r);
        sz = Z.ptr<float>(r);
        u_ptr = U.ptr<float>(r);
        v_ptr = V.ptr<float>(r);
        for(int c=0; c< width; c++){
            z = sz[c];
            u = u_ptr[c];
            v = v_ptr[c];
            if(z>z_min && u>0 && v>0 && u<width && v<height){
                id = floor(v)*width + u;
                cloud_array(id,0) = sx[c];
                cloud_array(id,1) = sy[c];
                cloud_array(id,2) = z;
            }
        }
    }
}

void organizePointCloudByCell(Eigen::MatrixXf & cloud_in, Eigen::MatrixXf & cloud_out, cv::Mat & cell_map){
    int width = cell_map.cols;
    int height = cell_map.rows;
    int mxn = width*height;
    int mxn2 = 2*mxn;

    int id, it(0);
    int * cell_map_ptr;
    for(int r=0; r< height; r++){
        cell_map_ptr = cell_map.ptr<int>(r);
        for(int c=0; c< width; c++){
            id = cell_map_ptr[c];
            *(cloud_out.data() + id) = *(cloud_in.data() + it);
            *(cloud_out.data() + mxn + id) = *(cloud_in.data() + mxn + it);
            *(cloud_out.data() + mxn2 + id) = *(cloud_in.data() + mxn2 + it);
            it++;
        }
    }
}

int main(int argc, char ** argv){

    std::string sequence;
    int PATCH_SIZE;
    if (argc>1){
        PATCH_SIZE = atoi(argv[1]);
        sequence = argv[2];
    }else{
        PATCH_SIZE = 20;
        sequence = "pipe";
    }

    std::stringstream string_buff;
    std::string data_path = "/home/grvc/programming/CAPE/Data/";
    string_buff << data_path << sequence;

    // Get intrinsics
    cv::Mat K_rgb, K_ir, dist_coeffs_rgb, dist_coeffs_ir, R_stereo, t_stereo;
    std::stringstream calib_path;
    calib_path << string_buff.str() << "/calib_params.xml";
    loadCalibParameters(calib_path.str(), K_rgb, dist_coeffs_rgb, K_ir, dist_coeffs_ir, R_stereo, t_stereo);
    float fx_ir  = K_ir.at<double>(0,0);  float fy_ir = K_ir.at<double>(1,1);
    float cx_ir  = K_ir.at<double>(0,2);  float cy_ir = K_ir.at<double>(1,2);
    float fx_rgb = K_rgb.at<double>(0,0); float fy_rgb = K_rgb.at<double>(1,1);
    float cx_rgb = K_rgb.at<double>(0,2); float cy_rgb = K_rgb.at<double>(1,2);

    // Read frame 1 to allocate and get dimension
    cv::Mat rgb_img, d_img;
    int width, height;
    std::stringstream image_path;
    std::stringstream depth_img_path;
    std::stringstream rgb_img_path;
    rgb_img_path   << string_buff.str() << "/rgb_0.png";
    depth_img_path << string_buff.str() << "/depth_0.png";

    rgb_img = cv::imread(rgb_img_path.str(),cv::IMREAD_COLOR);

    if(rgb_img.data){
        width = rgb_img.cols;
        height = rgb_img.rows;
    }else{
        std::cout << "Error loading file";
        return -1;
    }

    int nr_horizontal_cells = width/PATCH_SIZE;
    int nr_vertical_cells = height/PATCH_SIZE;

    // Pre-computations for backprojection
    cv::Mat_<float> X_pre(height,width);
    cv::Mat_<float> Y_pre(height,width);
    cv::Mat_<float> U(height,width);
    cv::Mat_<float> V(height,width);
    for (int r=0;r<height; r++){
        for (int c=0;c<width; c++){
            // Not efficient but at this stage doesn t matter
            X_pre.at<float>(r,c) = (c-cx_ir)/fx_ir;
            Y_pre.at<float>(r,c) = (r-cy_ir)/fy_ir;
        }
    }

    // Pre-computations for maping an image point cloud to a cache-friendly array where cell's local point clouds are contiguous
    cv::Mat_<int> cell_map(height,width);

    for (int r=0;r<height; r++){
        int cell_r = r/PATCH_SIZE;
        int local_r = r%PATCH_SIZE;
        for (int c=0;c<width; c++){
            int cell_c = c/PATCH_SIZE;
            int local_c = c%PATCH_SIZE;
            cell_map.at<int>(r,c) = (cell_r*nr_horizontal_cells+cell_c)*PATCH_SIZE*PATCH_SIZE + local_r*PATCH_SIZE + local_c;
        }
    }

    cv::Mat_<float> X(height,width);
    cv::Mat_<float> Y(height,width);
    cv::Mat_<float> X_t(height,width);
    cv::Mat_<float> Y_t(height,width);
    Eigen::MatrixXf cloud_array(width*height,3);
    Eigen::MatrixXf cloud_array_organized(width*height,3);

    cv::namedWindow("Seg");

    // Populate with random color codes
    for(int i=0; i<100;i++){
        cv::Vec3b color;
        color[0]=rand()%255;
        color[1]=rand()%255;
        color[2]=rand()%255;
        color_code.push_back(color);
    }

    // Add specific colors for planes
    color_code[0][0] = 0; color_code[0][1] = 0; color_code[0][2] = 255;
    color_code[1][0] = 255; color_code[1][1] = 0; color_code[1][2] = 204;
    color_code[2][0] = 255; color_code[2][1] = 100; color_code[2][2] = 0;
    color_code[3][0] = 0; color_code[3][1] = 153; color_code[3][2] = 255;
    // Add specific colors for cylinders
    color_code[50][0] = 178; color_code[50][1] = 255; color_code[50][2] = 0;
    color_code[51][0] = 255; color_code[51][1] = 0; color_code[51][2] = 51;
    color_code[52][0] = 0; color_code[52][1] = 255; color_code[52][2] = 51;
    color_code[53][0] = 153; color_code[53][1] = 0; color_code[53][2] = 255;

    // Initialize CAPE
    plane_detector = new CAPE(height, width, PATCH_SIZE, PATCH_SIZE, cylinder_detection, COS_ANGLE_MAX, MAX_MERGE_DIST);

    int i=0;
    while(true){

        // Read frame i
        rgb_img_path.str("");
        rgb_img_path << string_buff.str() << "/rgb_" << i << ".png";
        rgb_img = cv::imread(rgb_img_path.str(),cv::IMREAD_COLOR);

        if (!rgb_img.data)
            break;

        std::cout << "Frame: " << i << std::endl;

        // Read depth image
        depth_img_path.str("");
        depth_img_path << string_buff.str() << "/depth_" << i << ".png";

        d_img = cv::imread(depth_img_path.str(), cv::IMREAD_ANYDEPTH);
        d_img.convertTo(d_img, CV_32F);

        // Backproject to point cloud
        X = X_pre.mul(d_img); Y = Y_pre.mul(d_img);
        cloud_array.setZero();

        // The following transformation+projection is only necessary to visualize RGB with overlapped segments
        // Transform point cloud to color reference frame
        X_t = ((float)R_stereo.at<double>(0,0))*X + ((float)R_stereo.at<double>(0,1))*Y + ((float)R_stereo.at<double>(0,2))*d_img + (float)t_stereo.at<double>(0);
        Y_t = ((float)R_stereo.at<double>(1,0))*X + ((float)R_stereo.at<double>(1,1))*Y + ((float)R_stereo.at<double>(1,2))*d_img + (float)t_stereo.at<double>(1);
        d_img = ((float)R_stereo.at<double>(2,0))*X + ((float)R_stereo.at<double>(2,1))*Y + ((float)R_stereo.at<double>(2,2))*d_img + (float)t_stereo.at<double>(2);

        projectPointCloud(X_t, Y_t, d_img, U, V, fx_rgb, fy_rgb, cx_rgb, cy_rgb, t_stereo.at<double>(2), cloud_array);

        cv::Mat_<cv::Vec3b> seg_rz = cv::Mat_<cv::Vec3b>(height,width,cv::Vec3b(0,0,0));
        cv::Mat_<uchar> seg_output = cv::Mat_<uchar>(height,width,uchar(0));

        // Run CAPE
        int nr_planes, nr_cylinders;
        std::vector<PlaneSeg> plane_params;
        std::vector<CylinderSeg> cylinder_params;
        double t1 = cv::getTickCount();
        organizePointCloudByCell(cloud_array, cloud_array_organized, cell_map);
        plane_detector->process(cloud_array_organized, nr_planes, nr_cylinders, seg_output, plane_params, cylinder_params);
        double t2 = cv::getTickCount();
        double time_elapsed = (t2-t1)/(double)cv::getTickFrequency();
        std::cout<<"Total time elapsed: " << time_elapsed << std::endl;

        /* Uncomment this block to print model params
        for(int p_id=0; p_id<nr_planes;p_id++){
            cout<<"[Plane #"<<p_id<<"] with ";
            cout<<"normal: ("<<plane_params[p_id].normal[0]<<" "<<plane_params[p_id].normal[1]<<" "<<plane_params[p_id].normal[2]<<"), ";
            cout<<"d: "<<plane_params[p_id].d<<endl;
        }

        for(int c_id=0; c_id<nr_cylinders;c_id++){
            cout<<"[Cylinder #"<<c_id<<"] with ";
            cout<<"axis: ("<<cylinder_params[c_id].axis[0]<<" "<<cylinder_params[c_id].axis[1]<<" "<<cylinder_params[c_id].axis[2]<<"), ";
            cout<<"center: ("<<cylinder_params[c_id].centers[0].transpose()<<"), ";
            cout<<"radius: "<<cylinder_params[c_id].radii[0]<<endl;
        }
        */

        // Map segments with color codes and overlap segmented image w/ RGB
        uchar * sCode;
        uchar * dColor;
        uchar * srgb;
        int code;
        for(int r=0; r<  height; r++){
            dColor = seg_rz.ptr<uchar>(r);
            sCode = seg_output.ptr<uchar>(r);
            srgb = rgb_img.ptr<uchar>(r);
            for(int c=0; c< width; c++){
                code = *sCode;
                if (code>0){
                    dColor[c*3] =   color_code[code-1][0]/2 + srgb[0]/2;
                    dColor[c*3+1] = color_code[code-1][1]/2 + srgb[1]/2;
                    dColor[c*3+2] = color_code[code-1][2]/2 + srgb[2]/2;;
                }else{
                    dColor[c*3] =  srgb[0];
                    dColor[c*3+1] = srgb[1];
                    dColor[c*3+2] = srgb[2];
                }
                sCode++; srgb++; srgb++; srgb++;
            }
        }

        // Show frame rate and labels
        cv::rectangle(seg_rz,  cv::Point(0,0),cv::Point(width,20), cv::Scalar(0,0,0),-1);
        std::stringstream fps;
        fps << (int)(1/time_elapsed+0.5) << " fps";
        cv::putText(seg_rz, fps.str(), cv::Point(15,15), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255,255,255,1));
        std::cout << "Nr cylinders:" << nr_cylinders << std::endl;
        int cylinder_code_offset = 50;
        // show cylinder labels
        if (nr_cylinders>0){
            std::stringstream text;
            text<<"Cylinders:";

            cv::putText(seg_rz, text.str(), cv::Point(width/2,15), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255,255,255,1));
            for(int j=0;j<nr_cylinders;j++){
                cv::rectangle(seg_rz,  cv::Point(width/2 + 80+15*j,6),cv::Point(width/2 + 90+15*j,16), cv::Scalar(color_code[cylinder_code_offset+j][0],color_code[cylinder_code_offset+j][1],color_code[cylinder_code_offset+j][2]),-1);
            }
        }
        cv::imshow("Seg", seg_rz);
        cv::waitKey(1);
        i++;
    }

    return 0;
}


#include <iostream>
#include <boost/format.hpp>
// Qt headers
#include <QtGui>
#include <CGAL/Qt/GraphicsViewNavigation.h>
#include <QLineF>
#include <QRectF>
#include <QApplication>
#include <QGraphicsScene>
#include <QGraphicsView>
// CGAL headers
#include <CGAL/Qt/PointsGraphicsItem.h>
#include <CGAL/Qt/GraphicsViewPolylineInput.h>
#include <CGAL/Qt/utility.h>
// GraphicsView items and event filters (input classes)
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/Largest_empty_iso_rectangle_2.h>
// the two base classes
#include <QString>
#include <QFileDialog>
#include <QInputDialog>
#include <QGraphicsRectItem>
#include <QGraphicsLineItem>
using namespace std;

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point_2;
typedef CGAL::Triangle_2<K> Triangle_2;

int main(int argc, char **argv)
{
	CGAL::Qt::PointsGraphicsItem<std::vector<Point_2> > * pgi;
	QApplication app(argc, argv);
	QGraphicsScene scene;

	// AREA OF TRIANGLE
	FILE *fp = NULL;
	fp = fopen("../src/input.txt","r");
	int pt1,pt2;
	Point_2 points[3];
	int i=0;

	// DISPLAYING TRIANGL AND ITS AREA
	QPolygonF Triangle;

	while(fscanf(fp,"%d %d",&pt1,&pt2)!=EOF){

		points[i]=Point_2(pt1,pt2);
		Triangle.append(QPointF(pt1,pt2));
		i++;

	}

	// finding area of triangle
	Triangle_2 pgn(points[0],points[1],points[2]);
  	double Area = abs(pgn.area());
  	cout << Area;

	scene.addPolygon(Triangle);
	QString str = QStringLiteral("Area : %1 ").arg(Area);
	scene.addText(str);

    scene.setSceneRect(0,0, 300, 300);

    QGraphicsView* view = new QGraphicsView(&scene);
    CGAL::Qt::GraphicsViewNavigation navigation;
    view->installEventFilter(&navigation);
    view->viewport()->installEventFilter(&navigation);
    view->setRenderHint(QPainter::Antialiasing);

    view->show();

    return app.exec();
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <boost/none.hpp>
#include <boost/optional.hpp>
#include <boost/optional/optional_io.hpp>
#include <cstddef>

#include "DataStructures/DenseMatrix.hpp"
#include "DataStructures/DenseVector.hpp"
#include "Informer/Verbosity.hpp"
#include "NumericalAlgorithms/Convergence/Criteria.hpp"
#include "NumericalAlgorithms/Convergence/HasConverged.hpp"
#include "NumericalAlgorithms/Convergence/Reason.hpp"
#include "NumericalAlgorithms/LinearSolver/InnerProduct.hpp"
#include "Options/Options.hpp"
#include "Utilities/Gsl.hpp"

namespace LinearSolver {
namespace gmres::detail {

// Perform an Arnoldi orthogonalization to find a new `operand` that is
// orthogonal to all vectors in `basis_history`. Appends a new column to the
// `orthogonalization_history` that holds the inner product of the intermediate
// `operand` with each vector in the `basis_history` and itself.
template <typename VarsType>
void arnoldi_orthogonalize(
    const gsl::not_null<VarsType*> operand,
    const gsl::not_null<DenseMatrix<double>*> orthogonalization_history,
    const std::vector<VarsType>& basis_history,
    const size_t iteration) noexcept {
  // Resize matrix and make sure the new entries that are not being filled below
  // are zero.
  orthogonalization_history->resize(iteration + 2, iteration + 1);
  for (size_t j = 0; j < iteration; ++j) {
    (*orthogonalization_history)(iteration + 1, j) = 0.;
  }
  // Arnoldi orthogonalization
  for (size_t j = 0; j < iteration + 1; ++j) {
    const double orthogonalization = inner_product(basis_history[j], *operand);
    (*orthogonalization_history)(j, iteration) = orthogonalization;
    *operand -= orthogonalization * basis_history[j];
  }
  (*orthogonalization_history)(iteration + 1, iteration) =
      sqrt(inner_product(*operand, *operand));
  // Avoid an FPE if the new operand norm is exactly zero. In that case the
  // problem is solved and the algorithm will terminate (see Proposition 9.3 in
  // \cite Saad2003). Since there will be no next iteration we don't need to
  // normalize the operand.
  if (UNLIKELY((*orthogonalization_history)(iteration + 1, iteration) == 0.)) {
    return;
  }
  *operand /= (*orthogonalization_history)(iteration + 1, iteration);
}

// Solve the linear least-squares problem `||beta - H * y||` for `y`, where `H`
// is the Hessenberg matrix given by `orthogonalization_history` and `beta` is
// the vector `(initial_residual, 0, 0, ...)` by updating the QR decomposition
// of `H` from the previous iteration with a Givens rotation.
void solve_minimal_residual(
    gsl::not_null<DenseMatrix<double>*> orthogonalization_history,
    gsl::not_null<DenseVector<double>*> residual_history,
    gsl::not_null<DenseVector<double>*> givens_sine_history,
    gsl::not_null<DenseVector<double>*> givens_cosine_history,
    size_t iteration) noexcept;

// Find the vector that minimizes the residual by inverting the upper
// triangular matrix obtained above.
DenseVector<double> minimal_residual_vector(
    const DenseMatrix<double>& orthogonalization_history,
    const DenseVector<double>& residual_history) noexcept;

}  // namespace gmres::detail

namespace Serial {

template <typename VarsType>
struct IdentityPreconditioner {
  VarsType operator()(const VarsType& arg) const noexcept { return arg; }
};

struct NoIterationCallback {
  void operator()(const Convergence::HasConverged& /*has_converged*/) const
      noexcept {}
};

/*!
 * \brief A serial GMRES iterative solver for nonsymmetric linear systems of
 * equations.
 *
 * This is an iterative algorithm to solve general linear equations \f$Ax=b\f$
 * where \f$A\f$ is a linear operator. See \cite Saad2003, chapter 6.5 for a
 * description of the GMRES algorithm and Algorithm 9.6 for this implementation.
 * It is matrix-free, which means the operator \f$A\f$ needs not be provided
 * explicity as a matrix but only the operator action \f$A(x)\f$ must be
 * provided for an argument \f$x\f$.
 *
 * The GMRES algorithm does not require the operator \f$A\f$ to be symmetric or
 * positive-definite. Note that other algorithms such as conjugate gradients may
 * be more efficient for symmetric positive-definite operators.
 *
 * \par Convergence:
 * Given a set of \f$N_A\f$ equations (e.g. through an \f$N_A\times N_A\f$
 * matrix) the GMRES algorithm will converge to numerical precision in at most
 * \f$N_A\f$ iterations. However, depending on the properties of the linear
 * operator, an approximate solution can ideally be obtained in only a few
 * iterations. See \cite Saad2003, section 6.11.4 for details on the convergence
 * of the GMRES algorithm.
 *
 * \par Restarting:
 * This implementation of the GMRES algorithm supports restarting, as detailed
 * in \cite Saad2003, section 6.5.5. Since the GMRES algorithm iteratively
 * builds up an orthogonal basis of the solution space the cost of each
 * iteration increases linearly with the number of iterations. Therefore it is
 * sometimes helpful to restart the algorithm every \f$N_\mathrm{restart}\f$
 * iterations, discarding the set of basis vectors and starting again from the
 * current solution estimate. This strategy can improve the performance of the
 * solver, but note that the solver can stagnate for non-positive-definite
 * operators and is not guaranteed to converge within \f$N_A\f$ iterations
 * anymore. Set the `restart` argument of the constructor to
 * \f$N_\mathrm{restart}\f$ to activate restarting, or set it to zero to
 * deactivate restarting (default behaviour).
 *
 * \par Preconditioning:
 * This implementation of the GMRES algorithm also supports preconditioning.
 * You can provide a linear operator \f$P\f$ that approximates the inverse of
 * the operator \f$A\f$ to accelerate the convergence of the linear solve.
 * The algorithm is right-preconditioned, which allows the preconditioner to
 * change in every iteration ("flexible" variant). See \cite Saad2003, sections
 * 9.3.2 and 9.4.1 for details. This implementation follows Algorithm 9.6 in
 * \cite Saad2003.
 *
 * \par Improvements:
 * Further improvements can potentially be implemented for this algorithm, see
 * e.g. \cite Ayachour2003.
 *
 * \example
 * \snippet NumericalAlgorithms/LinearSolver/Test_Gmres.cpp gmres_example
 */
template <typename VarsType>
struct Gmres {
 private:
  struct ConvergenceCriteria {
    using type = Convergence::Criteria;
    static constexpr OptionString help =
        "Determine convergence of the algorithm";
  };
  struct Restart {
    using type = size_t;
    static constexpr OptionString help = "Iterations to run before restarting";
    static size_t default_value() noexcept { return 0; }
  };
  struct Verbosity {
    using type = ::Verbosity;
    static constexpr OptionString help = "Logging verbosity";
  };

 public:
  static constexpr OptionString help =
      "A serial GMRES iterative solver for nonsymmetric linear systems of\n"
      "equations Ax=b. It will converge to numerical precision in at most N_A\n"
      "iterations, where N_A is the number of equations represented by the\n"
      "linear operator A, but will ideally converge to a reasonable\n"
      "approximation of the solution x in only a few iterations.\n"
      "\n"
      "Restarting: It is sometimes helpful to restart the algorithm every\n"
      "N_restart iterations to speed it up. Note that it can stagnate for\n"
      "non-positive-definite matrices and is not guaranteed to converge\n"
      "within N_A iterations anymore when restarting is activated.\n"
      "Activate restarting by setting the 'Restart' option to N_restart, or\n"
      "deactivate restarting by setting it to zero (default).";
  using options = tmpl::list<ConvergenceCriteria, Verbosity, Restart>;

  Gmres(Convergence::Criteria convergence_criteria, ::Verbosity verbosity,
        size_t restart = 0) noexcept
      // clang-tidy: trivially copyable
      : convergence_criteria_(std::move(convergence_criteria)),  // NOLINT
        verbosity_(std::move(verbosity)),                        // NOLINT
        restart_(restart > 0 ? restart : convergence_criteria_.max_iterations) {
    initialize();
  }

  Gmres() = default;
  Gmres(const Gmres& /*rhs*/) = default;
  Gmres& operator=(const Gmres& /*rhs*/) = default;
  Gmres(Gmres&& /*rhs*/) = default;
  Gmres& operator=(Gmres&& /*rhs*/) = default;
  ~Gmres() = default;

  void initialize() noexcept {
    orthogonalization_history_.reserve(restart_ + 1);
    residual_history_.reserve(restart_ + 1);
    givens_sine_history_.reserve(restart_);
    givens_cosine_history_.reserve(restart_);
    basis_history_.resize(restart_ + 1);
    preconditioned_basis_history_.resize(restart_);
  }

  const Convergence::Criteria& convergence_criteria() const noexcept {
    return convergence_criteria_;
  }

  void pup(PUP::er& p) noexcept {  // NOLINT
    p | convergence_criteria_;
    p | verbosity_;
    p | restart_;
    if (p.isUnpacking()) {
      initialize();
    }
  }

  /*!
   * \brief Iteratively solve the problem \f$Ax=b\f$ for \f$x\f$ where \f$A\f$
   * is the `linear_operator` and \f$b\f$ is the `source`, starting \f$x\f$ at
   * `initial_guess`.
   *
   * Optionally provide a `preconditioner` (see class documentation).
   *
   * \return An instance of `Convergence::HasConverged` that provides
   * information on the convergence status of the completed solve, and the
   * approximate solution \f$x\f$.
   */
  template <typename LinearOperator, typename SourceType,
            typename Preconditioner = IdentityPreconditioner<VarsType>,
            typename IterationCallback = NoIterationCallback>
  std::pair<Convergence::HasConverged, VarsType> operator()(
      LinearOperator&& linear_operator, const SourceType& source,
      const VarsType& initial_guess,
      Preconditioner&& preconditioner = IdentityPreconditioner<VarsType>{},
      IterationCallback&& iteration_callback = NoIterationCallback{}) const
      noexcept;

 private:
  Convergence::Criteria convergence_criteria_{};
  ::Verbosity verbosity_{::Verbosity::Verbose};
  size_t restart_{};

  // Memory buffers to avoid re-allocating memory for successive solves:
  // The `orthogonalization_history_` is built iteratively from inner products
  // between existing and potential basis vectors and then Givens-rotated to
  // become upper-triangular.
  mutable DenseMatrix<double> orthogonalization_history_{};
  // The `residual_history_` holds the remaining residual in its last entry, and
  // the other entries `g` "source" the minimum residual vector `y` in
  // `R * y = g` where `R` is the upper-triangular `orthogonalization_history_`.
  mutable DenseVector<double> residual_history_{};
  // These represent the accumulated Givens rotations up to the current
  // iteration.
  mutable DenseVector<double> givens_sine_history_{};
  mutable DenseVector<double> givens_cosine_history_{};
  // These represent the orthogonal Krylov-subspace basis that is constructed
  // iteratively by Arnoldi-orthogonalizing a new vector in each iteration and
  // appending it to the `basis_history_`.
  mutable std::vector<VarsType> basis_history_{};
  // When a preconditioner is used it is applied to each new basis vector. The
  // preconditioned basis is used to construct the solution when the algorithm
  // has converged.
  mutable std::vector<VarsType> preconditioned_basis_history_{};
};

template <typename VarsType>
template <typename LinearOperator, typename SourceType, typename Preconditioner,
          typename IterationCallback>
std::pair<Convergence::HasConverged, VarsType> Gmres<VarsType>::operator()(
    LinearOperator&& linear_operator, const SourceType& source,
    const VarsType& initial_guess, Preconditioner&& preconditioner,
    IterationCallback&& iteration_callback) const noexcept {
  constexpr bool use_preconditioner =
      not std::is_same_v<Preconditioner, IdentityPreconditioner<VarsType>>;
  constexpr bool use_iteration_callback =
      not std::is_same_v<IterationCallback, NoIterationCallback>;

  auto result = initial_guess;
  Convergence::HasConverged has_converged{};
  size_t iteration = 0;

  while (not has_converged) {
    auto& initial_operand = basis_history_[0] =
        source - linear_operator(result);
    const double initial_residual_magnitude =
        sqrt(inner_product(initial_operand, initial_operand));
    has_converged = Convergence::HasConverged{convergence_criteria_, iteration,
                                              initial_residual_magnitude,
                                              initial_residual_magnitude};
    if (use_iteration_callback) {
      iteration_callback(has_converged);
    }
    if (UNLIKELY(has_converged)) {
      break;
    }
    initial_operand /= initial_residual_magnitude;
    residual_history_.resize(1);
    residual_history_[0] = initial_residual_magnitude;
    for (size_t k = 0; k < restart_; ++k) {
      auto& operand = basis_history_[k + 1];
      if (use_preconditioner) {
        preconditioned_basis_history_[k] = preconditioner(basis_history_[k]);
      }
      operand =
          linear_operator(use_preconditioner ? preconditioned_basis_history_[k]
                                             : basis_history_[k]);
      // Find a new orthogonal basis vector of the Krylov subspace
      gmres::detail::arnoldi_orthogonalize(
          make_not_null(&operand), make_not_null(&orthogonalization_history_),
          basis_history_, k);
      // Least-squares solve for the minimal residual
      gmres::detail::solve_minimal_residual(
          make_not_null(&orthogonalization_history_),
          make_not_null(&residual_history_),
          make_not_null(&givens_sine_history_),
          make_not_null(&givens_cosine_history_), k);
      ++iteration;
      has_converged = Convergence::HasConverged{
          convergence_criteria_, iteration, abs(residual_history_[k + 1]),
          initial_residual_magnitude};
      if (use_iteration_callback) {
        iteration_callback(has_converged);
      }
      if (UNLIKELY(has_converged)) {
        break;
      }
    }
    // Find the vector w.r.t. the constructed orthogonal basis of the Krylov
    // subspace that minimizes the residual
    const auto minres = gmres::detail::minimal_residual_vector(
        orthogonalization_history_, residual_history_);
    // Construct the solution from the orthogonal basis and the minimal residual
    // vector
    for (size_t i = 0; i < minres.size(); ++i) {
      result +=
          minres[i] * gsl::at(use_preconditioner ? preconditioned_basis_history_
                                                 : basis_history_,
                              i);
    }
  }
  // NOLINTNEXTLINE(hicpp-move-const-arg,performance-move-const-arg)
  return {std::move(has_converged), std::move(result)};
}

}  // namespace Serial
}  // namespace LinearSolver