// Copyright (c) 2022, ETH Zurich and UNC Chapel Hill.
// 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 ETH Zurich and UNC Chapel Hill 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 HOLDERS 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.
//
// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)

#include "estimators/homography_matrix.h"

#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>

#include "base/projection.h"
#include "estimators/utils.h"
#include "util/logging.h"

namespace colmap {

std::vector<HomographyMatrixEstimator::M_t> HomographyMatrixEstimator::Estimate(
    const std::vector<X_t>& points1, const std::vector<Y_t>& points2) {
  CHECK_EQ(points1.size(), points2.size());

  const size_t N = points1.size();

  // Center and normalize image points for better numerical stability.
  std::vector<X_t> normed_points1;
  std::vector<Y_t> normed_points2;
  Eigen::Matrix3d points1_norm_matrix;
  Eigen::Matrix3d points2_norm_matrix;
  CenterAndNormalizeImagePoints(points1, &normed_points1, &points1_norm_matrix);
  CenterAndNormalizeImagePoints(points2, &normed_points2, &points2_norm_matrix);

  // Setup constraint matrix.
  Eigen::Matrix<double, Eigen::Dynamic, 9> A = Eigen::MatrixXd::Zero(2 * N, 9);

  for (size_t i = 0, j = N; i < points1.size(); ++i, ++j) {
    const double s_0 = normed_points1[i](0);
    const double s_1 = normed_points1[i](1);
    const double d_0 = normed_points2[i](0);
    const double d_1 = normed_points2[i](1);

    A(i, 0) = -s_0;
    A(i, 1) = -s_1;
    A(i, 2) = -1;
    A(i, 6) = s_0 * d_0;
    A(i, 7) = s_1 * d_0;
    A(i, 8) = d_0;

    A(j, 3) = -s_0;
    A(j, 4) = -s_1;
    A(j, 5) = -1;
    A(j, 6) = s_0 * d_1;
    A(j, 7) = s_1 * d_1;
    A(j, 8) = d_1;
  }

  // Solve for the nullspace of the constraint matrix.
  Eigen::JacobiSVD<Eigen::Matrix<double, Eigen::Dynamic, 9>> svd(
      A, Eigen::ComputeFullV);

  const Eigen::VectorXd nullspace = svd.matrixV().col(8);
  Eigen::Map<const Eigen::Matrix3d> H_t(nullspace.data());

  const std::vector<M_t> models = {points2_norm_matrix.inverse() *
                                   H_t.transpose() * points1_norm_matrix};
  return models;
}

void HomographyMatrixEstimator::Residuals(const std::vector<X_t>& points1,
                                          const std::vector<Y_t>& points2,
                                          const M_t& H,
                                          std::vector<double>* residuals) {
  CHECK_EQ(points1.size(), points2.size());

  residuals->resize(points1.size());

  // Note that this code might not be as nice as Eigen expressions,
  // but it is significantly faster in various tests.

  const double H_00 = H(0, 0);
  const double H_01 = H(0, 1);
  const double H_02 = H(0, 2);
  const double H_10 = H(1, 0);
  const double H_11 = H(1, 1);
  const double H_12 = H(1, 2);
  const double H_20 = H(2, 0);
  const double H_21 = H(2, 1);
  const double H_22 = H(2, 2);

  for (size_t i = 0; i < points1.size(); ++i) {
    const double s_0 = points1[i](0);
    const double s_1 = points1[i](1);
    const double d_0 = points2[i](0);
    const double d_1 = points2[i](1);

    const double pd_0 = H_00 * s_0 + H_01 * s_1 + H_02;
    const double pd_1 = H_10 * s_0 + H_11 * s_1 + H_12;
    const double pd_2 = H_20 * s_0 + H_21 * s_1 + H_22;

    const double inv_pd_2 = 1.0 / pd_2;
    const double dd_0 = d_0 - pd_0 * inv_pd_2;
    const double dd_1 = d_1 - pd_1 * inv_pd_2;

    (*residuals)[i] = dd_0 * dd_0 + dd_1 * dd_1;
  }
}

}  // namespace colmap


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Olga Diamanti <olga.diam@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.

#include <igl/angle_bound_frame_fields.h>
#include <igl/edge_topology.h>
#include <igl/local_basis.h>
#include <igl/sparse.h>
#include <igl/speye.h>
#include <igl/slice.h>
#include <igl/polyroots.h>
#include <igl/colon.h>
#include <Eigen/Sparse>

#include <iostream>

namespace igl {

  template <typename DerivedV, typename DerivedF>
  class AngleBoundFFSolverData
  {
    public:
      const Eigen::PlainObjectBase<DerivedV> &V; int numV;
      const Eigen::PlainObjectBase<DerivedF> &F; int numF;

      Eigen::MatrixXi EV; int numE;
      Eigen::MatrixXi F2E;
      Eigen::MatrixXi E2F;
      Eigen::VectorXd K;

      Eigen::VectorXi isBorderEdge;
      int numInteriorEdges;
      Eigen::Matrix<int,Eigen::Dynamic,2> E2F_int;
      Eigen::VectorXi indInteriorToFull;
      Eigen::VectorXi indFullToInterior;

      DerivedV B1, B2, FN;

      //laplacians
      Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar>> DDA, DDB;

  private:
    IGL_INLINE void computeLaplacians();
    IGL_INLINE void computek();
    IGL_INLINE void computeCoefficientLaplacian(int n, Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &D);
    IGL_INLINE void precomputeInteriorEdges();

public:
      IGL_INLINE AngleBoundFFSolverData(const Eigen::PlainObjectBase<DerivedV> &_V,
                                   const Eigen::PlainObjectBase<DerivedF> &_F);
  };

  template <typename DerivedV, typename DerivedF, typename DerivedO>
  class AngleBoundFFSolver
  {
  public:
    IGL_INLINE AngleBoundFFSolver(const AngleBoundFFSolverData<DerivedV, DerivedF> &_data,
                                  const typename DerivedV::Scalar &_thetaMin = 30,
                                 int _maxIter = 50,
                                 const typename DerivedV::Scalar &_lambdaInit = 100,
                                 const typename DerivedV::Scalar &_lambdaMultFactor = 1.01,
                                const bool _doHardConstraints = false);
    IGL_INLINE bool solve(const Eigen::VectorXi &isConstrained,
                          const Eigen::PlainObjectBase<DerivedO> &initialSolution,
                          Eigen::PlainObjectBase<DerivedO> &output,
                          typename DerivedV::Scalar *lambdaOut = NULL);

  private:

    const AngleBoundFFSolverData<DerivedV, DerivedF> &data;

    //polyVF data
    Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> Acoeff, Bcoeff;
    Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 2> pvU, pvV;
    typename DerivedV::Scalar lambda;

    //parameters
    typename DerivedV::Scalar lambdaInit,lambdaMultFactor;
    int maxIter;
    typename DerivedV::Scalar thetaMin;
    bool doHardConstraints;

    typename DerivedV::Scalar computeAngle(const std::complex<typename DerivedV::Scalar> &u,
                                           const std::complex<typename DerivedV::Scalar> &v);
//    IGL_INLINE void computeAngles(Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> &angles);

    IGL_INLINE int getNumOutOfBounds();

    IGL_INLINE void rotateAroundBisector(const std::complex<typename DerivedV::Scalar> &uin,
                         const std::complex<typename DerivedV::Scalar> &vin,
                         const typename DerivedV::Scalar theta,
                         std::complex<typename DerivedV::Scalar> &uout,
                         std::complex<typename DerivedV::Scalar> &vout);

    IGL_INLINE void localStep();

    IGL_INLINE void globalStep(const Eigen::Matrix<int, Eigen::Dynamic, 1>  &isConstrained,
                               const Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1>  &Ak,
                               const Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1>  &Bk);

    IGL_INLINE void minQuadWithKnownMini(const Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &Q,
                         const Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &f,
                         const Eigen::VectorXi isConstrained,
                         const Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> &xknown,
                                         Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> &x);
    IGL_INLINE void setFieldFromCoefficients();
    IGL_INLINE void setCoefficientsFromField();

  };
}

//Implementation
/***************************** Data ***********************************/

template <typename DerivedV, typename DerivedF>
IGL_INLINE igl::AngleBoundFFSolverData<DerivedV, DerivedF>::
AngleBoundFFSolverData(const Eigen::PlainObjectBase<DerivedV> &_V,
                  const Eigen::PlainObjectBase<DerivedF> &_F):
V(_V),
numV(_V.rows()),
F(_F),
numF(_F.rows())
{
  igl::edge_topology(V,F,EV,F2E,E2F);
  numE = EV.rows();

  precomputeInteriorEdges();

  igl::local_basis(V,F,B1,B2,FN);

  computek();

  computeLaplacians();

};

template <typename DerivedV, typename DerivedF>
IGL_INLINE void igl::AngleBoundFFSolverData<DerivedV, DerivedF>::computeLaplacians()
{
  computeCoefficientLaplacian(2, DDA);

  computeCoefficientLaplacian(4, DDB);
}

template<typename DerivedV, typename DerivedF>
IGL_INLINE void igl::AngleBoundFFSolverData<DerivedV, DerivedF>::
precomputeInteriorEdges()
{
  // Flag border edges
  numInteriorEdges = 0;
  isBorderEdge.setZero(numE,1);
  indFullToInterior = Eigen::VectorXi::Constant(numE,-1);

  for(unsigned i=0; i<numE; ++i)
  {
    if ((E2F(i,0) == -1) || ((E2F(i,1) == -1)))
      isBorderEdge[i] = 1;
    else
    {
      indFullToInterior[i] = numInteriorEdges;
      numInteriorEdges++;
    }
  }

  E2F_int.resize(numInteriorEdges, 2);
  indInteriorToFull.setZero(numInteriorEdges,1);
  int ii = 0;
  for (int k=0; k<numE; ++k)
  {
    if (isBorderEdge[k])
      continue;
    E2F_int.row(ii) = E2F.row(k);
    indInteriorToFull[ii] = k;
    ii++;
  }

}

template<typename DerivedV, typename DerivedF>
IGL_INLINE void igl::AngleBoundFFSolverData<DerivedV, DerivedF>::
computeCoefficientLaplacian(int n, Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &D)
{
  std::vector<Eigen::Triplet<std::complex<typename DerivedV::Scalar> >> tripletList;

  // For every non-border edge
  for (unsigned eid=0; eid<numE; ++eid)
  {
    if (!isBorderEdge[eid])
    {
      int fid0 = E2F(eid,0);
      int fid1 = E2F(eid,1);

      tripletList.push_back(Eigen::Triplet<std::complex<typename DerivedV::Scalar> >(fid0,
                                                                                fid0,
                                                                                std::complex<typename DerivedV::Scalar>(1.)));
      tripletList.push_back(Eigen::Triplet<std::complex<typename DerivedV::Scalar> >(fid1,
                                                                                fid1,
                                                                                std::complex<typename DerivedV::Scalar>(1.)));
      tripletList.push_back(Eigen::Triplet<std::complex<typename DerivedV::Scalar> >(fid0,
                                                                                fid1,
                                                                                -1.*std::polar(1.,-1.*n*K[eid])));
      tripletList.push_back(Eigen::Triplet<std::complex<typename DerivedV::Scalar> >(fid1,
                                                                                fid0,
                                                                                -1.*std::polar(1.,1.*n*K[eid])));

    }
  }
  D.resize(numF,numF);
  D.setFromTriplets(tripletList.begin(), tripletList.end());

}

template<typename DerivedV, typename DerivedF>
IGL_INLINE void igl::AngleBoundFFSolverData<DerivedV, DerivedF>::
computek()
{
  K.setZero(numE);
  // For every non-border edge
  for (unsigned eid=0; eid<numE; ++eid)
  {
    if (!isBorderEdge[eid])
    {
      int fid0 = E2F(eid,0);
      int fid1 = E2F(eid,1);

      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> N0 = FN.row(fid0);
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> N1 = FN.row(fid1);

      // find common edge on triangle 0 and 1
      int fid0_vc = -1;
      int fid1_vc = -1;
      for (unsigned i=0;i<3;++i)
      {
        if (F2E(fid0,i) == eid)
          fid0_vc = i;
        if (F2E(fid1,i) == eid)
          fid1_vc = i;
      }
      assert(fid0_vc != -1);
      assert(fid1_vc != -1);

      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> common_edge = V.row(F(fid0,(fid0_vc+1)%3)) - V.row(F(fid0,fid0_vc));
      common_edge.normalize();

      // Map the two triangles in a new space where the common edge is the x axis and the N0 the z axis
      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> P;
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> o = V.row(F(fid0,fid0_vc));
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> tmp = -N0.cross(common_edge);
      P << common_edge, tmp, N0;
      //      P.transposeInPlace();

      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> V0;
      V0.row(0) = V.row(F(fid0,0)) -o;
      V0.row(1) = V.row(F(fid0,1)) -o;
      V0.row(2) = V.row(F(fid0,2)) -o;

      V0 = (P*V0.transpose()).transpose();

      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> V1;
      V1.row(0) = V.row(F(fid1,0)) -o;
      V1.row(1) = V.row(F(fid1,1)) -o;
      V1.row(2) = V.row(F(fid1,2)) -o;
      V1 = (P*V1.transpose()).transpose();

      // compute rotation R such that R * N1 = N0
      // i.e. map both triangles to the same plane
      double alpha = -atan2(V1((fid1_vc+2)%3,2),V1((fid1_vc+2)%3,1));

      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> R;
      R << 1,          0,            0,
      0, cos(alpha), -sin(alpha) ,
      0, sin(alpha),  cos(alpha);
      V1 = (R*V1.transpose()).transpose();

      // measure the angle between the reference frames
      // k_ij is the angle between the triangle on the left and the one on the right
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> ref0 = V0.row(1) - V0.row(0);
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> ref1 = V1.row(1) - V1.row(0);

      ref0.normalize();
      ref1.normalize();

      double ktemp = atan2(ref1(1),ref1(0)) - atan2(ref0(1),ref0(0));

      // just to be sure, rotate ref0 using angle ktemp...
      Eigen::Matrix<typename DerivedV::Scalar, 2, 2> R2;
      R2 << cos(ktemp), -sin(ktemp), sin(ktemp), cos(ktemp);

      Eigen::Matrix<typename DerivedV::Scalar, 1, 2> tmp1 = R2*(ref0.head(2)).transpose();

      K[eid] = ktemp;
    }
  }

}

/***************************** Solver ***********************************/
template <typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO>::
AngleBoundFFSolver(const AngleBoundFFSolverData<DerivedV, DerivedF> &_data,
                   const typename DerivedV::Scalar &_thetaMin,
                  int _maxIter,
                  const typename DerivedV::Scalar &_lambdaInit,
                  const typename DerivedV::Scalar &_lambdaMultFactor,
                   const bool _doHardConstraints):
data(_data),
lambdaInit(_lambdaInit),
maxIter(_maxIter),
lambdaMultFactor(_lambdaMultFactor),
doHardConstraints(_doHardConstraints),
thetaMin(_thetaMin)
{
  Acoeff.resize(data.numF,1);
  Bcoeff.resize(data.numF,1);
  pvU.setZero(data.numF, 2);
  pvV.setZero(data.numF, 2);
};

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE void igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO>::
rotateAroundBisector(const std::complex<typename DerivedV::Scalar> &uin,
                          const std::complex<typename DerivedV::Scalar> &vin,
                          const typename DerivedV::Scalar diff,
                          std::complex<typename DerivedV::Scalar> &uout,
                          std::complex<typename DerivedV::Scalar> &vout)
{
  //rotate 2D complex vectors u and v around their bisector so that their
  //angle is at least theta

  uout = uin;
  vout = vin;
  typename DerivedV::Scalar au = arg(uin);
  typename DerivedV::Scalar av = arg(vin);
  if (au<av)
  {
    uout = std::polar (1.0,-.5*diff)*uin;
    vout = std::polar (1.0, .5*diff)*vin;
  }
  else
  {
    uout = std::polar (1.0, .5*diff)*uin;
    vout = std::polar (1.0,-.5*diff)*vin;
  }

}

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE void igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO>::
localStep()
{
  for (int j =0; j<data.numF; ++j)
  {

    std::complex<typename DerivedV::Scalar> u(pvU(j,0),pvU(j,1));
    std::complex<typename DerivedV::Scalar> v(pvV(j,0),pvV(j,1));

    typename DerivedV::Scalar current_angle = computeAngle(u, v);
    if (current_angle<thetaMin*M_PI/180)
    {
      // bring all to 1st or 4th quarter plane
      if ((arg(u)>=0.5*M_PI || arg(u)<-0.5*M_PI ))
        u = -u;
      if ((arg(v)>=0.5*M_PI || arg(v)<-0.5*M_PI ))
        v = -v;
      assert(fabs(computeAngle(u, v) - current_angle)<1e-5);

      if ( fabs(arg(u) - arg(v)) >0.5*M_PI )
        v = -v;
      assert(fabs(computeAngle(u, v) - current_angle)<1e-5);

      std::complex<typename DerivedV::Scalar> u1, v1;
      typename DerivedV::Scalar diff = thetaMin*M_PI/180 - current_angle + 1e-6;
      rotateAroundBisector(u, v, diff, u1, v1);

//      if (computeAngle(u1, v1)<thetaMin*M_PI/180)
//      {
//        std::cerr<<"u = ["<<real(u)<<","<<imag(u)<< "]; v= ["<<real(v)<<","<<imag(v)<<"];"<<std::endl;
//        std::cerr<<"u1 = ["<<real(u1)<<","<<imag(u1)<< "]; v1= ["<<real(v1)<<","<<imag(v1)<<"];"<<std::endl;
//        std::cerr<<"current_angle = "<<current_angle<<std::endl;
//        std::cerr<<"aout = "<<computeAngle(u1, v1)<< "; theta= "<<thetaMin*M_PI/180<<";"<<std::endl;
//      }
//      assert(computeAngle(u1, v1)>=thetaMin*M_PI/180);

      pvU.row(j) << real(u1),imag(u1);
      pvV.row(j) << real(v1),imag(v1);
    }
  }

}

//
//template<typename DerivedV, typename DerivedF, typename DerivedO>
//IGL_INLINE void igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO>::
//computeAngles(Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> &angles)
//{
//  angles.resize(data.numF,1);
//  for (int i =0; i<data.numF; ++i)
//  {
//    std::complex<typename DerivedV::Scalar> u(pvU(i,0),pvU(i,1));
//    std::complex<typename DerivedV::Scalar> v(pvV(i,0),pvV(i,1));
//    angles[i] = fabs(arg(u) - arg(v));
//    if (angles[i]>M_PI)
//      angles[i] = 2*M_PI-angles[i];
//    if (angles[i]>.5*M_PI)
//      angles[i] = M_PI-angles[i];
//  }
//}

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE typename DerivedV::Scalar igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO>::
computeAngle(const std::complex<typename DerivedV::Scalar> &u,
             const std::complex<typename DerivedV::Scalar> &v)
{
  typename DerivedV::Scalar angle = std::min(fabs(arg(u*conj(v))), fabs(arg(u*conj(-v))));

//  typename DerivedV::Scalar angle;
//  typename DerivedV::Scalar a1 = fabs(arg(u*conj(v)));
//  typename DerivedV::Scalar a2 = fabs(arg(u*conj(-v)));
//  if (a1 < a2)
//    angle = a1;
//  else
//  {
//    angle = a2; v = -v;
//  }

//  typename DerivedV::Scalar angle = fabs(arg(u) - arg(v));
//  if (angle>M_PI)
//  {
//    u = -u;
//    angle = fabs(arg(u) - arg(v));
//  };
//
//  if (angle>.5*M_PI)
//  {
//    v = -v;
//    angle = fabs(arg(u) - arg(v));
//  };
//
//  assert(fabs(angle-angle1)<1e-6);

//  if (angle>M_PI)
//    angle = 2*M_PI-angle;
//  if (angle>.5*M_PI)
//    angle = M_PI-angle;

//  typename DerivedV::Scalar angle = fabs(arg(u) - arg(v));
//    if (angle>M_PI)
//      angle = 2*M_PI-angle;
//    if (angle>.5*M_PI)
//      angle = M_PI-angle;

  assert(angle <= .5*M_PI && angle >0);

  return angle;
}

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE int igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO>::
getNumOutOfBounds()
{
  Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> angles;
//  computeAngles(angles);
  int numOoB = 0;
  for (int i =0; i<data.numF; ++i)
  {
    std::complex<typename DerivedV::Scalar> u(pvU(i,0),pvU(i,1));
    std::complex<typename DerivedV::Scalar> v(pvV(i,0),pvV(i,1));
    typename DerivedV::Scalar angle = computeAngle(u,v);
//    if (angles[i] <thetaMin*M_PI/180)
    if (angle <thetaMin*M_PI/180)
      numOoB ++;
  }
  return numOoB;
}

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE void igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO>::
setCoefficientsFromField()
{
  for (int i = 0; i <data.numF; ++i)
  {
    std::complex<typename DerivedV::Scalar> u(pvU(i,0),pvU(i,1));
    std::complex<typename DerivedV::Scalar> v(pvV(i,0),pvV(i,1));
    Acoeff(i) = u*u+v*v;
    Bcoeff(i) = u*u*v*v;
  }
}

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE void igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO>::
globalStep(const Eigen::Matrix<int, Eigen::Dynamic, 1>  &isConstrained,
           const Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1>  &Ak,
           const Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1>  &Bk)
{
  setCoefficientsFromField();

  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > I;
  igl::speye(data.numF, data.numF, I);
  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > QA = data.DDA+lambda*I;
  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > fA = (-2*lambda*Acoeff).sparseView();

  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > QB = data.DDB+lambda*I;
  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > fB = (-2*lambda*I*Bcoeff).sparseView();

  if(doHardConstraints)
  {
    minQuadWithKnownMini(QA, fA, isConstrained, Ak, Acoeff);
    minQuadWithKnownMini(QB, fB, isConstrained, Bk, Bcoeff);
  }
  else
  {
    Eigen::Matrix<int, Eigen::Dynamic, 1>isknown_; isknown_.setZero(data.numF,1);
    Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> xknown_; xknown_.setZero(0,1);
    minQuadWithKnownMini(QA, fA, isknown_, xknown_, Acoeff);
    minQuadWithKnownMini(QB, fB, isknown_, xknown_, Bcoeff);
  }
  setFieldFromCoefficients();

}

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE void igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO>::
setFieldFromCoefficients()
{
  for (int i = 0; i <data.numF; ++i)
  {
    //    poly coefficients: 1, 0, -Acoeff, 0, Bcoeff
    //    matlab code from roots (given there are no trailing zeros in the polynomial coefficients)
    Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> polyCoeff(5,1);
    polyCoeff<<1., 0., -Acoeff(i), 0., Bcoeff(i);

    Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> roots;
    polyRoots<std::complex<typename DerivedV::Scalar>>(polyCoeff,roots);

    std::complex<typename DerivedV::Scalar> u = roots[0];
    int maxi = -1;
    float maxd = -1;
    for (int k =1; k<4; ++k)
    {
      float dist = abs(roots[k]+u);
      if (dist>maxd)
      {
        maxd = dist;
        maxi = k;
      }
    }
    std::complex<typename DerivedV::Scalar> v = roots[maxi];
    pvU(i,0) = real(u); pvU(i,1) = imag(u);
    pvV(i,0) = real(v); pvV(i,1) = imag(v);
  }

}

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE void igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO>::
minQuadWithKnownMini(const Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &Q,
                     const Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &f,
                     const Eigen::VectorXi isConstrained,
                     const Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> &xknown,
                     Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> &x)
{
  int N = Q.rows();

  int nc = xknown.rows();
  Eigen::VectorXi known; known.setZero(nc,1);
  Eigen::VectorXi unknown; unknown.setZero(N-nc,1);

  int indk = 0, indu = 0;
  for (int i = 0; i<N; ++i)
    if (isConstrained[i])
    {
      known[indk] = i;
      indk++;
    }
    else
    {
      unknown[indu] = i;
      indu++;
    }

  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar>> Quu, Quk;

  igl::slice(Q,unknown, unknown, Quu);
  igl::slice(Q,unknown, known, Quk);

  std::vector<typename Eigen::Triplet<std::complex<typename DerivedV::Scalar> > > tripletList;

  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > fu(N-nc,1);

  igl::slice(f,unknown, Eigen::VectorXi::Zero(1,1), fu);

  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > rhs = (Quk*xknown).sparseView()+.5*fu;

  Eigen::SparseLU< Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar>>> solver;
  solver.compute(-Quu);
  if(solver.info()!=Eigen::Success)
  {
    std::cerr<<"Decomposition failed!"<<std::endl;
    return;
  }
  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar>>  b  = solver.solve(rhs);
  if(solver.info()!=Eigen::Success)
  {
    std::cerr<<"Solving failed!"<<std::endl;
    return;
  }

  indk = 0, indu = 0;
  x.setZero(N,1);
  for (int i = 0; i<N; ++i)
    if (isConstrained[i])
      x[i] = xknown[indk++];
    else
      x[i] = b.coeff(indu++,0);

}

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE bool igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO>::
solve(const Eigen::VectorXi &isConstrained,
      const Eigen::PlainObjectBase<DerivedO> &initialSolution,
      Eigen::PlainObjectBase<DerivedO> &output,
      typename DerivedV::Scalar *lambdaOut)
{
  int numConstrained = isConstrained.sum();
  // coefficient values
  Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> Ak, Bk;

  pvU.resize(data.numF,2);
  pvV.resize(data.numF,2);
  for (int fi = 0; fi <data.numF; ++fi)
  {
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &b1 = data.B1.row(fi);
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &b2 = data.B2.row(fi);
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &u3 = initialSolution.block(fi,0,1,3);
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &v3 = initialSolution.block(fi,3,1,3);
    pvU.row(fi)<< u3.dot(b1), u3.dot(b2);
    pvV.row(fi)<< v3.dot(b1), v3.dot(b2);
  }
  setCoefficientsFromField();
  Ak.resize(numConstrained,1);
  Bk.resize(numConstrained,1);
  int ind = 0;
  for (int i = 0; i <data.numF; ++i)
  {
    if(isConstrained[i])
    {
      Ak(ind) = Acoeff[i];
      Bk(ind) = Bcoeff[i];
      ind ++;
    }
  }

  typename DerivedV::Scalar smoothnessValue;
  int oob;

  smoothnessValue = (Acoeff.adjoint()*data.DDA*Acoeff + Bcoeff.adjoint()*data.DDB*Bcoeff).real()[0];
  printf("\n\nInitial smoothness: %.5g\n",smoothnessValue);
  oob = getNumOutOfBounds();
  printf("\n\nInitial out-of-bounds: %d\n",oob);
  printf(" %d %.5g %d\n",-1, smoothnessValue, oob);

  lambda = lambdaInit;
  for (int iter = 0; iter<maxIter; ++iter)
  {
    printf("\n\n--- Iteration %d ---\n",iter);

    localStep();
    globalStep(isConstrained, Ak, Bk);

    smoothnessValue = (Acoeff.adjoint()*data.DDA*Acoeff + Bcoeff.adjoint()*data.DDB*Bcoeff).real()[0];

    printf("Smoothness: %.5g\n",smoothnessValue);

    oob = getNumOutOfBounds();

    bool stoppingCriterion = (oob == 0) ;
    if (stoppingCriterion)
      break;
    lambda = lambda*lambdaMultFactor;
//    printf(" %d %.5g %d\n",iter, smoothnessValue, oob);

  }

  output.setZero(data.numF,6);
  for (int fi=0; fi<data.numF; ++fi)
  {
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &b1 = data.B1.row(fi);
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &b2 = data.B2.row(fi);
    output.block(fi,0, 1, 3) = pvU(fi,0)*b1 + pvU(fi,1)*b2;
    output.block(fi,3, 1, 3) = pvV(fi,0)*b1 + pvV(fi,1)*b2;
  }

  if (lambdaOut)
    *lambdaOut = lambda;

  return (oob==0);
}

template <typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE bool igl::angle_bound_frame_fields(const Eigen::PlainObjectBase<DerivedV> &V,
                                            const Eigen::PlainObjectBase<DerivedF> &F,
                                              const typename DerivedV::Scalar &thetaMin,
                                            const Eigen::VectorXi &isConstrained,
                                            const Eigen::PlainObjectBase<DerivedO> &initialSolution,
                                            Eigen::PlainObjectBase<DerivedO> &output,
                                            int maxIter,
                                            const typename DerivedV::Scalar &lambdaInit,
                                            const typename DerivedV::Scalar &lambdaMultFactor,
                                              const bool doHardConstraints)
{
  igl::AngleBoundFFSolverData<DerivedV, DerivedF> csdata(V, F);
  igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO> cs(csdata, thetaMin, maxIter, lambdaInit, lambdaMultFactor, doHardConstraints);
  return (cs.solve(isConstrained, initialSolution, output));
}

template <typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE bool igl::angle_bound_frame_fields(const igl::AngleBoundFFSolverData<DerivedV, DerivedF> &csdata,
                                              const typename DerivedV::Scalar &thetaMin,
                                            const Eigen::VectorXi &isConstrained,
                                            const Eigen::PlainObjectBase<DerivedO> &initialSolution,
                                            Eigen::PlainObjectBase<DerivedO> &output,
                                            int maxIter,
                                            const typename DerivedV::Scalar &lambdaInit,
                                            const typename DerivedV::Scalar &lambdaMultFactor,
                                              const bool doHardConstraints,
                                            typename DerivedV::Scalar *lambdaOut)
{
  igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO> cs(csdata, thetaMin, maxIter, lambdaInit, lambdaMultFactor, doHardConstraints);
  return (cs.solve(isConstrained, initialSolution, output, lambdaOut));
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
#endif


#include "vtkTensorLines.h"

#include "TensorLines.hh"
#include "utils.hh"

#include <Eigen/Geometry>

#include <vtkCellIterator.h>
#include <vtkDataArray.h>
#include <vtkDataSet.h>
#include <vtkDataSetAttributes.h>
#include <vtkDoubleArray.h>
#include <vtkGenericCell.h>
#include <vtkIdList.h>
#include <vtkIntArray.h>
#include <vtkUnsignedLongLongArray.h>
#include <vtkPointData.h>
#include <vtkCellData.h>
#include <vtkPoints.h>
#include <vtkPolyData.h>
#include <vtkSmartPointer.h>
#include <vtkUnstructuredGrid.h>

#include <vtkCommand.h>
#include <vtkInformation.h>
#include <vtkInformationVector.h>
#include <vtkObjectFactory.h>
#include <vtkStreamingDemandDrivenPipeline.h>

#include <algorithm>
#include <array>
#include <chrono>
#include <iostream>
#include <list>
#include <map>
#include <unordered_map>
#include <unordered_set>
#include <vector>

using namespace cpp_utils;

namespace
{
// vtk Tensors are stored in row major order by convention
using Mat3d = Eigen::Matrix<double, 3, 3, Eigen::RowMajor, 3, 3>;
using Mat3dm = Eigen::Map<Mat3d>;
using Vec3d = tl::Vec3d;
using Vec3dm = Eigen::Map<Vec3d>;

struct TriFace
{
    std::array<vtkIdType, 3> points;
    vtkIdType cellId;
};

std::vector<TriFace> buildFaceList(vtkDataSet* dataset)
{
    auto face_list = std::vector<TriFace>{};
    auto add_face = [&](vtkIdList* point_ids,
                        vtkIdType cell_id,
                        vtkIdType i1,
                        vtkIdType i2,
                        vtkIdType i3) {
        auto tri = TriFace{{point_ids->GetId(i1),
                            point_ids->GetId(i2),
                            point_ids->GetId(i3)},
                           cell_id};
        face_list.push_back(tri);
    };

    // Collect faces and remember cell ID
    auto it = vtkSmartPointer<vtkCellIterator>(dataset->NewCellIterator());
    for(it->InitTraversal(); !it->IsDoneWithTraversal(); it->GoToNextCell())
    {
        if(it->GetCellType() != VTK_TETRA)
        {
            std::cout << "WARNING: Dataset contains non-tet cells, which will "
                         "be ignored.\n";
            continue;
        }

        auto* point_ids = it->GetPointIds();
        auto cid = it->GetCellId();

        add_face(point_ids, cid, 0, 1, 2);
        add_face(point_ids, cid, 1, 3, 2);
        add_face(point_ids, cid, 0, 3, 1);
        add_face(point_ids, cid, 0, 2, 3);
    }
    return face_list;
}

std::array<vtkSmartPointer<vtkDoubleArray>, 3>
computeCellDerivatives(vtkDataSet* dataset,
                       const char* point_data_name)
{
    auto point_data = vtkSmartPointer<vtkDataArray>(
            dataset->GetPointData()->GetArray(point_data_name));

    auto ncomps = point_data->GetNumberOfComponents();

    auto derivs = vtkSmartPointer<vtkDoubleArray>::New();
    derivs->SetNumberOfComponents(ncomps*3);
    derivs->SetNumberOfTuples(dataset->GetNumberOfCells());

    auto it = vtkSmartPointer<vtkCellIterator>(dataset->NewCellIterator());
    for(it->InitTraversal(); !it->IsDoneWithTraversal(); it->GoToNextCell())
    {
        // if(it->GetCellType() != VTK_TETRA)
        // {
        //     std::cout << "WARNING: Dataset contains non-tet cells, which will "
        //                  "be ignored.\n";
        //     continue;
        // }

        auto* point_ids = it->GetPointIds();
        auto npts = point_ids->GetNumberOfIds();
        auto cid = it->GetCellId();

        // copy point data to work array
        auto pt_data =
                std::vector<double>(as_unsigned(npts * ncomps));
        for(auto i: range(npts))
        {
            point_data->GetTuple(point_ids->GetId(i), &pt_data[as_unsigned(i*ncomps)]);
        }

        auto cell = vtkSmartPointer<vtkGenericCell>::New();
        it->GetCell(cell);
        auto cell_center = std::array<double, 3>{};
        cell->GetParametricCenter(cell_center.data());
        auto deriv_data = std::vector<double>(as_unsigned(ncomps*3));
        cell->Derivatives(0,
                          cell_center.data(),
                          pt_data.data(),
                          ncomps,
                          deriv_data.data());

        derivs->SetTuple(cid, deriv_data.data());
    }

    auto dx = vtkSmartPointer<vtkDoubleArray>::New();
    dx->SetNumberOfComponents(ncomps);
    dx->SetNumberOfTuples(dataset->GetNumberOfCells());
    for(auto i: range(ncomps))
    {
        dx->CopyComponent(i, derivs, i*3);
    }

    auto dy = vtkSmartPointer<vtkDoubleArray>::New();
    dy->SetNumberOfComponents(ncomps);
    dy->SetNumberOfTuples(dataset->GetNumberOfCells());
    for(auto i: range(ncomps))
    {
        dy->CopyComponent(i, derivs, i*3+1);
    }

    auto dz = vtkSmartPointer<vtkDoubleArray>::New();
    dz->SetNumberOfComponents(ncomps);
    dz->SetNumberOfTuples(dataset->GetNumberOfCells());
    for(auto i: range(ncomps))
    {
        dz->CopyComponent(i, derivs, i*3+2);
    }
    return {dx, dy, dz};
}

std::vector<tl::TLResult> computePEVPoints(const std::vector<TriFace>& faces,
                                             vtkPoints* points,
                                             vtkDataArray* array1,
                                             vtkDataArray* array2,
                                             vtkAlgorithm* progress_alg,
                                             const tl::TLOptions& opts)
{
    const auto step = 1. / double(faces.size());
    progress_alg->UpdateProgress(0);
    auto results = std::vector<tl::TLResult>(faces.size());
    auto terminate = false;
#pragma omp parallel for schedule(guided, 1)
    for(auto i = std::size_t{0}; i < faces.size(); ++i)
    {
#pragma omp flush(terminate)
        if(terminate) continue;

        auto face = faces[i];

        auto p1 = Vec3d{};
        points->GetPoint(face.points[0], p1.data());
        auto p2 = Vec3d{};
        points->GetPoint(face.points[1], p2.data());
        auto p3 = Vec3d{};
        points->GetPoint(face.points[2], p3.data());

        auto s1 = Mat3d{};
        array1->GetTuple(face.points[0], s1.data());
        auto s2 = Mat3d{};
        array1->GetTuple(face.points[1], s2.data());
        auto s3 = Mat3d{};
        array1->GetTuple(face.points[2], s3.data());

        auto t1 = Mat3d{};
        array2->GetTuple(face.points[0], t1.data());
        auto t2 = Mat3d{};
        array2->GetTuple(face.points[1], t2.data());
        auto t3 = Mat3d{};
        array2->GetTuple(face.points[2], t3.data());

        auto points = tl::findParallelEigenvectors(
                {s1, s2, s3},
                {t1, t2, t3},
                {p1, p2, p3},
                opts);
        results[i] = points;
#pragma omp critical(progress)
        {
            progress_alg->UpdateProgress(progress_alg->GetProgress() + step);
            if(progress_alg->GetAbortExecute())
            {
                std::cout << "Terminate request accepted" << std::endl;
                terminate = true;
            }
        }
    }
    return results;
}

std::vector<tl::TLResult> computeTCLPoints(const std::vector<TriFace>& faces,
                                             vtkPoints* points,
                                             vtkDataArray* tensors,
                                             vtkDataArray* tx,
                                             vtkDataArray* ty,
                                             vtkDataArray* tz,
                                             vtkAlgorithm* progress_alg,
                                             const tl::TLOptions& opts)
{
    const auto step = 1. / double(faces.size());
    progress_alg->UpdateProgress(0);
    auto results = std::vector<tl::TLResult>(faces.size());
    auto terminate = false;
#pragma omp parallel for
    for(auto i = std::size_t{0}; i < faces.size(); ++i)
    {
#pragma omp flush(terminate)
        if(terminate) continue;

        auto face = faces[i];

        auto p1 = Vec3d{};
        points->GetPoint(face.points[0], p1.data());
        auto p2 = Vec3d{};
        points->GetPoint(face.points[1], p2.data());
        auto p3 = Vec3d{};
        points->GetPoint(face.points[2], p3.data());

        auto s1 = Mat3d{};
        tensors->GetTuple(face.points[0], s1.data());
        auto s2 = Mat3d{};
        tensors->GetTuple(face.points[1], s2.data());
        auto s3 = Mat3d{};
        tensors->GetTuple(face.points[2], s3.data());

        auto sx = Mat3d{};
        tx->GetTuple(face.cellId, sx.data());

        auto sy = Mat3d{};
        ty->GetTuple(face.cellId, sy.data());

        auto sz = Mat3d{};
        tz->GetTuple(face.cellId, sz.data());

        auto points = tl::findTensorCoreLines(
                {s1, s2, s3},
                {sx, sy, sz},
                {p1, p2, p3},
                opts);
        results[i] = points;
#pragma omp critical(progress)
        {
            progress_alg->UpdateProgress(progress_alg->GetProgress() + step);
            if(progress_alg->GetAbortExecute())
            {
                std::cout << "Terminate request accepted" << std::endl;
                terminate = true;
            }
        }
    }
    return results;
}

std::vector<tl::TLResult> computeTopoPoints(const std::vector<TriFace>& faces,
                                            vtkPoints* points,
                                            vtkDataArray* tensors,
                                            vtkAlgorithm* progress_alg,
                                            const tl::TLOptions& opts)
{
    const auto step = 1. / double(faces.size());
    progress_alg->UpdateProgress(0);
    auto results = std::vector<tl::TLResult>(faces.size());
    auto terminate = false;
#pragma omp parallel for
    for(auto i = std::size_t{0}; i < faces.size(); ++i)
    {
#pragma omp flush(terminate)
        if(terminate) continue;

        auto face = faces[i];

        auto p1 = Vec3d{};
        points->GetPoint(face.points[0], p1.data());
        auto p2 = Vec3d{};
        points->GetPoint(face.points[1], p2.data());
        auto p3 = Vec3d{};
        points->GetPoint(face.points[2], p3.data());

        auto s1 = Mat3d{};
        tensors->GetTuple(face.points[0], s1.data());
        auto s2 = Mat3d{};
        tensors->GetTuple(face.points[1], s2.data());
        auto s3 = Mat3d{};
        tensors->GetTuple(face.points[2], s3.data());

        auto points = tl::findTensorTopology(
                {s1, s2, s3},
                {p1, p2, p3},
                opts);
        results[i] = points;
#pragma omp critical(progress)
        {
            progress_alg->UpdateProgress(progress_alg->GetProgress() + step);
            if(progress_alg->GetAbortExecute())
            {
                std::cout << "Terminate request accepted" << std::endl;
                terminate = true;
            }
        }
    }
    return results;
}
}

vtkStandardNewMacro(vtkTensorLines)

vtkTensorLines::vtkTensorLines()
{
    this->SetNumberOfInputPorts(1);
    this->SetNumberOfOutputPorts(1);
    this->SetInputArrayToProcess(0,
                                 0,
                                 0,
                                 vtkDataObject::FIELD_ASSOCIATION_POINTS,
                                 vtkDataSetAttributes::TENSORS);
    this->SetInputArrayToProcess(1,
                                 0,
                                 0,
                                 vtkDataObject::FIELD_ASSOCIATION_POINTS,
                                 vtkDataSetAttributes::TENSORS);
}

vtkPolyData* vtkTensorLines::GetOutput()
{
    return this->GetOutput(0);
}

vtkPolyData* vtkTensorLines::GetOutput(int port)
{
    return vtkPolyData::SafeDownCast(this->GetOutputDataObject(port));
}

int vtkTensorLines::ProcessRequest(vtkInformation* request,
                                   vtkInformationVector** inputVector,
                                   vtkInformationVector* outputVector)
{
    // Create an output object of the correct type.
    if(request->Has(vtkDemandDrivenPipeline::REQUEST_DATA_OBJECT()))
    {
        return this->RequestDataObject(request, inputVector, outputVector);
    }
    // generate the data
    if(request->Has(vtkDemandDrivenPipeline::REQUEST_DATA()))
    {
        return this->RequestData(request, inputVector, outputVector);
    }

    if(request->Has(vtkStreamingDemandDrivenPipeline::REQUEST_UPDATE_EXTENT()))
    {
        return this->RequestUpdateExtent(request, inputVector, outputVector);
    }

    // execute information
    if(request->Has(vtkDemandDrivenPipeline::REQUEST_INFORMATION()))
    {
        return this->RequestInformation(request, inputVector, outputVector);
    }

    return this->Superclass::ProcessRequest(request, inputVector, outputVector);
}

int vtkTensorLines::FillOutputPortInformation(int port,
                                              vtkInformation* info)
{
    // now add our info
    if(port == 0)
    {
        info->Set(vtkDataObject::DATA_TYPE_NAME(), "vtkPolyData");
    }
    // else if(port == 1)
    // {
    //     info->Set(vtkDataObject::DATA_TYPE_NAME(), "vtkPolyData");
    // }
    return 1;
}

int vtkTensorLines::FillInputPortInformation(int vtkNotUsed(port),
                                             vtkInformation* info)
{
    info->Set(vtkAlgorithm::INPUT_REQUIRED_DATA_TYPE(), "vtkUnstructuredGrid");
    return 1;
}

int vtkTensorLines::RequestDataObject(
        vtkInformation* vtkNotUsed(request),
        vtkInformationVector** vtkNotUsed(inputVector),
        vtkInformationVector* outputVector)
{
    // RequestDataObject (RDO) is an earlier pipeline pass. During RDO, each
    // filter is supposed to produce an empty data object of the proper type

    auto* outInfo = outputVector->GetInformationObject(0);
    auto* output = vtkPolyData::SafeDownCast(
            outInfo->Get(vtkDataObject::DATA_OBJECT()));
    // auto* outInfo2 = outputVector->GetInformationObject(1);
    // auto* output2 = vtkPolyData::SafeDownCast(
    //         outInfo2->Get(vtkDataObject::DATA_OBJECT()));

    if(!output)
    {
        output = vtkPolyData::New();
        outInfo->Set(vtkDataObject::DATA_OBJECT(), output);
        output->FastDelete();

        this->GetOutputPortInformation(0)->Set(
                vtkDataObject::DATA_EXTENT_TYPE(), output->GetExtentType());
    }
    // if(!output2)
    // {
    //     output2 = vtkPolyData::New();
    //     outInfo2->Set(vtkDataObject::DATA_OBJECT(), output2);
    //     output2->FastDelete();

    //     this->GetOutputPortInformation(1)->Set(
    //             vtkDataObject::DATA_EXTENT_TYPE(), output2->GetExtentType());
    // }

    return 1;
}

int vtkTensorLines::RequestInformation(
        vtkInformation* vtkNotUsed(request),
        vtkInformationVector** vtkNotUsed(inputVector),
        vtkInformationVector* vtkNotUsed(outputVector))
{
    return 1;
}

int vtkTensorLines::RequestUpdateExtent(
        vtkInformation* vtkNotUsed(request),
        vtkInformationVector** inputVector,
        vtkInformationVector* vtkNotUsed(outputVector))
{
    auto numInputPorts = this->GetNumberOfInputPorts();
    for(auto i = 0; i < numInputPorts; i++)
    {
        auto numInputConnections = this->GetNumberOfInputConnections(i);
        for(auto j = 0; j < numInputConnections; j++)
        {
            auto* inputInfo = inputVector[i]->GetInformationObject(j);
            inputInfo->Set(vtkStreamingDemandDrivenPipeline::EXACT_EXTENT(), 1);
        }
    }
    return 1;
}

int vtkTensorLines::RequestData(vtkInformation* vtkNotUsed(request),
                                         vtkInformationVector** inputVector,
                                         vtkInformationVector* outputVector)
{
    using namespace std::chrono;
    using minutes = duration<double, std::chrono::minutes::period>;
    using seconds = duration<double, std::chrono::seconds::period>;
    using milliseconds = duration<double, std::chrono::milliseconds::period>;

    auto* outInfo = outputVector->GetInformationObject(0);
    auto* output = vtkPolyData::SafeDownCast(
            outInfo->Get(vtkDataObject::DATA_OBJECT()));

    // auto* outInfo2 = outputVector->GetInformationObject(1);
    // auto* output2 = vtkPolyData::SafeDownCast(
    //         outInfo2->Get(vtkDataObject::DATA_OBJECT()));

    auto* inInfo = inputVector[0]->GetInformationObject(0);
    auto* input = vtkUnstructuredGrid::SafeDownCast(
            inInfo->Get(vtkDataObject::DATA_OBJECT()));

    // Get the two DataArrays corresponding to the tensor data
    auto* array1 = this->GetInputArrayToProcess(0, inputVector);
    auto* array2 = this->GetInputArrayToProcess(1, inputVector);

    if(!array1 ||
       (_line_type == LineType::ParallelEigenvectors && !array2))
    {
        vtkErrorMacro(<< "Not all input arrays could be found (Maybe you "
                         "specified the wrong name?).");
        return 0;
    }

    // Check if the data arrays have the correct number of components
    if(array1->GetNumberOfComponents() != 9
       || (_line_type == LineType::ParallelEigenvectors
           && array2->GetNumberOfComponents() != 9))
    {
        vtkErrorMacro(<< "All input arrays must be tensors with 9 components.");
        return 0;
    }

    if(this->GetInputArrayInformation(0)->Get(
               vtkDataObject::FIELD_ASSOCIATION())
               != vtkDataObject::FIELD_ASSOCIATION_POINTS
       || (_line_type == LineType::ParallelEigenvectors
           && this->GetInputArrayInformation(1)->Get(
                      vtkDataObject::FIELD_ASSOCIATION())
                      != vtkDataObject::FIELD_ASSOCIATION_POINTS))
    {
        vtkErrorMacro(<< "All input arrays must be point data.");
        return 0;
    }

    // Point and CellArrays for output dataset
    output->SetPoints(vtkPoints::New());
    output->SetVerts(vtkCellArray::New());
    output->SetPolys(vtkCellArray::New());

    // Output arrays for point information
    auto eig_rank1 = vtkSmartPointer<vtkDoubleArray>::New();
    eig_rank1->SetName("Rank1");
    output->GetPointData()->AddArray(eig_rank1);
    auto eig_rank2 = vtkSmartPointer<vtkDoubleArray>::New();
    eig_rank2->SetName("Rank2");
    output->GetPointData()->AddArray(eig_rank2);
    auto eival1 = vtkSmartPointer<vtkDoubleArray>::New();
    eival1->SetName("Eigenvalue 1");
    output->GetPointData()->AddArray(eival1);
    auto eival2 = vtkSmartPointer<vtkDoubleArray>::New();
    eival2->SetName("Eigenvalue 2");
    output->GetPointData()->AddArray(eival2);
    auto eivec = vtkSmartPointer<vtkDoubleArray>::New();
    eivec->SetName("Eigenvector");
    eivec->SetNumberOfComponents(3);
    output->GetPointData()->SetVectors(eivec);
    auto imag1 = vtkSmartPointer<vtkDoubleArray>::New();
    imag1->SetName("Imaginary 1");
    output->GetPointData()->AddArray(imag1);
    auto imag2 = vtkSmartPointer<vtkDoubleArray>::New();
    imag2->SetName("Imaginary 2");
    output->GetPointData()->AddArray(imag2);
    auto csize = vtkSmartPointer<vtkUnsignedLongLongArray>::New();
    csize->SetName("Cluster Size");
    output->GetPointData()->AddArray(csize);
    auto pos_unc = vtkSmartPointer<vtkDoubleArray>::New();
    pos_unc->SetName("Position Uncertainty");
    output->GetPointData()->AddArray(pos_unc);
    auto dir_unc = vtkSmartPointer<vtkDoubleArray>::New();
    dir_unc->SetName("Direction Uncertainty");
    output->GetPointData()->AddArray(dir_unc);
    auto stability = vtkSmartPointer<vtkDoubleArray>::New();
    stability->SetName("Line Stability");
    output->GetPointData()->AddArray(stability);

    // // List of faces that might have non-line structures in separate output
    // output2->SetPoints(vtkPoints::New());
    // output2->GetPoints()->DeepCopy(input->GetPoints());
    // output2->Allocate();

    // auto direction = vtkSmartPointer<vtkDoubleArray>::New();
    // direction->SetNumberOfComponents(3);
    // direction->SetName("Direction");
    // output2->GetCellData()->AddArray(direction);

    // Copy faces to array for parallel looping
    auto faces = buildFaceList(input);
    auto start = high_resolution_clock::now();

    auto opts = tl::TLOptions{this->GetTolerance(),
                                this->GetClusterEpsilon(),
                                this->GetMaxCandidates()};

    auto fresults = std::vector<tl::TLResult>{};

    if(_line_type == LineType::TensorCoreLines)
    {
        auto derivs = computeCellDerivatives(input, array1->GetName());
        fresults = computeTCLPoints(faces,
                                   input->GetPoints(),
                                   array1,
                                   derivs[0],
                                   derivs[1],
                                   derivs[2],
                                   this,
                                   opts);
    }
    else if(_line_type == LineType::ParallelEigenvectors)
    {
        fresults = computePEVPoints(
                faces, input->GetPoints(), array1, array2, this, opts);
    }
    else if(_line_type == LineType::TensorTopology)
    {
        fresults = computeTopoPoints(
                faces, input->GetPoints(), array1, this, opts);
    }

    auto end_pointsearch = high_resolution_clock::now();

    // map cell IDs to parallel eigenvector points found on their faces
    auto cell_map = std::map<vtkIdType, vtkSmartPointer<vtkIdList>>{};

    for(auto i : range(faces.size()))
    {
        auto pev_points = fresults[i];
        auto cid = faces[i].cellId;
        for(const auto& p : pev_points.points)
        {
            auto pid = output->GetPoints()->InsertNextPoint(p.pos.data());
            eig_rank1->InsertValue(pid, double(p.s_rank));
            eig_rank2->InsertValue(pid, double(p.t_rank));
            eival1->InsertValue(pid, p.s_eival);
            eival2->InsertValue(pid, p.t_eival);
            eivec->InsertTuple(pid, p.eivec.data());
            imag1->InsertValue(pid, p.s_has_imaginary ? 1. : 0.);
            imag2->InsertValue(pid, p.t_has_imaginary ? 1. : 0.);
            csize->InsertValue(pid, p.cluster_size);
            pos_unc->InsertValue(pid, p.pos_uncertainty);
            dir_unc->InsertValue(pid, p.dir_uncertainty);
            stability->InsertValue(pid, p.line_stability);

            if(!cell_map[cid].Get())
            {
                cell_map[cid] = vtkSmartPointer<vtkIdList>::New();
            }
            cell_map[cid]->InsertNextId(pid);
        }
    }

    output->SetLines(vtkCellArray::New());

    auto cell_points = vtkSmartPointer<vtkIdList>::New();

    for(const auto& c : cell_map)
    {
        auto point_list = c.second;
        auto npoints = point_list->GetNumberOfIds();
        // For cells with exactly two parallel eigenvector points, connect them
        // with a line
        if(npoints == 2)
        {
            output->InsertNextCell(VTK_LINE, point_list);
        }
        else
        {
            // Match points by eigenvector direction

            using Matrix3X = Eigen::Matrix3Xd;
            using MatrixX = Eigen::MatrixXd;

            // Get eigenvectors of cell points
            auto eigdirs = Matrix3X(3, npoints).eval();
            for(auto i : range(npoints))
            {
                eigdirs.col(i) = Vec3dm{eivec->GetTuple(point_list->GetId(i))};
            }

            // Compute pairwise vector deviations
            auto dist = MatrixX::Ones(npoints, npoints).eval();
            for(auto i : range(npoints))
            {
                for(auto j : range(i + 1, npoints))
                {
                    dist(i, j) =
                            eigdirs.col(i).cross(eigdirs.col(j)).squaredNorm();
                }
            }

            // Greedily find closest two vectors and connect
            auto unlinked = std::unordered_set<vtkIdType>{};
            for(auto i : range(npoints))
            {
                unlinked.insert(i);
            }
            if(npoints < 10)
            {
                while(dist.sum() < double(npoints * npoints))
                {
                    auto row = Matrix3X::Index{};
                    auto col = Matrix3X::Index{};
                    dist.minCoeff(&row, &col);
                    auto line = vtkSmartPointer<vtkIdList>::New();
                    line->SetNumberOfIds(2);
                    line->InsertId(0, point_list->GetId(row));
                    line->InsertId(1, point_list->GetId(col));
                    output->InsertNextCell(VTK_LINE, line);
                    dist.col(col).setOnes();
                    dist.col(row).setOnes();
                    dist.row(col).setOnes();
                    dist.row(row).setOnes();
                    unlinked.erase(row);
                    unlinked.erase(col);
                }
            }

            // Add vertex for last unlinked point if any
            for(auto i : unlinked)
            {
                output->InsertNextCell(
                        VTK_VERTEX, 1, point_list->GetPointer(i));
            }
        }
    }

    // for(auto i : range(faces.size()))
    // {
    //     auto failed_dirs = fresults[i].non_line_dirs;
    //     auto cellpts = vtkSmartPointer<vtkIdList>::New();
    //     cellpts->SetNumberOfIds(3);
    //     cellpts->InsertId(0, faces[i].points[0]);
    //     cellpts->InsertId(1, faces[i].points[1]);
    //     cellpts->InsertId(2, faces[i].points[2]);
    //     for(const auto& f: failed_dirs)
    //     {
    //         auto cid = output2->InsertNextCell(VTK_TRIANGLE, cellpts);
    //         direction->InsertTuple(cid, f.data());
    //     }
    // }

    auto end_all = high_resolution_clock::now();
    auto duration_all = seconds(end_all - start);
    auto duration_pointsearch = seconds(end_pointsearch - start);

    std::cout << "Processed dataset in " << minutes(duration_all).count()
              << " minutes" << std::endl;
    std::cout << "Point search time: " << minutes(duration_pointsearch).count()
              << " minutes" << std::endl;
    std::cout << "Postprocessing time: "
              << seconds(duration_all - duration_pointsearch).count()
              << " seconds" << std::endl;
    std::cout << "Number of faces processed: " << faces.size() << std::endl;
    std::cout << "Average time per face: "
              << (milliseconds(duration_pointsearch) / faces.size()).count()
              << " milliseconds" << std::endl;

    this->UpdateProgress(1.);

    return 1;
}


// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// Purpose:  Implementation of the krovak (Krovak) projection.
//           Definition: http://www.ihsenergy.com/epsg/guid7.html#1.4.3
// Author:   Thomas Flemming, tf@ttqv.com
// Copyright (c) 2001, Thomas Flemming, tf@ttqv.com

// 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.

#ifndef BOOST_GEOMETRY_PROJECTIONS_KROVAK_HPP
#define BOOST_GEOMETRY_PROJECTIONS_KROVAK_HPP

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct krovak {}; // Krovak

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace krovak
    {
            static double epsilon = 1e-15;
            static double S45 = 0.785398163397448;  /* 45 deg */
            static double S90 = 1.570796326794896;  /* 90 deg */
            static double UQ  = 1.04216856380474;   /* DU(2, 59, 42, 42.69689) */
            static double S0  = 1.37008346281555;   /* Latitude of pseudo standard parallel 78deg 30'00" N */
            /* Not sure at all of the appropriate number for max_iter... */
            static int max_iter = 100;

            template <typename T>
            struct par_krovak
            {
                T alpha;
                T k;
                T n;
                T rho0;
                T ad;
                int czech;
            };

            /**
               NOTES: According to EPSG the full Krovak projection method should have
                      the following parameters.  Within PROJ.4 the azimuth, and pseudo
                      standard parallel are hardcoded in the algorithm and can't be
                      altered from outside.  The others all have defaults to match the
                      common usage with Krovak projection.

              lat_0 = latitude of centre of the projection

              lon_0 = longitude of centre of the projection

              ** = azimuth (true) of the centre line passing through the centre of the projection

              ** = latitude of pseudo standard parallel

              k  = scale factor on the pseudo standard parallel

              x_0 = False Easting of the centre of the projection at the apex of the cone

              y_0 = False Northing of the centre of the projection at the apex of the cone

             **/

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_krovak_ellipsoid
                : public base_t_fi<base_krovak_ellipsoid<T, Parameters>, T, Parameters>
            {
                par_krovak<T> m_proj_parm;

                inline base_krovak_ellipsoid(const Parameters& par)
                    : base_t_fi<base_krovak_ellipsoid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(e_forward)  ellipsoid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y) const
                {
                    T gfi, u, deltav, s, d, eps, rho;

                    gfi = math::pow( (T(1) + this->m_par.e * sin(lp_lat)) / (T(1) - this->m_par.e * sin(lp_lat)), this->m_proj_parm.alpha * this->m_par.e / T(2));

                    u = 2. * (atan(this->m_proj_parm.k * math::pow( tan(lp_lat / T(2) + S45), this->m_proj_parm.alpha) / gfi)-S45);
                    deltav = -lp_lon * this->m_proj_parm.alpha;

                    s = asin(cos(this->m_proj_parm.ad) * sin(u) + sin(this->m_proj_parm.ad) * cos(u) * cos(deltav));
                    d = asin(cos(u) * sin(deltav) / cos(s));

                    eps = this->m_proj_parm.n * d;
                    rho = this->m_proj_parm.rho0 * math::pow(tan(S0 / T(2) + S45) , this->m_proj_parm.n) / math::pow(tan(s / T(2) + S45) , this->m_proj_parm.n);

                    xy_y = rho * cos(eps);
                    xy_x = rho * sin(eps);

                    xy_y *= this->m_proj_parm.czech;
                    xy_x *= this->m_proj_parm.czech;
                }

                // INVERSE(e_inverse)  ellipsoid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat) const
                {
                    T u, deltav, s, d, eps, rho, fi1, xy0;
                    int i;

                    // TODO: replace with std::swap()
                    xy0 = xy_x;
                    xy_x = xy_y;
                    xy_y = xy0;

                    xy_x *= this->m_proj_parm.czech;
                    xy_y *= this->m_proj_parm.czech;

                    rho = sqrt(xy_x * xy_x + xy_y * xy_y);
                    eps = atan2(xy_y, xy_x);

                    d = eps / sin(S0);
                    s = T(2) * (atan(math::pow(this->m_proj_parm.rho0 / rho, T(1) / this->m_proj_parm.n) * tan(S0 / T(2) + S45)) - S45);

                    u = asin(cos(this->m_proj_parm.ad) * sin(s) - sin(this->m_proj_parm.ad) * cos(s) * cos(d));
                    deltav = asin(cos(s) * sin(d) / cos(u));

                    lp_lon = this->m_par.lam0 - deltav / this->m_proj_parm.alpha;

                    /* ITERATION FOR lp_lat */
                    fi1 = u;

                    for (i = max_iter; i ; --i) {
                        lp_lat = T(2) * ( atan( math::pow( this->m_proj_parm.k, T(-1) / this->m_proj_parm.alpha)  *
                                              math::pow( tan(u / T(2) + S45) , T(1) / this->m_proj_parm.alpha)  *
                                              math::pow( (T(1) + this->m_par.e * sin(fi1)) / (T(1) - this->m_par.e * sin(fi1)) , this->m_par.e / T(2))
                                            )  - S45);

                        if (fabs(fi1 - lp_lat) < epsilon)
                            break;
                        fi1 = lp_lat;
                    }
                    if( i == 0 )
                        BOOST_THROW_EXCEPTION( projection_exception(error_non_convergent) );

                   lp_lon -= this->m_par.lam0;
                }

                static inline std::string get_name()
                {
                    return "krovak_ellipsoid";
                }

            };

            // Krovak
            template <typename Parameters, typename T>
            inline void setup_krovak(Parameters& par, par_krovak<T>& proj_parm)
            {
                T u0, n0, g;

                /* we want Bessel as fixed ellipsoid */
                par.a = 6377397.155;
                par.e = sqrt(par.es = 0.006674372230614);

                /* if latitude of projection center is not set, use 49d30'N */
                if (!pj_param_exists(par.params, "lat_0"))
                    par.phi0 = 0.863937979737193;

                /* if center long is not set use 42d30'E of Ferro - 17d40' for Ferro */
                /* that will correspond to using longitudes relative to greenwich    */
                /* as input and output, instead of lat/long relative to Ferro */
                if (!pj_param_exists(par.params, "lon_0"))
                    par.lam0 = 0.7417649320975901 - 0.308341501185665;

                /* if scale not set default to 0.9999 */
                if (!pj_param_exists(par.params, "k"))
                    par.k0 = 0.9999;

                proj_parm.czech = 1;
                if( !pj_param_exists(par.params, "czech") )
                    proj_parm.czech = -1;

                /* Set up shared parameters between forward and inverse */
                proj_parm.alpha = sqrt(T(1) + (par.es * math::pow(cos(par.phi0), 4)) / (T(1) - par.es));
                u0 = asin(sin(par.phi0) / proj_parm.alpha);
                g = math::pow( (T(1) + par.e * sin(par.phi0)) / (T(1) - par.e * sin(par.phi0)) , proj_parm.alpha * par.e / T(2) );
                proj_parm.k = tan( u0 / 2. + S45) / math::pow(tan(par.phi0 / T(2) + S45) , proj_parm.alpha) * g;
                n0 = sqrt(T(1) - par.es) / (T(1) - par.es * math::pow(sin(par.phi0), 2));
                proj_parm.n = sin(S0);
                proj_parm.rho0 = par.k0 * n0 / tan(S0);
                proj_parm.ad = S90 - UQ;
            }

    }} // namespace detail::krovak
    #endif // doxygen

    /*!
        \brief Krovak projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Pseudocylindrical
         - Ellipsoid
        \par Projection parameters
         - lat_ts: Latitude of true scale (degrees)
         - lat_0: Latitude of origin
         - lon_0: Central meridian
         - k: Scale factor on the pseudo standard parallel
        \par Example
        \image html ex_krovak.gif
    */
    template <typename T, typename Parameters>
    struct krovak_ellipsoid : public detail::krovak::base_krovak_ellipsoid<T, Parameters>
    {
        inline krovak_ellipsoid(const Parameters& par) : detail::krovak::base_krovak_ellipsoid<T, Parameters>(par)
        {
            detail::krovak::setup_krovak(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::krovak, krovak_ellipsoid, krovak_ellipsoid)

        // Factory entry(s)
        template <typename T, typename Parameters>
        class krovak_entry : public detail::factory_entry<T, Parameters>
        {
            public :
                virtual base_v<T, Parameters>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<krovak_ellipsoid<T, Parameters>, T, Parameters>(par);
                }
        };

        template <typename T, typename Parameters>
        inline void krovak_init(detail::base_factory<T, Parameters>& factory)
        {
            factory.add_to_factory("krovak", new krovak_entry<T, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_KROVAK_HPP


#include <iostream>
#include "source/tessellation/geometry.hpp"
#include "source/tessellation/Delaunay.hpp"
#include "source/misc/simple_io.hpp"
#include "source/newtonian/two_dimensional/geometric_outer_boundaries/SquareBox.hpp"
#include "source/misc/mesh_generator.hpp"
#ifdef RICH_MPI
#include <boost/mpi/environment.hpp>
#include <boost/mpi/communicator.hpp>
#endif // RICH_MPI
#include "source/newtonian/two_dimensional/hdf5_diagnostics.hpp"
#include <boost/foreach.hpp>
#include "source/tessellation/VoronoiMesh.hpp"

using namespace std;

#ifdef RICH_MPI
vector<Vector2D> delineate_rectangle
(const pair<Vector2D,Vector2D>& boundary)
{
  vector<Vector2D> res(4);
  res.at(0) = boundary.first;
  res.at(1) = Vector2D(boundary.second.x,boundary.first.y);
  res.at(2) = boundary.second;
  res.at(3) = Vector2D(boundary.first.x,boundary.second.y);
  return res;
}

vector<Vector2D> my_convex_hull
(const Tessellation& tess,
 int index)
{
  vector<Vector2D> res;
  ConvexHull(res,tess,index);
  return res;
}

vector<Vector2D> distribute_grid
(const vector<Vector2D>& complete_grid,
 const Tessellation& proc_tess)
{
  const boost::mpi::communicator world;
  vector<Vector2D> res;
  const vector<Vector2D> ch_list =
    my_convex_hull(proc_tess,world.rank());
  BOOST_FOREACH(const Vector2D& v, complete_grid)
    {
      if(PointInCell(ch_list,v))
	res.push_back(v);
    }
  return res;
}
#endif // RICH_MPI

int main(void)
{
#ifdef RICH_MPI

  boost::mpi::environment env;
  boost::mpi::communicator world;

  try{

  const SquareBox obc
    (Vector2D(0,0),
     Vector2D(1,1));
  const vector<Vector2D> all_points =
    RandSquare(1000,
	       obc.getBoundary().first.x,
	       obc.getBoundary().second.x,
	       obc.getBoundary().first.y,
	       obc.getBoundary().second.y);
  if(world.rank()==0){
    Delaunay tri;
    tri.build_delaunay
      (all_points,
       delineate_rectangle
       (obc.getBoundary()));
    tri.BuildBoundary(&obc, obc.GetBoxEdges());
    WriteDelaunay(tri,string("whole.h5"));
  }
  VoronoiMesh super
    (RandSquare(world.size(),
		obc.getBoundary().first.x,
		obc.getBoundary().second.x,
		obc.getBoundary().first.y,
		obc.getBoundary().second.y),
     obc);
  const vector<Vector2D> local_points =
    distribute_grid
    (all_points,
     super);
  Delaunay tri;
  tri.build_delaunay
    (local_points,
     delineate_rectangle
     (obc.getBoundary()));
  vector<vector<int> > dummy_1;
  //vector<int> dummy_2;
  tri.BuildBoundary
    (&obc,
     super,
     dummy_1);
  WriteDelaunay(tri,string("part_"+int2str(world.rank())+".h5"));
  }
  catch(const UniversalError& eo){
    cout << eo.GetErrorMessage() << endl;
    throw;
  }
#else

  write_number(0,"serial_ignore.txt");

#endif // RICH_MPI
  return 0;
}


/**
 *  Copyright (C) 2012
 *    Ekaterina Potapova
 *    Automation and Control Institute
 *    Vienna University of Technology
 *    Gusshausstraße 25-29
 *    1040 Vienna, Austria
 *    potapova(at)acin.tuwien.ac.at
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see http://www.gnu.org/licenses/
 */

#include "v4r/attention_segmentation/algo.h"

#include <Eigen/Dense>

namespace v4r
{

void filterGaussian(cv::Mat &input, cv::Mat &output, cv::Mat &mask)
{
  float kernel[5] = {1.0,4.0,6.0,4.0,1.0};

  cv::Mat temp = cv::Mat_<float>::zeros(input.rows, input.cols);

  for(int i = 0; i < input.rows; ++i)
  {
    for(int j = 0; j < input.cols; j = j+2)
    {
      if(mask.at<float>(i,j) > 0)
      {

	float value = 0;
        float kernel_sum = 0;

        for(int k = 0; k < 5; ++k)
        {
	  int jj = j + (k-2);
	  if( (jj >=0) && (jj < input.cols) )
	  {
	    if(mask.at<float>(i,jj) > 0)
	    {
	      value += kernel[k] * input.at<float>(i,jj);
	      kernel_sum += kernel[k];
	    }
	  }
        }

        if(kernel_sum > 0)
        {
	  temp.at<float>(i,j) = value / kernel_sum;
        }
      }
    }
  }

  cv::Mat temp2 = cv::Mat_<float>::zeros(temp.rows, temp.cols);

  for(int i = 0; i < temp.rows; i = i+2)
  {
    for(int j = 0; j < temp.cols; j = j+2)
    {

      if(mask.at<float>(i,j) > 0)
      {

	float value = 0;
        float kernel_sum = 0;

        for(int k = 0; k < 5; ++k)
        {
	  int ii = i + (k-2);
	  if( (ii >=0) && (ii < temp.rows) )
	  {
	    if(mask.at<float>(ii,j) > 0)
	    {
	      value += kernel[k] * temp.at<float>(ii,j);
	      kernel_sum += kernel[k];
	    }
	  }
        }

        if(kernel_sum > 0)
        {
	  temp2.at<float>(i,j) = value / kernel_sum;
        }
      }
    }
  }

  int new_width = input.cols / 2;
  int new_height = input.rows / 2;

  cv::Mat mask_output = cv::Mat_<float>::zeros(new_height,new_width);
  output = cv::Mat_<float>::zeros(new_height,new_width);

  for(int i = 0; i < new_height; ++i)
  {
    for(int j = 0; j < new_width; ++j)
    {
      if(mask.at<float>(2*i,2*j) > 0)
      {
	mask_output.at<float>(i,j) = 1.0;
	output.at<float>(i,j) = temp2.at<float>(2*i,2*j);
      }
    }
  }

  mask_output.copyTo(mask);
}

void buildDepthPyramid(cv::Mat &image, std::vector<cv::Mat> &pyramid, cv::Mat &mask, unsigned int levelNumber)
{
  pyramid.resize(levelNumber+1);
  image.copyTo(pyramid.at(0));

  cv::Mat oldMask;
  mask.copyTo(oldMask);

  for(unsigned int i = 1; i <= levelNumber; ++i)
  {
    cv::Mat tempImage;

    filterGaussian(pyramid.at(i-1),tempImage,oldMask);
    tempImage.copyTo(pyramid.at(i));

  }
}

void createPointCloudPyramid(std::vector<cv::Mat> &pyramidX, std::vector<cv::Mat> &pyramidY, std::vector<cv::Mat> &pyramidZ,
			     std::vector<cv::Mat> &pyramidIndices, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr > &pyramidCloud)
{
  assert( pyramidX.size() == pyramidY.size() );
  assert( pyramidX.size() == pyramidZ.size() );
  assert( pyramidX.size() == pyramidIndices.size() );

  unsigned int num_levels = pyramidX.size();

  pyramidCloud.resize(num_levels);

  for(unsigned int idx = 0; idx < num_levels; ++idx)
  {
    assert( pyramidX.at(idx).rows == pyramidY.at(idx).rows );
    assert( pyramidX.at(idx).cols == pyramidY.at(idx).cols );
    assert( pyramidX.at(idx).rows == pyramidZ.at(idx).rows );
    assert( pyramidX.at(idx).cols == pyramidZ.at(idx).cols );
    assert( pyramidX.at(idx).rows == pyramidIndices.at(idx).rows );
    assert( pyramidX.at(idx).cols == pyramidIndices.at(idx).cols );

    pyramidCloud.at(idx) = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>() );

    unsigned int cur_height = pyramidX.at(idx).rows;
    unsigned int cur_width = pyramidX.at(idx).cols;

    pyramidCloud.at(idx)->points.resize(cur_height*cur_width);
    pyramidCloud.at(idx)->width = cur_width;
    pyramidCloud.at(idx)->height = cur_height;

//     cv::imshow("pyramidZ.at(idx)",pyramidZ.at(idx));
//     cv::waitKey(-1);

    for(unsigned int i = 0 ; i < cur_height; ++i)
    {
      for(unsigned int j = 0; j < cur_width; ++j)
      {
	pcl::PointXYZRGB p_cur;

	//std::cerr << "(" << pyramidIndices.at(idx).at<float>(i,j) << "-- " << pyramidX.at(idx).at<float>(i,j) << "," << pyramidY.at(idx).at<float>(i,j) << "," << pyramidZ.at(idx).at<float>(i,j) << ") ";

        //if(pyramidIndices.at(idx).at<float>(i,j) > 0)
	//{
	  p_cur.x = pyramidX.at(idx).at<float>(i,j);
	  p_cur.y = pyramidY.at(idx).at<float>(i,j);
	  p_cur.z = pyramidZ.at(idx).at<float>(i,j);
	//}
	//else
// 	{
// 	  p_cur.x = std::numeric_limits<float>::quiet_NaN();
// 	  p_cur.y = std::numeric_limits<float>::quiet_NaN();
// 	  p_cur.z = std::numeric_limits<float>::quiet_NaN();
// 	}

	int p_idx = i*cur_width + j;
	pyramidCloud.at(idx)->points.at(p_idx) = p_cur;
      }
    }

    //std::cerr <<  std::endl << std::endl;

//   cv::imshow("output",output);
//   cv::waitKey(-1);

  }
}

void createNormalPyramid(std::vector<cv::Mat> &pyramidNx, std::vector<cv::Mat> &pyramidNy, std::vector<cv::Mat> &pyramidNz, std::vector<cv::Mat> &pyramidIndices, std::vector<pcl::PointCloud<pcl::Normal>::Ptr > &pyramidNormal)
{
  assert( pyramidNx.size() == pyramidNy.size() );
  assert( pyramidNx.size() == pyramidNz.size() );
  assert( pyramidIndices.size() == pyramidNz.size() );

  unsigned int num_levels = pyramidNx.size();

  pyramidNormal.resize(num_levels);

  for(unsigned int idx = 0; idx < num_levels; ++idx)
  {
    assert( pyramidNx.at(idx).rows == pyramidNy.at(idx).rows );
    assert( pyramidNx.at(idx).cols == pyramidNy.at(idx).cols );
    assert( pyramidNx.at(idx).rows == pyramidNz.at(idx).rows );
    assert( pyramidNx.at(idx).cols == pyramidNz.at(idx).cols );
    assert( pyramidNx.at(idx).rows == pyramidIndices.at(idx).rows );
    assert( pyramidNx.at(idx).cols == pyramidIndices.at(idx).cols );

    pyramidNormal.at(idx) = pcl::PointCloud<pcl::Normal>::Ptr(new pcl::PointCloud<pcl::Normal>() );

    unsigned int cur_height = pyramidNx.at(idx).rows;
    unsigned int cur_width = pyramidNx.at(idx).cols;

    pyramidNormal.at(idx)->points.clear();

    for(unsigned int i = 0 ; i < cur_height; ++i)
    {
      for(unsigned int j = 0; j < cur_width; ++j)
      {
        if(pyramidIndices.at(idx).at<float>(i,j) > 0)
	{
	  pcl::Normal p_cur;
	  p_cur.normal[0] = pyramidNx.at(idx).at<float>(i,j);
	  p_cur.normal[1] = pyramidNy.at(idx).at<float>(i,j);
	  p_cur.normal[2] = pyramidNz.at(idx).at<float>(i,j);

	  pyramidNormal.at(idx)->points.push_back(p_cur);
	}
      }
    }

    pyramidNormal.at(idx)->width = pyramidNormal.at(idx)->points.size();
    pyramidNormal.at(idx)->height = 1;

  }
}

void createIndicesPyramid(std::vector<cv::Mat> &pyramidIndices, std::vector<pcl::PointIndices::Ptr> &pyramidIndiceSets)
{
  unsigned int num_levels = pyramidIndices.size();

  pyramidIndiceSets.resize(num_levels);

  for(unsigned int idx = 0; idx < num_levels; ++idx)
  {
    pyramidIndiceSets.at(idx) = pcl::PointIndices::Ptr(new pcl::PointIndices() );

    unsigned int cur_height = pyramidIndices.at(idx).rows;
    unsigned int cur_width = pyramidIndices.at(idx).cols;

    pyramidIndiceSets.at(idx)->indices.clear();

    for(unsigned int i = 0 ; i < cur_height; ++i)
    {
      for(unsigned int j = 0; j < cur_width; ++j)
      {
        if(pyramidIndices.at(idx).at<float>(i,j) > 0)
	{
	  int p_idx = i*cur_width + j;
	  pyramidIndiceSets.at(idx)->indices.push_back(p_idx);
	}
      }
    }

  }
}

void upscaleImage(cv::Mat &input, cv::Mat &output, unsigned int width, unsigned int height)
{
  assert( width >= (unsigned int)(2*input.cols) );
  assert( height >= (unsigned int)(2*input.rows) );

  output = cv::Mat_<float>::zeros(height, width);

  for(unsigned int i = 0; i < (unsigned int)input.rows; ++i)
  {
    for(unsigned int j = 0; j < (unsigned int)input.cols; ++j)
    {
      output.at<float>(2*i,2*j) = input.at<float>(i,j);
    }
  }

  //cv::imshow("output",output);
  //cv::waitKey(-1);

  for(unsigned int i = 0; i < (unsigned int)output.rows; i=i+2)
  {
    for(unsigned int j = 1; j < (unsigned int)output.cols-1; j=j+2)
    {
      output.at<float>(i,j) = ( output.at<float>(i,j-1) + output.at<float>(i,j+1) ) / 2.0;
    }
  }
  for(unsigned int i = 1; i < (unsigned int)output.rows-1; i=i+2)
  {
    for(unsigned int j = 0; j < (unsigned int)output.cols; j=j+2)
    {
      output.at<float>(i,j) = ( output.at<float>(i-1,j) + output.at<float>(i+1,j) ) / 2.0;
    }
  }
  for(unsigned int i = 1; i < (unsigned int)output.rows-1; i=i+2)
  {
    for(unsigned int j = 1; j < (unsigned int)output.cols-1; j=j+2)
    {
      output.at<float>(i,j) = ( output.at<float>(i-1,j-1) + output.at<float>(i-1,j+1) + output.at<float>(i+1,j-1) + output.at<float>(i+1,j+1) ) / 4.0;
    }
  }
}

void downscaleImage(cv::Mat &input, cv::Mat &output, unsigned int width, unsigned int height)
{
  assert( 2*width <= (unsigned int)(input.cols) );
  assert( 2*height <= (unsigned int)(input.rows) );

  output = cv::Mat_<float>::zeros(height, width);

  for(unsigned int i = 0; i < (unsigned int)output.rows; ++i)
  {
    for(unsigned int j = 0; j < (unsigned int)output.cols; ++j)
    {
      output.at<float>(i,j) = input.at<float>(2*i,2*j);
    }
  }
}

void scaleImage(std::vector<cv::Mat> &inputPyramid, cv::Mat &input, cv::Mat &output, int inLevel, int outLevel)
{
  assert(input.cols == inputPyramid.at(inLevel).cols);
  assert(input.rows == inputPyramid.at(inLevel).rows);

  if( inLevel < outLevel )
  {
    input.copyTo(output);
    for(int l = inLevel+1; l <= outLevel; ++l)
    {
      cv::Mat temp;
      downscaleImage(output,temp,inputPyramid.at(l).cols,inputPyramid.at(l).rows);
      temp.copyTo(output);
    }
  }
  else if ( inLevel > outLevel )
  {
    input.copyTo(output);
    for(int l = inLevel-1; l >= outLevel; --l)
    {
      cv::Mat temp;
      upscaleImage(output,temp,inputPyramid.at(l).cols,inputPyramid.at(l).rows);
      temp.copyTo(output);
    }
  }
  else
  {
    input.copyTo(output);
  }
}

bool inPoly(std::vector<cv::Point> &poly, cv::Point p)
{
  cv::Point newPoint;
  cv::Point oldPoint;
  cv::Point p1, p2;

  bool inside = false;

  if (poly.size() < 3)
  {
    return(false);
  }

  oldPoint = poly.at(poly.size()-1);
  for(unsigned int i = 0 ; i < poly.size(); i++)
  {
    newPoint = poly.at(i);
    if(newPoint.y > oldPoint.y)
    {
      p1 = oldPoint;
      p2 = newPoint;
    }
    else
    {
      p1 = newPoint;
      p2 = oldPoint;
    }

    if((newPoint.y < p.y) == (p.y <= oldPoint.y)          /* edge "open" at one end */
      && ((long)p.x-(long)p1.x)*(long)(p2.y-p1.y) < ((long)p2.x-(long)p1.x)*(long)(p.y-p1.y))
    {
      inside = !inside;
    }
    oldPoint = newPoint;
  }
  return(inside);
}

void buildPolygonMap(cv::Mat &polygonMap, std::vector<std::vector<cv::Point> > &polygons)
{
  for(int i = 0; i < polygonMap.rows; ++i)
  {
    for(int j = 0; j < polygonMap.cols; ++j)
    {
      unsigned int k = 0;
      while((polygonMap.at<uchar>(i,j) == 0) && (k < polygons.size()))
      {
        if(inPoly(polygons.at(k),cv::Point(j,i)))
          polygonMap.at<uchar>(i,j) = k+1;
        k += 1;
      }
    }
  }
}

void buildCountourMap(cv::Mat &polygonMap, std::vector<std::vector<cv::Point> > &polygons, cv::Scalar color)
{
  for(unsigned int i = 0; i < polygons.size(); ++i)
  {
    for(unsigned int j = 0; j < polygons.at(i).size(); ++j)
    {
      cv::line(polygonMap,polygons.at(i).at(j),polygons.at(i).at((j+1)%polygons.at(i).size()),color,2);
    }
  }
}

float normPDF(float x, float mean, float stddev)
{
  float val = exp(-(x-mean)*(x-mean)/(2*stddev*stddev));
  val /= sqrt(2*3.14)*stddev;
  return val;
}

float normPDF(std::vector<float> x, std::vector<float> mean, cv::Mat stddev)
{
  int dim = mean.size();

  EIGEN_ALIGN16 Eigen::MatrixXf _stddev = Eigen::MatrixXf::Zero(dim,dim);
  EIGEN_ALIGN16 Eigen::VectorXf _x = Eigen::VectorXf::Zero(dim);

  for(int i = 0; i < dim; ++i)
  {
    _x[i] = x.at(i) - mean.at(i);
    for(int j = 0; j < dim; ++j)
    {
      _stddev(i,j) = stddev.at<float>(i,j);
    }
  }

  //std::cerr << "_x.transpose() = " << _x.transpose() << std::endl;
  //std::cerr << "_stddev = " << _stddev << std::endl;
  //std::cerr << "_x = " << _x << std::endl;

  float value = (_x.transpose())*_stddev*_x;
  value /= -2;
  value = exp(value);

  float det = _stddev.determinant();

  value /= sqrt(pow(2*3.14,dim)*det);

  return(value);

}

void addNoise(cv::Mat &image, cv::Mat &nImage, cv::RNG &rng,float min, float max)
{
  nImage = cv::Mat_<float>::zeros(image.rows, image.cols);
  for(int i = 0; i < image.rows; ++i)
  {
    for(int j = 0; j < image.cols; ++j)
    {
      float val = image.at<float>(i,j) + rng.uniform(min,max);
      nImage.at<float>(i,j) = (val < 0 ? 0 : (val > 1 ? 1 : val));
    }
  }
}

void normPDF(cv::Mat &mat, float mean, float stddev, cv::Mat &res)
{
  res = cv::Mat_<float>::zeros(mat.rows,mat.cols);
  res = mat - mean;
  res = res.mul(res);
  float b = - 1.0f / (2 * stddev * stddev);
  res = res * b;

  cv::exp(res,res);
  float a = 1.0f/(stddev*sqrt(2*M_PI));
  res = res * a;
}

void normalizeDist(std::vector<float> &dist)
{
  float total_sum = 0;
  for(unsigned int i = 0; i < dist.size(); ++i)
  {
    total_sum += dist.at(i);
  }

  for(unsigned int i = 0; i < dist.size(); ++i)
  {
    dist.at(i) /= total_sum;
  }
}

float getMean(std::vector<float> dist, float total_num)
{
  float mean = 0;
  for(unsigned int i = 0; i < dist.size(); ++i)
  {
    mean += (dist.at(i)/total_num)*i;
  }
  return mean;
}

float getStd(std::vector<float> dist, float mean, float total_num)
{
  float stdDev = 0;

  for(unsigned int i = 0; i < dist.size(); ++i)
  {
    stdDev += (i-mean)*(i-mean)*(dist.at(i)/total_num);
  }

  stdDev = sqrt(stdDev);

  return stdDev;
}

long commulativeFunctionArgValue(float x, std::vector<float> &A)
{
  long min = 0;
  long max = A.size()-1;
  while(max!=min+1)
  {
    long mid = (min+max)/2;
    if(x <= A.at(mid))
    {
      max = mid;
    }
    else
    {
      min = mid;
    }
  }

  if(A.at(min) >= x)
    return(min);
  else
    return(max);
}

void createContoursFromMasks(std::vector<cv::Mat> &masks, std::vector<std::vector<cv::Point> > &contours)
{
  // to extract contours
  contours.clear();
  contours.resize(masks.size());

  for(unsigned int i = 0; i < masks.size(); ++i)
  {
    std::vector<std::vector<cv::Point> > temp;
    cv::Mat temp_image;
    masks.at(i).copyTo(temp_image);
    cv::findContours(temp_image,temp,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_NONE);
    if(temp.size())
    {
      long totalPointsNum = 0;
      for(unsigned int j = 0; j < temp.size(); ++j)
      {
	totalPointsNum += temp.at(j).size();
      }
      contours.at(i).resize(totalPointsNum);
      totalPointsNum = 0;
      for(unsigned int j = 0; j < temp.size(); ++j)
      {
	for(unsigned int k = 0; k < temp.at(j).size(); ++k)
	{
	  contours.at(i).at(totalPointsNum) = temp.at(j).at(k);
	  totalPointsNum += 1;
	}
      }
    }
  }
}

uchar Num01Transitions(cv::Mat s, int j, int i)
{

  uchar p2 = s.at<uchar>(j-1,i);
  uchar p3 = s.at<uchar>(j-1,i+1);
  uchar p4 = s.at<uchar>(j,i+1);
  uchar p5 = s.at<uchar>(j+1,i+1);
  uchar p6 = s.at<uchar>(j+1,i);
  uchar p7 = s.at<uchar>(j+1,i-1);
  uchar p8 = s.at<uchar>(j,i-1);
  uchar p9 = s.at<uchar>(j-1,i-1);

  uchar Nt = 0;

  if((p3-p2) == 1)
    Nt++;
  if((p4-p3) == 1)
    Nt++;
  if((p5-p4) == 1)
    Nt++;
  if((p6-p5) == 1)
    Nt++;
  if((p7-p6) == 1)
    Nt++;
  if((p8-p7) == 1)
    Nt++;
  if((p9-p8) == 1)
    Nt++;
  if((p2-p9) == 1)
    Nt++;

  return Nt;
}

void MConnectivity(cv::Mat &s, uchar *element)
{
  for(int i = 1; i < s.rows-1; ++i)
  {
    for(int j = 1; j < s.cols-1; ++j)
    {
      if(s.at<uchar>(i,j) > 0)
      {
	//s.at<uchar>(i,j) = 0;

	bool remove = true;
	for(int p = 0; p < 8; ++p)
        {
          int new_x = j + dx8[p];
          int new_y = i + dy8[p];

          uchar value = s.at<uchar>(new_y,new_x);

	  if(element[p] < 2)
	  {
	    if(value != element[p])
	    {
	      remove = false;
	    }
	  }
        }

	if(remove)
	{
	  s.at<uchar>(i,j) = 0;
	}
      }
    }
  }
}

V4R_EXPORTS void Skeleton(cv::Mat a, cv::Mat &s)
{
  int width  = a.cols;
  int height = a.rows;

  a.copyTo(s);

  cv::Scalar prevsum(0);

  while(true)
  {

    cv::Mat m = cv::Mat_<uchar>::ones(height,width);

    for(int j = 1; j < height-1; ++j)
    {
      for(int i = 1; i < width-1; ++i)
      {
        if (s.at<uchar>(j, i) == 1)
	{
	  uchar p2 = s.at<uchar>(j-1,i);
	  uchar p3 = s.at<uchar>(j-1,i+1);
	  uchar p4 = s.at<uchar>(j,i+1);
	  uchar p5 = s.at<uchar>(j+1,i+1);
	  uchar p6 = s.at<uchar>(j+1,i);
	  uchar p7 = s.at<uchar>(j+1,i-1);
	  uchar p8 = s.at<uchar>(j,i-1);
	  uchar p9 = s.at<uchar>(j-1,i-1);

	  uchar condA = p2+p3+p4+p5+p6+p7+p8+p9;
          uchar condB = Num01Transitions(s,j,i);
          uchar condC = p2 * p4 * p6;
          uchar condD = p4 * p6 * p8;

          if ((condA >= 2) && (condA <= 6) && (condB == 1) && (condC == 0) && (condD == 0))
             m.at<uchar>(j, i) = 0;
	}

      }
    }

    for(int j = 1; j < height-1; ++j)
    {
      for(int i = 1; i < width-1; ++i)
      {
	s.at<uchar>(j,i) = s.at<uchar>(j,i)*m.at<uchar>(j,i);
      }
    }

    m = cv::Mat_<uchar>::ones(height,width);
    for(int j = 1; j < height-1; ++j)
    {
      for(int i = 1; i < width-1; ++i)
      {

        if (s.at<uchar>(j, i) == 1)
	{
          uchar p2 = s.at<uchar>(j-1,i);
	  uchar p3 = s.at<uchar>(j-1,i+1);
	  uchar p4 = s.at<uchar>(j,i+1);
	  uchar p5 = s.at<uchar>(j+1,i+1);
	  uchar p6 = s.at<uchar>(j+1,i);
	  uchar p7 = s.at<uchar>(j+1,i-1);
	  uchar p8 = s.at<uchar>(j,i-1);
	  uchar p9 = s.at<uchar>(j-1,i-1);

	  uchar condA = p2+p3+p4+p5+p6+p7+p8+p9;
          uchar condB = Num01Transitions(s,j,i);
          uchar condC = p2 * p4 * p8;
          uchar condD = p2 * p6 * p8;

          if ((condA >= 2) && (condA <= 6) && (condB == 1) && (condC == 0) && (condD == 0))
            m.at<uchar>(j, i) = 0;
	}
      }
    }

    for(int j = 1; j < height-1; ++j)
    {
      for(int i = 1; i < width-1; ++i)
      {
	s.at<uchar>(j,i) = s.at<uchar>(j,i)*m.at<uchar>(j,i);
      }
    }

    cv::Scalar newsum = cv::sum(s);

    if (newsum(0) == prevsum(0))
      break;

    prevsum = newsum;

  }

  prevsum = cv::sum(s);

  uchar e1[8] = {2,1,2,1,2,0,0,0};
  uchar e2[8] = {2,1,2,0,0,0,2,1};
  uchar e3[8] = {0,0,2,1,2,1,2,0};
  uchar e4[8] = {2,0,0,0,2,1,2,1};

  while(true)
  {
    MConnectivity(s,e1);
    MConnectivity(s,e2);
    MConnectivity(s,e3);
    MConnectivity(s,e4);

    cv::Scalar newsum = cv::sum(s);

    if (newsum(0) == prevsum(0))
      break;

    prevsum = newsum;
  }
}

float calculateDistance(cv::Point center, cv::Point point)
{
  float distance = sqrt((center.x-point.x)*(center.x-point.x) + (center.y-point.y)*(center.y-point.y));
  return(distance);
}

float calculateDistance(cv::Point center, cv::Point point, float sigma)
{
  float distance = (center.x-point.x)*(center.x-point.x) + (center.y-point.y)*(center.y-point.y);
  distance = exp(-distance/(2*sigma*sigma));
  return(distance);
}

void calculateObjectCenter(cv::Mat mask, cv::Point &center)
{
  assert(mask.type() == CV_8UC1);

  float x_cen = 0;
  float y_cen = 0;
  cv::Scalar area = cv::sum(mask);

  for(int i = 0; i < mask.rows; ++i)
  {
    for(int j = 0; j < mask.cols; ++j)
    {
      uchar value = mask.at<uchar>(i,j);
      if(value > 0)
      {
	x_cen = x_cen + ((float)j)/area(0);
        y_cen = y_cen + ((float)i)/area(0);
      }
    }
  }

  if(mask.at<uchar>(y_cen,x_cen) == 0)
  {
    // search for closes 4-neighbour point
    std::list<cv::Point> points;
    points.push_back(cv::Point(x_cen,y_cen));
    cv::Mat used = cv::Mat_<uchar>::zeros(mask.rows,mask.cols);
    used.at<uchar>(y_cen,x_cen) = 1;
    while(points.size())
    {
      cv::Point p = points.front();
      points.pop_front();
      if(mask.at<uchar>(p.y,p.x) > 0)
      {
        x_cen = p.x;
	y_cen = p.y;
        break;
      }

      for(int i = 0; i < 8; ++i)
      {
        int new_x = p.x + dx8[i];
        int new_y = p.y + dy8[i];

	if((new_x < 0) || (new_y < 0) || (new_x >= used.cols) || (new_y >= used.rows))
	  continue;

	if(used.at<uchar>(new_y,new_x) <= 0)
	{
          points.push_back(cv::Point(new_x,new_y));
	  used.at<uchar>(new_y,new_x) = 1;
	}
      }
    }
  }

  center.x = x_cen;
  center.y = y_cen;
}

void calculateObjectCenter(std::vector<cv::Point> contour, cv::Mat mask, cv::Point &center)
{
  float x_cen = 0;
  float y_cen = 0;

  for(unsigned int i = 0; i < contour.size(); ++i)
  {
    x_cen = x_cen + ((float)contour.at(i).x)/contour.size();
    y_cen = y_cen + ((float)contour.at(i).y)/contour.size();
  }

  if(mask.at<uchar>(y_cen,x_cen) == 0)
  {
    // search for closes 4-neighbour point
    std::list<cv::Point> points;
    points.push_back(cv::Point(x_cen,y_cen));
    cv::Mat used = cv::Mat_<uchar>::zeros(mask.rows,mask.cols);
    used.at<uchar>(y_cen,x_cen) = 1;
    while(points.size())
    {
      cv::Point p = points.front();
      points.pop_front();
      if(mask.at<uchar>(p.y,p.x) > 0)
      {
        x_cen = p.x;
	y_cen = p.y;
        break;
      }

      for(int i = 0; i < 8; ++i)
      {
        int new_x = p.x + dx8[i];
        int new_y = p.y + dy8[i];

	if((new_x < 0) || (new_y < 0) || (new_x >= used.cols) || (new_y >= used.rows))
	  continue;

	if(used.at<uchar>(new_y,new_x) <= 0)
	{
          points.push_back(cv::Point(new_x,new_y));
	  used.at<uchar>(new_y,new_x) = 1;
	}
      }
    }
  }

  center.x = x_cen;
  center.y = y_cen;
}

void get2DNeighbors(const cv::Mat &patches, cv::Mat &neighbors, int patchesNumber)
{
  neighbors = cv::Mat_<bool>(patchesNumber,patchesNumber);
  neighbors.setTo(false);

  //@ep TODO: uncomment?
//   int dr[4] = {-1,-1, 0, 1};
//   int dc[4] = { 0,-1,-1,-1};

  int dr[4] = {-1,0,-1};
  int dc[4] = { 0,-1,-1};

  for(int r = 1; r < patches.rows-1; r++)
  {
    for(int c = 1; c < patches.cols-1; c++)
    {
      // if the patch exist
      if(patches.at<int>(r,c) != -1)
      {

	int patchIdx =  patches.at<int>(r,c);

	//@ep: why we did not use 1,-1 shift???
	for(int i = 0; i < 3; ++i) //@ep: TODO 3->4
	{

	  int nr = r + dr[i];
	  int nc = c + dc[i];

	  int currentPatchIdx =  patches.at<int>(nr,nc);
	  if(currentPatchIdx == -1)
	    continue;

	  if(patchIdx != currentPatchIdx)
	  {
	    neighbors.at<bool>(currentPatchIdx,patchIdx) = true;
            neighbors.at<bool>(patchIdx,currentPatchIdx) = true;
	  }
	}
      }
    }
  }
}

} //namespace v4r