/* ----------------------------------------------------------------------
 *
 *                    *** Smooth Mach Dynamics ***
 *
 * This file is part of the USER-SMD package for LAMMPS.
 * Copyright (2014) Georg C. Ganzenmueller, georg.ganzenmueller@emi.fhg.de
 * Fraunhofer Ernst-Mach Institute for High-Speed Dynamics, EMI,
 * Eckerstrasse 4, D-79104 Freiburg i.Br, Germany.
 *
 * ----------------------------------------------------------------------- */

/* ----------------------------------------------------------------------
 LAMMPS - Large-scale Atomic/Molecular Massively Parallel Simulator
 http://lammps.sandia.gov, Sandia National Laboratories
 Steve Plimpton, sjplimp@sandia.gov

 Copyright (2003) Sandia Corporation.  Under the terms of Contract
 DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government retains
 certain rights in this software.  This software is distributed under
 the GNU General Public License.

 See the README file in the top-level LAMMPS directory.
 ------------------------------------------------------------------------- */

/* ----------------------------------------------------------------------
   Contributing author: Mike Parks (SNL)
------------------------------------------------------------------------- */

#include <cmath>
#include <cfloat>
#include <cstdlib>
#include <cstring>
#include <cstdio>
#include <iostream>
#include <Eigen/Eigen>
#include "pair_smd_triangulated_surface.h"
#include "atom.h"
#include "domain.h"
#include "force.h"
#include "update.h"
#include "modify.h"
#include "fix.h"
#include "comm.h"
#include "neighbor.h"
#include "neigh_list.h"
#include "neigh_request.h"
#include "memory.h"
#include "error.h"

using namespace std;
using namespace LAMMPS_NS;
using namespace Eigen;

#define SQRT2 1.414213562e0

/* ---------------------------------------------------------------------- */

PairTriSurf::PairTriSurf(LAMMPS *lmp) :
                Pair(lmp) {

        onerad_dynamic = onerad_frozen = maxrad_dynamic = maxrad_frozen = NULL;
        bulkmodulus = NULL;
        kn = NULL;
        scale = 1.0;
}

/* ---------------------------------------------------------------------- */

PairTriSurf::~PairTriSurf() {

        if (allocated) {
                memory->destroy(setflag);
                memory->destroy(cutsq);
                memory->destroy(bulkmodulus);
                memory->destroy(kn);

                delete[] onerad_dynamic;
                delete[] onerad_frozen;
                delete[] maxrad_dynamic;
                delete[] maxrad_frozen;
        }
}

/* ---------------------------------------------------------------------- */

void PairTriSurf::compute(int eflag, int vflag) {
        int i, j, ii, jj, inum, jnum, itype, jtype;
        double rsq, r, evdwl, fpair;
        int *ilist, *jlist, *numneigh, **firstneigh;
        double rcut, r_geom, delta, r_tri, r_particle, touch_distance, dt_crit;
        int tri, particle;
        Vector3d normal, x1, x2, x3, x4, x13, x23, x43, w, cp, x4cp, vnew, v_old;
        ;
        Vector3d xi, x_center, dx;
        Matrix2d C;
        Vector2d w2d, rhs;

        evdwl = 0.0;
        ev_init(eflag, vflag);

        tagint *mol = atom->molecule;
        double **f = atom->f;
        double **smd_data_9 = atom->smd_data_9;
        double **x = atom->x;
        double **x0 = atom->x0;
        double **v = atom->v;
        double *rmass = atom->rmass;
        int *type = atom->type;
        int nlocal = atom->nlocal;
        double *radius = atom->contact_radius;
        double rcutSq;
        Vector3d offset;

        int newton_pair = force->newton_pair;
        int periodic = (domain->xperiodic || domain->yperiodic || domain->zperiodic);

        inum = list->inum;
        ilist = list->ilist;
        numneigh = list->numneigh;
        firstneigh = list->firstneigh;

        int max_neighs = 0;
        stable_time_increment = 1.0e22;

        // loop over neighbors of my atoms using a half neighbor list
        for (ii = 0; ii < inum; ii++) {
                i = ilist[ii];
                itype = type[i];
                jlist = firstneigh[i];
                jnum = numneigh[i];
                max_neighs = MAX(max_neighs, jnum);

                for (jj = 0; jj < jnum; jj++) {
                        j = jlist[jj];

                        j &= NEIGHMASK;

                        jtype = type[j];

                        /*
                         * decide which one of i, j is triangle and which is particle
                         */
                        if ((mol[i] < 65535) && (mol[j] >= 65535)) {
                                particle = i;
                                tri = j;
                        } else if ((mol[j] < 65535) && (mol[i] >= 65535)) {
                                particle = j;
                                tri = i;
                        } else {
                                error->one(FLERR, "unknown case");
                        }

                        //x_center << x[tri][0], x[tri][1], x[tri][2]; // center of triangle
                        x_center(0) = x[tri][0];
                        x_center(1) = x[tri][1];
                        x_center(2) = x[tri][2];
                        //x4 << x[particle][0], x[particle][1], x[particle][2];
                        x4(0) = x[particle][0];
                        x4(1) = x[particle][1];
                        x4(2) = x[particle][2];
                        dx = x_center - x4; //
                        if (periodic) {
                                domain->minimum_image(dx(0), dx(1), dx(2));
                        }
                        rsq = dx.squaredNorm();

                        r_tri = scale * radius[tri];
                        r_particle = scale * radius[particle];
                        rcut = r_tri + r_particle;
                        rcutSq = rcut * rcut;

                        //printf("type i=%d, type j=%d, r=%f, ri=%f, rj=%f\n", itype, jtype, sqrt(rsq), ri, rj);

                        if (rsq < rcutSq) {

                                /*
                                 * gather triangle information
                                 */
                                normal(0) = x0[tri][0];
                                normal(1) = x0[tri][1];
                                normal(2) = x0[tri][2];

                                /*
                                 * distance check: is particle closer than its radius to the triangle plane?
                                 */
                                if (fabs(dx.dot(normal)) < radius[particle]) {
                                        /*
                                         * get other two triangle vertices
                                         */
                                        x1(0) = smd_data_9[tri][0];
                                        x1(1) = smd_data_9[tri][1];
                                        x1(2) = smd_data_9[tri][2];
                                        x2(0) = smd_data_9[tri][3];
                                        x2(1) = smd_data_9[tri][4];
                                        x2(2) = smd_data_9[tri][5];
                                        x3(0) = smd_data_9[tri][6];
                                        x3(1) = smd_data_9[tri][7];
                                        x3(2) = smd_data_9[tri][8];

                                        PointTriangleDistance(x4, x1, x2, x3, cp, r);

                                        /*
                                         * distance to closest point
                                         */
                                        x4cp = x4 - cp;

                                        /*
                                         * flip normal to point in direction of x4cp
                                         */

                                        if (x4cp.dot(normal) < 0.0) {
                                                normal *= -1.0;
                                        }

                                        /*
                                         * penalty force pushes particle away from triangle
                                         */
                                        if (r < 1.0 * radius[particle]) {

                                                delta = radius[particle] - r; // overlap distance
                                                r_geom = radius[particle];
                                                fpair = 1.066666667e0 * bulkmodulus[itype][jtype] * delta * sqrt(delta * r_geom);
                                                dt_crit = 3.14 * sqrt(rmass[particle] / (fpair / delta));
                                                stable_time_increment = MIN(stable_time_increment, dt_crit);

                                                evdwl = r * fpair * 0.4e0 * delta; // GCG 25 April: this expression conserves total energy

                                                fpair /= (r + 1.0e-2 * radius[particle]); // divide by r + softening and multiply with non-normalized distance vector

                                                if (particle < nlocal) {
                                                        f[particle][0] += x4cp(0) * fpair;
                                                        f[particle][1] += x4cp(1) * fpair;
                                                        f[particle][2] += x4cp(2) * fpair;
                                                }

                                                if (tri < nlocal) {
                                                        f[tri][0] -= x4cp(0) * fpair;
                                                        f[tri][1] -= x4cp(1) * fpair;
                                                        f[tri][2] -= x4cp(2) * fpair;
                                                }

                                                if (evflag) {
                                                        ev_tally(i, j, nlocal, newton_pair, evdwl, 0.0, fpair, x4cp(0), x4cp(1), x4cp(2));
                                                }

                                        }

                                        /*
                                         * if particle comes too close to triangle, reflect its velocity and explicitly move it away
                                         */

                                        touch_distance = 1.0 * radius[particle];
                                        if (r < touch_distance) {

                                                /*
                                                 * reflect velocity if it points toward triangle
                                                 */

                                                normal = x4cp / r;

                                                //v_old << v[particle][0], v[particle][1], v[particle][2];
                                                v_old(0) = v[particle][0];
                                                v_old(1) = v[particle][1];
                                                v_old(2) = v[particle][2];
                                                if (v_old.dot(normal) < 0.0) {
                                                        //printf("flipping velocity\n");
                                                        vnew = 1.0 * (-2.0 * v_old.dot(normal) * normal + v_old);
                                                        v[particle][0] = vnew(0);
                                                        v[particle][1] = vnew(1);
                                                        v[particle][2] = vnew(2);
                                                }

                                                //printf("moving particle on top of triangle\n");
                                                x[particle][0] = cp(0) + touch_distance * normal(0);
                                                x[particle][1] = cp(1) + touch_distance * normal(1);
                                                x[particle][2] = cp(2) + touch_distance * normal(2);
                                        }

                                }
                        }
                }
        }

//      int max_neighs_all = 0;
//      MPI_Allreduce(&max_neighs, &max_neighs_all, 1, MPI_INT, MPI_MAX, world);
//      if (comm->me == 0) {
//              printf("max. neighs in tri pair is %d\n", max_neighs_all);
//      }
//
//              double stable_time_increment_all = 0.0;
//              MPI_Allreduce(&stable_time_increment, &stable_time_increment_all, 1, MPI_DOUBLE, MPI_MIN, world);
//              if (comm->me == 0) {
//                      printf("stable time step tri pair is %f\n", stable_time_increment_all);
//              }
}

/* ----------------------------------------------------------------------
 allocate all arrays
 ------------------------------------------------------------------------- */

void PairTriSurf::allocate() {
        allocated = 1;
        int n = atom->ntypes;

        memory->create(setflag, n + 1, n + 1, "pair:setflag");
        for (int i = 1; i <= n; i++)
                for (int j = i; j <= n; j++)
                        setflag[i][j] = 0;

        memory->create(bulkmodulus, n + 1, n + 1, "pair:kspring");
        memory->create(kn, n + 1, n + 1, "pair:kn");

        memory->create(cutsq, n + 1, n + 1, "pair:cutsq"); // always needs to be allocated, even with granular neighborlist

        onerad_dynamic = new double[n + 1];
        onerad_frozen = new double[n + 1];
        maxrad_dynamic = new double[n + 1];
        maxrad_frozen = new double[n + 1];
}

/* ----------------------------------------------------------------------
 global settings
 ------------------------------------------------------------------------- */

void PairTriSurf::settings(int narg, char **arg) {
        if (narg != 1)
                error->all(FLERR, "Illegal number of args for pair_style smd/tri_surface");

        scale = force->numeric(FLERR, arg[0]);
        if (comm->me == 0) {
                printf("\n>>========>>========>>========>>========>>========>>========>>========>>========\n");
                printf("SMD/TRI_SURFACE CONTACT SETTINGS:\n");
                printf("... effective contact radius is scaled by %f\n", scale);
                printf(">>========>>========>>========>>========>>========>>========>>========>>========\n");
        }

}

/* ----------------------------------------------------------------------
 set coeffs for one or more type pairs
 ------------------------------------------------------------------------- */

void PairTriSurf::coeff(int narg, char **arg) {
        if (narg != 3)
                error->all(FLERR, "Incorrect args for pair coefficients");
        if (!allocated)
                allocate();

        int ilo, ihi, jlo, jhi;
        force->bounds(FLERR,arg[0], atom->ntypes, ilo, ihi);
        force->bounds(FLERR,arg[1], atom->ntypes, jlo, jhi);

        double bulkmodulus_one = atof(arg[2]);

        // set short-range force constant
        double kn_one = 0.0;
        if (domain->dimension == 3) {
                kn_one = (16. / 15.) * bulkmodulus_one; //assuming poisson ratio = 1/4 for 3d
        } else {
                kn_one = 0.251856195 * (2. / 3.) * bulkmodulus_one; //assuming poisson ratio = 1/3 for 2d
        }

        int count = 0;
        for (int i = ilo; i <= ihi; i++) {
                for (int j = MAX(jlo, i); j <= jhi; j++) {
                        bulkmodulus[i][j] = bulkmodulus_one;
                        kn[i][j] = kn_one;
                        setflag[i][j] = 1;
                        count++;
                }
        }

        if (count == 0)
                error->all(FLERR, "Incorrect args for pair coefficients");
}

/* ----------------------------------------------------------------------
 init for one type pair i,j and corresponding j,i
 ------------------------------------------------------------------------- */

double PairTriSurf::init_one(int i, int j) {

        if (!allocated)
                allocate();

        if (setflag[i][j] == 0)
                error->all(FLERR, "All pair coeffs are not set");

        bulkmodulus[j][i] = bulkmodulus[i][j];
        kn[j][i] = kn[i][j];

        // cutoff = sum of max I,J radii for
        // dynamic/dynamic & dynamic/frozen interactions, but not frozen/frozen

        double cutoff = maxrad_dynamic[i] + maxrad_dynamic[j];
        cutoff = MAX(cutoff, maxrad_frozen[i] + maxrad_dynamic[j]);
        cutoff = MAX(cutoff, maxrad_dynamic[i] + maxrad_frozen[j]);

        if (comm->me == 0) {
                printf("cutoff for pair smd/smd/tri_surface = %f\n", cutoff);
        }
        return cutoff;
}

/* ----------------------------------------------------------------------
 init specific to this pair style
 ------------------------------------------------------------------------- */

void PairTriSurf::init_style() {
        int i;

        // error checks

        if (!atom->contact_radius_flag)
                error->all(FLERR, "Pair style smd/smd/tri_surface requires atom style with contact_radius");

        // old: half list
        int irequest = neighbor->request(this);
        neighbor->requests[irequest]->size = 1;

        // need a full neighbor list
//      int irequest = neighbor->request(this);
//      neighbor->requests[irequest]->half = 0;
//      neighbor->requests[irequest]->full = 1;

        // set maxrad_dynamic and maxrad_frozen for each type
        // include future Fix pour particles as dynamic

        for (i = 1; i <= atom->ntypes; i++)
                onerad_dynamic[i] = onerad_frozen[i] = 0.0;

        double *radius = atom->radius;
        int *type = atom->type;
        int nlocal = atom->nlocal;

        for (i = 0; i < nlocal; i++) {
                onerad_dynamic[type[i]] = MAX(onerad_dynamic[type[i]], radius[i]);
        }

        MPI_Allreduce(&onerad_dynamic[1], &maxrad_dynamic[1], atom->ntypes, MPI_DOUBLE, MPI_MAX, world);
        MPI_Allreduce(&onerad_frozen[1], &maxrad_frozen[1], atom->ntypes, MPI_DOUBLE, MPI_MAX, world);
}

/* ----------------------------------------------------------------------
 neighbor callback to inform pair style of neighbor list to use
 optional granular history list
 ------------------------------------------------------------------------- */

void PairTriSurf::init_list(int id, NeighList *ptr) {
        if (id == 0)
                list = ptr;
}

/* ----------------------------------------------------------------------
 memory usage of local atom-based arrays
 ------------------------------------------------------------------------- */

double PairTriSurf::memory_usage() {

        return 0.0;
}

/*
 * distance between triangle and point
 */
/*
 function [dist,PP0] = pointTriangleDistance(TRI,P)
 % calculate distance between a point and a triangle in 3D
 % SYNTAX
 %   dist = pointTriangleDistance(TRI,P)
 %   [dist,PP0] = pointTriangleDistance(TRI,P)
 %
 % DESCRIPTION
 %   Calculate the distance of a given point P from a triangle TRI.
 %   Point P is a row vector of the form 1x3. The triangle is a matrix
 %   formed by three rows of points TRI = [P1;P2;P3] each of size 1x3.
 %   dist = pointTriangleDistance(TRI,P) returns the distance of the point P
 %   to the triangle TRI.
 %   [dist,PP0] = pointTriangleDistance(TRI,P) additionally returns the
 %   closest point PP0 to P on the triangle TRI.
 %
 % Author: Gwendolyn Fischer
 % Release: 1.0
 % Release date: 09/02/02
 % Release: 1.1 Fixed Bug because of normalization
 % Release: 1.2 Fixed Bug because of typo in region 5 20101013
 % Release: 1.3 Fixed Bug because of typo in region 2 20101014

 % Possible extention could be a version tailored not to return the distance
 % and additionally the closest point, but instead return only the closest
 % point. Could lead to a small speed gain.

 % Example:
 % %% The Problem
 % P0 = [0.5 -0.3 0.5];
 %
 % P1 = [0 -1 0];
 % P2 = [1  0 0];
 % P3 = [0  0 0];
 %
 % vertices = [P1; P2; P3];
 % faces = [1 2 3];
 %
 % %% The Engine
 % [dist,PP0] = pointTriangleDistance([P1;P2;P3],P0);
 %
 % %% Visualization
 % [x,y,z] = sphere(20);
 % x = dist*x+P0(1);
 % y = dist*y+P0(2);
 % z = dist*z+P0(3);
 %
 % figure
 % hold all
 % patch('Vertices',vertices,'Faces',faces,'FaceColor','r','FaceAlpha',0.8);
 % plot3(P0(1),P0(2),P0(3),'b*');
 % plot3(PP0(1),PP0(2),PP0(3),'*g')
 % surf(x,y,z,'FaceColor','b','FaceAlpha',0.3)
 % view(3)

 % The algorithm is based on
 % "David Eberly, 'Distance Between Point and Triangle in 3D',
 % Geometric Tools, LLC, (1999)"
 % http:\\www.geometrictools.com/Documentation/DistancePoint3Triangle3.pdf
 %
 %        ^t
 %  \     |
 %   \reg2|
 %    \   |
 %     \  |
 %      \ |
 %       \|
 %        *P2
 %        |\
%        | \
%  reg3  |  \ reg1
 %        |   \
%        |reg0\
%        |     \
%        |      \ P1
 % -------*-------*------->s
 %        |P0      \
%  reg4  | reg5    \ reg6
 */

//void PairTriSurf::PointTriangleDistance(const Vector3d P, const Vector3d TRI1, const Vector3d TRI2, const Vector3d TRI3,
//              Vector3d &CP, double &dist) {
//
//      Vector3d B, E0, E1, D;
//      double a, b, c, d, e, f;
//      double det, s, t, sqrDistance, tmp0, tmp1, numer, denom, invDet;
//
//      // rewrite triangle in normal form
//      B = TRI1;
//      E0 = TRI2 - B;
//      E1 = TRI3 - B;
//
//      D = B - P;
//      a = E0.dot(E0);
//      b = E0.dot(E1);
//      c = E1.dot(E1);
//      d = E0.dot(D);
//      e = E1.dot(D);
//      f = D.dot(D);
//
//      det = a * c - b * b;
//      //% do we have to use abs here?
//      s = b * e - c * d;
//      t = b * d - a * e;
//
//      //% Terible tree of conditionals to determine in which region of the diagram
//      //% shown above the projection of the point into the triangle-plane lies.
//      if ((s + t) <= det) {
//              if (s < 0) {
//                      if (t < 0) {
//                              // %region4
//                              if (d < 0) {
//                                      t = 0;
//                                      if (-d >= a) {
//                                              s = 1;
//                                              sqrDistance = a + 2 * d + f;
//                                      } else {
//                                              s = -d / a;
//                                              sqrDistance = d * s + f;
//                                      }
//                              } else {
//                                      s = 0;
//                                      if (e >= 0) {
//                                              t = 0;
//                                              sqrDistance = f;
//                                      } else {
//                                              if (-e >= c) {
//                                                      t = 1;
//                                                      sqrDistance = c + 2 * e + f;
//                                              } else {
//                                                      t = -e / c;
//                                                      sqrDistance = e * t + f;
//                                              }
//                                      }
//                              }
//                              // end % of region 4
//                      } else {
//                              // % region 3
//                              s = 0;
//                              if (e >= 0) {
//                                      t = 0;
//                                      sqrDistance = f;
//                              } else {
//                                      if (-e >= c) {
//                                              t = 1;
//                                              sqrDistance = c + 2 * e + f;
//                                      } else {
//                                              t = -e / c;
//                                              sqrDistance = e * t + f;
//                                      }
//                              }
//                      }
//                      // end of region 3
//              } else {
//                      if (t < 0) {
//                              //% region 5
//                              t = 0;
//                              if (d >= 0) {
//                                      s = 0;
//                                      sqrDistance = f;
//                              } else {
//                                      if (-d >= a) {
//                                              s = 1;
//                                              sqrDistance = a + 2 * d + f;
//                                      } else {
//                                              s = -d / a;
//                                              sqrDistance = d * s + f;
//                                      }
//                              }
//                      } else {
//                              // region 0
//                              invDet = 1 / det;
//                              s = s * invDet;
//                              t = t * invDet;
//                              sqrDistance = s * (a * s + b * t + 2 * d) + t * (b * s + c * t + 2 * e) + f;
//                      }
//              }
//      } else {
//              if (s < 0) {
//                      // % region 2
//                      tmp0 = b + d;
//                      tmp1 = c + e;
//                      if (tmp1 > tmp0) { //% minimum on edge s+t=1
//                              numer = tmp1 - tmp0;
//                              denom = a - 2 * b + c;
//                              if (numer >= denom) {
//                                      s = 1;
//                                      t = 0;
//                                      sqrDistance = a + 2 * d + f;
//                              } else {
//                                      s = numer / denom;
//                                      t = 1 - s;
//                                      sqrDistance = s * (a * s + b * t + 2 * d) + t * (b * s + c * t + 2 * e) + f;
//                              }
//                      } else
//                              // % minimum on edge s=0
//                              s = 0;
//                      if (tmp1 <= 0) {
//                              t = 1;
//                              sqrDistance = c + 2 * e + f;
//                      } else {
//                              if (e >= 0) {
//                                      t = 0;
//                                      sqrDistance = f;
//                              } else {
//                                      t = -e / c;
//                                      sqrDistance = e * t + f;
//                              }
//                      }
//              } //end % of region     2
//              else {
//                      if (t < 0) {
//                              // %region6
//                              tmp0 = b + e;
//                              tmp1 = a + d;
//                              if (tmp1 > tmp0) {
//                                      numer = tmp1 - tmp0;
//                                      denom = a - 2 * b + c;
//                                      if (numer >= denom) {
//                                              t = 1;
//                                              s = 0;
//                                              sqrDistance = c + 2 * e + f;
//                                      } else {
//                                              t = numer / denom;
//                                              s = 1 - t;
//                                              sqrDistance = s * (a * s + b * t + 2 * d) + t * (b * s + c * t + 2 * e) + f;
//                                      }
//                              } else {
//                                      t = 0;
//                                      if (tmp1 <= 0) {
//                                              s = 1;
//                                              sqrDistance = a + 2 * d + f;
//                                      } else {
//                                              if (d >= 0) {
//                                                      s = 0;
//                                                      sqrDistance = f;
//                                              } else {
//                                                      s = -d / a;
//                                                      sqrDistance = d * s + f;
//                                              }
//                                      }
//                              } // % end region 6
//                      } else {
//                              //% region 1
//                              numer = c + e - b - d;
//                              if (numer <= 0) {
//                                      s = 0;
//                                      t = 1;
//                                      sqrDistance = c + 2 * e + f;
//                              } else {
//                                      denom = a - 2 * b + c;
//                                      if (numer >= denom) {
//                                              s = 1;
//                                              t = 0;
//                                              sqrDistance = a + 2 * d + f;
//                                      } else {
//                                              s = numer / denom;
//                                              t = 1 - s;
//                                              sqrDistance = s * (a * s + b * t + 2 * d) + t * (b * s + c * t + 2 * e) + f;
//                                      }
//                              } //% end of region 1
//                      }
//              }
//      }
//
//      // % account for numerical round-off error
//      if (sqrDistance < 0) {
//              sqrDistance = 0;
//      }
//
//      dist = sqrt(sqrDistance);
//
//      // closest point
//      CP = B + s * E0 + t * E1;
//
//}
/*
 * % The algorithm is based on
 % "David Eberly, 'Distance Between Point and Triangle in 3D',
 % Geometric Tools, LLC, (1999)"
 % http:\\www.geometrictools.com/Documentation/DistancePoint3Triangle3.pdf
 */

void PairTriSurf::PointTriangleDistance(const Vector3d sourcePosition, const Vector3d TRI0, const Vector3d TRI1,
                const Vector3d TRI2, Vector3d &CP, double &dist) {

        Vector3d edge0 = TRI1 - TRI0;
        Vector3d edge1 = TRI2 - TRI0;
        Vector3d v0 = TRI0 - sourcePosition;

        double a = edge0.dot(edge0);
        double b = edge0.dot(edge1);
        double c = edge1.dot(edge1);
        double d = edge0.dot(v0);
        double e = edge1.dot(v0);

        double det = a * c - b * b;
        double s = b * e - c * d;
        double t = b * d - a * e;

        if (s + t < det) {
                if (s < 0.f) {
                        if (t < 0.f) {
                                if (d < 0.f) {
                                        s = clamp(-d / a, 0.f, 1.f);
                                        t = 0.f;
                                } else {
                                        s = 0.f;
                                        t = clamp(-e / c, 0.f, 1.f);
                                }
                        } else {
                                s = 0.f;
                                t = clamp(-e / c, 0.f, 1.f);
                        }
                } else if (t < 0.f) {
                        s = clamp(-d / a, 0.f, 1.f);
                        t = 0.f;
                } else {
                        float invDet = 1.f / det;
                        s *= invDet;
                        t *= invDet;
                }
        } else {
                if (s < 0.f) {
                        float tmp0 = b + d;
                        float tmp1 = c + e;
                        if (tmp1 > tmp0) {
                                float numer = tmp1 - tmp0;
                                float denom = a - 2 * b + c;
                                s = clamp(numer / denom, 0.f, 1.f);
                                t = 1 - s;
                        } else {
                                t = clamp(-e / c, 0.f, 1.f);
                                s = 0.f;
                        }
                } else if (t < 0.f) {
                        if (a + d > b + e) {
                                float numer = c + e - b - d;
                                float denom = a - 2 * b + c;
                                s = clamp(numer / denom, 0.f, 1.f);
                                t = 1 - s;
                        } else {
                                s = clamp(-e / c, 0.f, 1.f);
                                t = 0.f;
                        }
                } else {
                        float numer = c + e - b - d;
                        float denom = a - 2 * b + c;
                        s = clamp(numer / denom, 0.f, 1.f);
                        t = 1.f - s;
                }
        }

        CP = TRI0 + s * edge0 + t * edge1;
        dist = (CP - sourcePosition).norm();

}

double PairTriSurf::clamp(const double a, const double min, const double max) {
        if (a < min) {
                return min;
        } else if (a > max) {
                return max;
        } else {
                return a;
        }
}

void *PairTriSurf::extract(const char *str, int &/*i*/) {
        //printf("in PairTriSurf::extract\n");
        if (strcmp(str, "smd/tri_surface/stable_time_increment_ptr") == 0) {
                return (void *) &stable_time_increment;
        }

        return NULL;

}


/*
 *            Copyright 2009-2020 The VOTCA Development Team
 *                       (http://www.votca.org)
 *
 *      Licensed under the Apache License, Version 2.0 (the "License")
 *
 * You may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *              http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

// Third party includes
#include <boost/format.hpp>

// VOTCA includes
#include <votca/tools/elements.h>

// Local VOTCA includes
#include "votca/xtp/atom.h"
#include "votca/xtp/forces.h"
#include "votca/xtp/statetracker.h"

namespace votca {
namespace xtp {

using std::flush;
void Forces::Initialize(tools::Property& options) {
  force_method_ = options.get(".method").as<std::string>();

  displacement_ = options.get(".displacement").as<double>();  // Angstrom
  displacement_ *= tools::conv::ang2bohr;

  remove_total_force_ = options.get(".CoMforce_removal").as<bool>();
}

void Forces::Calculate(const Orbitals& orbitals) {

  Index natoms = orbitals.QMAtoms().size();
  forces_ = Eigen::MatrixX3d::Zero(natoms, 3);

  for (Index atom_index = 0; atom_index < natoms; atom_index++) {

    XTP_LOG(Log::debug, *pLog_)
        << "FORCES--DEBUG working on atom " << atom_index << flush;
    Eigen::Vector3d atom_force = Eigen::Vector3d::Zero();
    // Calculate Force on this atom
    if (force_method_ == "forward") {
      atom_force = NumForceForward(orbitals, atom_index);
    }
    if (force_method_ == "central") {
      atom_force = NumForceCentral(orbitals, atom_index);
    }
    forces_.row(atom_index) = atom_force.transpose();
  }
  if (remove_total_force_) {
    RemoveTotalForce();
  }
  return;
}

void Forces::Report() const {

  XTP_LOG(Log::error, *pLog_)
      << (boost::format(" ---- FORCES (Hartree/Bohr)   ")).str() << flush;
  XTP_LOG(Log::error, *pLog_)
      << (boost::format("      %1$s differences   ") % force_method_).str()
      << flush;
  XTP_LOG(Log::error, *pLog_)
      << (boost::format("      displacement %1$1.4f Angstrom   ") %
          (displacement_ * tools::conv::bohr2ang))
             .str()
      << flush;
  XTP_LOG(Log::error, *pLog_)
      << (boost::format(" Atom\t x\t  y\t  z ")).str() << flush;

  for (Index i = 0; i < forces_.rows(); i++) {
    XTP_LOG(Log::error, *pLog_)
        << (boost::format("%1$4d    %2$+1.4f  %3$+1.4f  %4$+1.4f") % i %
            forces_(i, 0) % forces_(i, 1) % forces_(i, 2))
               .str()
        << flush;
  }
  return;
}

Eigen::Vector3d Forces::NumForceForward(Orbitals orbitals, Index atom_index) {
  Eigen::Vector3d force = Eigen::Vector3d::Zero();
  // get this atoms's current coordinates
  double energy_center =
      orbitals.getTotalStateEnergy(tracker_.CalcState(orbitals));
  const Eigen::Vector3d current_pos = orbitals.QMAtoms()[atom_index].getPos();
  for (Index i_cart = 0; i_cart < 3; i_cart++) {
    Eigen::Vector3d displacement_vec = Eigen::Vector3d::Zero();
    displacement_vec[i_cart] = displacement_;
    // update the coordinate
    Eigen::Vector3d pos_displaced = current_pos + displacement_vec;
    orbitals.updateAtomPostion(atom_index, pos_displaced);
    gwbse_engine_.ExcitationEnergies(orbitals);
    double energy_displaced =
        orbitals.getTotalStateEnergy(tracker_.CalcState(orbitals));
    force(i_cart) = (energy_center - energy_displaced) / displacement_;
    orbitals.updateAtomPostion(
        atom_index, current_pos);  // restore original coordinate into segment
  }                                // Cartesian directions
  return force;
}

Eigen::Vector3d Forces::NumForceCentral(Orbitals orbitals, Index atom_index) {
  Eigen::Vector3d force = Eigen::Vector3d::Zero();
  const Eigen::Vector3d current_pos = orbitals.QMAtoms()[atom_index].getPos();
  for (Index i_cart = 0; i_cart < 3; i_cart++) {
    XTP_LOG(Log::debug, *pLog_)
        << "FORCES--DEBUG           Cartesian component " << i_cart << flush;
    Eigen::Vector3d displacement_vec = Eigen::Vector3d::Zero();
    displacement_vec[i_cart] = displacement_;
    // update the coordinate
    Eigen::Vector3d pos_displaced = current_pos + displacement_vec;
    orbitals.updateAtomPostion(atom_index, pos_displaced);
    gwbse_engine_.ExcitationEnergies(orbitals);
    double energy_displaced_plus =
        orbitals.getTotalStateEnergy(tracker_.CalcState(orbitals));
    // update the coordinate
    pos_displaced = current_pos - displacement_vec;
    orbitals.updateAtomPostion(atom_index, pos_displaced);
    gwbse_engine_.ExcitationEnergies(orbitals);
    double energy_displaced_minus =
        orbitals.getTotalStateEnergy(tracker_.CalcState(orbitals));
    force(i_cart) =
        0.5 * (energy_displaced_minus - energy_displaced_plus) / displacement_;
    orbitals.updateAtomPostion(
        atom_index, current_pos);  // restore original coordinate into orbital
  }
  return force;
}

void Forces::RemoveTotalForce() {
  Eigen::Vector3d avgtotal_force =
      forces_.colwise().sum() / double(forces_.rows());
  for (Index i_atom = 0; i_atom < forces_.rows(); i_atom++) {
    forces_.row(i_atom) -= avgtotal_force;
  }
  return;
}

}  // namespace xtp
}  // namespace votca


/*

Copyright (c) 2005-2017, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

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

*/

#include "BidomainCorrectionTermAssembler.hpp"
#include "UblasIncludes.hpp"
#include <boost/numeric/ublas/vector_proxy.hpp>

template<unsigned ELEM_DIM, unsigned SPACE_DIM>
BidomainCorrectionTermAssembler<ELEM_DIM,SPACE_DIM>::BidomainCorrectionTermAssembler(
     AbstractTetrahedralMesh<ELEM_DIM,SPACE_DIM>* pMesh,
     BidomainTissue<SPACE_DIM>* pTissue)
        : AbstractCorrectionTermAssembler<ELEM_DIM,SPACE_DIM,2>(pMesh,pTissue)
{
    mpConfig = HeartConfig::Instance();
    assert(mpConfig->GetUseStateVariableInterpolation());
}

template<unsigned ELEM_DIM, unsigned SPACE_DIM>
c_vector<double,2*(ELEM_DIM+1)> BidomainCorrectionTermAssembler<ELEM_DIM,SPACE_DIM>::ComputeVectorTerm(
    c_vector<double, ELEM_DIM+1> &rPhi,
    c_matrix<double, SPACE_DIM, ELEM_DIM+1> &rGradPhi /* not used */,
    ChastePoint<SPACE_DIM> &rX /* not used */,
    c_vector<double,2> &rU,
    c_matrix<double, 2, SPACE_DIM> &rGradU /* not used */,
    Element<ELEM_DIM,SPACE_DIM>* pElement /* not used */)
{
    double Am = mpConfig->GetSurfaceAreaToVolumeRatio();

    // compute the ionic current at this quadrature point using the
    // interpolated state variables, and a random choice of cell (all
    // should be the same)
    unsigned node_global_index = pElement->GetNodeGlobalIndex(0);
    AbstractCardiacCellInterface* p_any_cell = this->mpCardiacTissue->GetCardiacCellOrHaloCell(node_global_index);
    double ionic_sv_interp = p_any_cell->GetIIonic(&(this->mStateVariablesAtQuadPoint));

    c_vector<double,2*(ELEM_DIM+1)> ret;

    ublas::vector_slice<c_vector<double, 2*(ELEM_DIM+1)> > slice_V  (ret, slice (0, 2, ELEM_DIM+1));
    ublas::vector_slice<c_vector<double, 2*(ELEM_DIM+1)> > slice_Phi(ret, slice (1, 2, ELEM_DIM+1));

    // add on the SVI ionic current, and take away the original NCI (linearly
    // interpolated ionic current) that would have been added as part of
    // the matrix-based assembly stage.
    noalias(slice_V)   = rPhi * (-Am) * ( ionic_sv_interp - this->mIionicInterp );
    // no correction needed for elliptic equation
    noalias(slice_Phi) = zero_vector<double>(ELEM_DIM+1);

    return ret;
}

// Explicit instantiation
template class BidomainCorrectionTermAssembler<1,1>;
template class BidomainCorrectionTermAssembler<2,2>;
template class BidomainCorrectionTermAssembler<3,3>;


#pragma once
#include <boost/pending/disjoint_sets.hpp>
#include "xtensor/xtensor.hpp"
#include <queue>
#include <functional>
#include "affogato/segmentation/mutex_watershed.hxx"
#include "affogato/util.hxx"

namespace affogato {
namespace segmentation {

    //
    // semantic helper functions:
    // check_semantic_constraint: check if two representatives have different semantic labels
    // assign_semantic_label: assign the semantic label of a representative if not yet existent
    // merge_semantic_labels: assign two representatives to the same semantic label
    //

    template<class SEMANTIC_LABELING>
    inline bool check_semantic_constraint(const uint64_t ru, const uint64_t rv,
                                          const SEMANTIC_LABELING & semantic_labeling) {
        int64_t slu = semantic_labeling[ru];
        int64_t slv = semantic_labeling[rv];

        if (slu >= 0 && slv >= 0)
            return slu != slv;
        return false;
    }

    // set semantic label of 'ru' to 'sl'
    template<class SEMANTIC_LABELING>
    inline void assign_semantic_label(const uint64_t ru, const int64_t sl,
                                      SEMANTIC_LABELING & semantic_labeling) {
        if(semantic_labeling[ru] < 0){
            semantic_labeling[ru] = sl;
        }
    }

    // assign same semantic label to 'ru' and 'rv', assume they don't have different labels
    template<class SEMANTIC_LABELING>
    inline void merge_semantic_labels(const uint64_t ru, const uint64_t rv,
                                      SEMANTIC_LABELING & semantic_labeling) {
        int64_t slu = semantic_labeling[ru];
        int64_t slv = semantic_labeling[rv];

        if (slu >= 0 && slv < 0)
            semantic_labeling[rv] = slu;
        if (slu < 0 && slv >= 0)
            semantic_labeling[ru] = slv;
    }

    // compute mutex clustering for a graph with attrative and mutex edges
    template<class EDGE_ARRAY, class WEIGHT_ARRAY, class NODE_ARRAY, class SEMANTIC_ARRAY>
    void compute_semantic_mws_clustering(const size_t number_of_labels,
                                         const xt::xexpression<EDGE_ARRAY> & uvs_exp,
                                         const xt::xexpression<EDGE_ARRAY> & mutex_uvs_exp,
                                         const xt::xexpression<EDGE_ARRAY> & semantic_uts_exp,
                                         const xt::xexpression<WEIGHT_ARRAY> & weights_exp,
                                         const xt::xexpression<WEIGHT_ARRAY> & mutex_weights_exp,
                                         const xt::xexpression<WEIGHT_ARRAY> & semantic_weights_exp,
                                         xt::xexpression<NODE_ARRAY> & node_labeling_exp,
                                         xt::xexpression<SEMANTIC_ARRAY> & semantic_labeling_exp) {

        // casts
        const auto & uvs = uvs_exp.derived_cast();
        const auto & mutex_uvs = mutex_uvs_exp.derived_cast();
        const auto & semantic_uts = semantic_uts_exp.derived_cast();
        const auto & weights = weights_exp.derived_cast();
        const auto & mutex_weights = mutex_weights_exp.derived_cast();
        const auto & semantic_weights = semantic_weights_exp.derived_cast();
        auto & node_labeling = node_labeling_exp.derived_cast();
        auto & semantic_labeling = semantic_labeling_exp.derived_cast();

        // make ufd
        std::vector<uint64_t> ranks(number_of_labels);
        std::vector<uint64_t> parents(number_of_labels);
        boost::disjoint_sets<uint64_t*, uint64_t*> ufd(&ranks[0], &parents[0]);
        for(uint64_t label = 0; label < number_of_labels; ++label) {
            ufd.make_set(label);
        }

        // determine number of edge types
        const size_t num_edges = uvs.shape()[0];
        const size_t num_mutex = mutex_uvs.shape()[0];
        const size_t num_semantic = semantic_uts.shape()[0];
        const size_t num_internal = num_edges + num_mutex;

        // argsort ALL edges
        // we sort in ascending order
        std::vector<size_t> indices(num_edges + num_mutex + num_semantic);
        std::iota(indices.begin(), indices.end(), 0);
        std::sort(indices.begin(), indices.end(), [&](const size_t a, const size_t b){
            const double val_a = (a >= num_internal) ? semantic_weights(a - num_internal) :
                                 (a >= num_edges)    ? mutex_weights(a - num_edges)
                                                     : weights(a);
            const double val_b = (b >= num_internal) ? semantic_weights(b - num_internal) :
                                 (b >= num_edges)    ? mutex_weights(b - num_edges)
                                                     : weights(b);
            return val_a > val_b;
        });

        // data-structure storing mutex edges
        typedef std::vector<std::vector<uint64_t>> MutexStorage;
        MutexStorage mutexes(number_of_labels);

        // iterate over all edges
        for(const size_t edge_id : indices) {

            // check wether this edge is semantic
            const bool is_semantic_edge = edge_id >= num_internal;
            // check whether this edge is mutex via the edge offset
            const bool is_mutex_edge = edge_id >= num_edges;

            const size_t id = is_semantic_edge ? edge_id - num_internal :
                              is_mutex_edge ? edge_id - num_edges : edge_id;

            // get first incidental node and its current representative of edge_id
            const uint64_t u = is_semantic_edge ? semantic_uts(id, 0) :
                               is_mutex_edge ? mutex_uvs(id, 0) : uvs(id, 0);
            uint64_t ru = ufd.find_set(u);

            if(!is_semantic_edge){

                // find the edge-id or mutex id and the connected nodes
                const uint64_t v = is_mutex_edge ? mutex_uvs(id, 1) : uvs(id, 1);

                // find the current representatives
                uint64_t rv = ufd.find_set(v);

                // if the nodes are already connected, do nothing
                if(ru == rv) {
                    continue;
                }

                if(check_semantic_constraint(ru, rv, semantic_labeling)){
                    continue;
                }

                // if we already have a mutex, we do not need to do anything
                // (if this is a regular edge, we do not link, if it is a mutex edge
                //  we do not need to insert the redundant mutex constraint)
                if(check_mutex(ru, rv, mutexes)) {
                    continue;
                }

                if(is_mutex_edge) {

                    // insert mutex constraint
                    insert_mutex(ru, rv, id, mutexes);

                } else {

                    // link the nodes and merge their mutex constraints
                    ufd.link(u, v);
                    // check  if we have to swap the roots
                    if(ufd.find_set(ru) == rv) {
                        std::swap(ru, rv);
                    }
                    // merge mutexes from rv -> ru
                    merge_mutexes(rv, ru, mutexes);
                }
            } else{
                // find semantic label associated to edge
                int64_t t = semantic_uts(id, 1);
                assign_semantic_label(ru, t, semantic_labeling);
            }
        }

        // get node labeling into output
        util::export_consecutive_labels(ufd, number_of_labels, node_labeling);

        for(size_t label = 0; label < number_of_labels; ++label) {
            uint64_t root = ufd.find_set(label);
            semantic_labeling[label] = semantic_labeling[root];
        }
    }

    // compute mutex segmentation via kruskal
    template<class WEIGHT_ARRAY, class NODE_ARRAY, class SEMANTIC_ARRAY, class INDICATOR_ARRAY>
    void compute_semantic_mws_segmentation(const xt::xexpression<WEIGHT_ARRAY> & sorted_flat_indices_exp,
                                           const xt::xexpression<INDICATOR_ARRAY> & valid_edges_exp,
                                           const std::vector<std::vector<int>> & offsets,
                                           const size_t number_of_attractive_channels,
                                           const std::vector<int> & image_shape,
                                           xt::xexpression<NODE_ARRAY> & node_labeling_exp,
                                           xt::xexpression<SEMANTIC_ARRAY> & semantic_labeling_exp) {

        // casts
        const auto & sorted_flat_indices = sorted_flat_indices_exp.derived_cast();
        const auto & valid_edges = valid_edges_exp.derived_cast();
        auto & node_labeling = node_labeling_exp.derived_cast();
        auto & semantic_labeling = semantic_labeling_exp.derived_cast();

        // determine number of nodes and attractive edges
        const size_t number_of_nodes = node_labeling.size();
        const size_t number_of_attractive_edges = number_of_nodes * number_of_attractive_channels;
        const size_t number_of_offsets = offsets.size();
        const size_t ndims = offsets[0].size();
        const size_t number_of_internal_edges = number_of_nodes * number_of_offsets;

        std::vector<int64_t> array_stride(ndims);
        int64_t current_stride = 1;
        for (int i = ndims-1; i >= 0; --i){
            array_stride[i] = current_stride;
            current_stride *= image_shape[i];
        }

        std::vector<int64_t> offset_strides;
        for (const auto & offset: offsets){
            int64_t stride = 0;
            for (int i = 0; i < offset.size(); ++i){
                stride += offset[i] * array_stride[i];
            }
            offset_strides.push_back(stride);
        }

        // make ufd
        std::vector<uint64_t> ranks(number_of_nodes);
        std::vector<uint64_t> parents(number_of_nodes);
        boost::disjoint_sets<uint64_t*, uint64_t*> ufd(&ranks[0], &parents[0]);
        for(uint64_t label = 0; label < number_of_nodes; ++label) {
            ufd.make_set(label);
        }

        // data-structure for storing semantic labels, unassigned are set to 0
        // std::vector<uint64_t> semantic_labeling(number_of_nodes, 0);

        // data-structure storing mutex edges
        typedef std::vector<std::vector<uint64_t>> MutexStorage;
        MutexStorage mutexes(number_of_nodes);

        // iterate over all edges
        for(const size_t edge_id : sorted_flat_indices) {

            if(!valid_edges(edge_id)){
                continue;
            }

            // check wether this edge is semantic
            const bool is_semantic_edge = edge_id >= number_of_internal_edges;
            // check whether this edge is mutex via the edge offset
            const bool is_mutex_edge = edge_id >= number_of_attractive_edges;

            // get first incidental node and its current representative of edge_id
            const uint64_t u = edge_id % number_of_nodes;
            uint64_t ru = ufd.find_set(u);

            if(!is_semantic_edge){
                // const auto affCoord_ = xt::unravel_from_strides(edge_id, strides, layout);

                // get second incidental node
                const uint64_t v = u + offset_strides[edge_id / number_of_nodes];
                // find the current representative
                uint64_t rv = ufd.find_set(v);

                // if the nodes are already connected, do nothing
                if(ru == rv) {
                    continue;
                }

                if(check_semantic_constraint(ru, rv, semantic_labeling)){
                    continue;
                }

                // if we already have a mutex, we do not need to do anything
                // (if this is a regular edge, we do not link, if it is a mutex edge
                //  we do not need to insert the redundant mutex constraint)
                if(check_mutex(ru, rv, mutexes)) {
                    continue;
                }

                if(is_mutex_edge) {

                    // insert the mutex edge into both mutex edge storages
                    insert_mutex(ru, rv, edge_id, mutexes);

                } else {

                    ufd.link(u, v);
                    // check  if we have to swap the roots
                    if(ufd.find_set(ru) == rv) {
                        std::swap(ru, rv);
                    }
                    // merge mutexes from rv -> ru
                    merge_mutexes(rv, ru, mutexes);
                    merge_semantic_labels(ru, rv, semantic_labeling);
                }
            }else{
                // find semantic label associated to edge
                int64_t sl = edge_id / number_of_nodes - number_of_offsets;

                // add semantic class to cluster
                assign_semantic_label(ru, sl, semantic_labeling);
            }
        }

        // get node labeling into output
        util::export_consecutive_labels(ufd, number_of_nodes, node_labeling);

        for(size_t label = 0; label < number_of_nodes; ++label) {
            uint64_t root = ufd.find_set(label);
            semantic_labeling[label] = semantic_labeling[root];
        }
    }
}
}


//Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

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

#ifndef UUID_8AC84A2217C411E0A7AF3A1BDFD72085
#define UUID_8AC84A2217C411E0A7AF3A1BDFD72085

#include <boost/qvm/inline.hpp>
#include <boost/qvm/quat_traits.hpp>
#include <boost/qvm/deduce_vec.hpp>
#include <boost/qvm/static_assert.hpp>
#include <boost/qvm/enable_if.hpp>

namespace
boost
    {
    namespace
    qvm
        {
        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <class Q>
            struct
            quat_v_
                {
                template <class R>
                operator R() const
                    {
                    R r;
                    assign(r,*this);
                    return r;
                    }

                private:

                quat_v_( quat_v_ const & );
                quat_v_ const & operator=( quat_v_ const & );
                ~quat_v_();
                };
            }

        template <class V>
        struct vec_traits;

        template <class Q>
        struct
        vec_traits< qvm_detail::quat_v_<Q> >
            {
            typedef qvm_detail::quat_v_<Q> this_vector;
            typedef typename quat_traits<Q>::scalar_type scalar_type;
            static int const dim=3;

            template <int I>
            BOOST_QVM_INLINE_CRITICAL
            static
            scalar_type
            read_element( this_vector const & q )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<dim);
                return quat_traits<Q>::template read_element<I+1>( reinterpret_cast<Q const &>(q) );
                }

            template <int I>
            BOOST_QVM_INLINE_CRITICAL
            static
            scalar_type &
            write_element( this_vector & q )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<dim);
                return quat_traits<Q>::template write_element<I+1>( reinterpret_cast<Q &>(q) );
                }
            };

        template <class Q,int D>
        struct
        deduce_vec<qvm_detail::quat_v_<Q>,D>
            {
            typedef vec<typename quat_traits<Q>::scalar_type,D> type;
            };

        template <class Q,int D>
        struct
        deduce_vec2<qvm_detail::quat_v_<Q>,qvm_detail::quat_v_<Q>,D>
            {
            typedef vec<typename quat_traits<Q>::scalar_type,D> type;
            };

        template <class Q>
        BOOST_QVM_INLINE_TRIVIAL
        typename enable_if_c<
            is_quat<Q>::value,
            qvm_detail::quat_v_<Q> const &>::type
        V( Q const & a )
            {
            return reinterpret_cast<qvm_detail::quat_v_<Q> const &>(a);
            }

        template <class Q>
        BOOST_QVM_INLINE_TRIVIAL
        typename enable_if_c<
            is_quat<Q>::value,
            qvm_detail::quat_v_<Q> &>::type
        V( Q & a )
            {
            return reinterpret_cast<qvm_detail::quat_v_<Q> &>(a);
            }

        template <class Q> BOOST_QVM_INLINE_TRIVIAL typename enable_if_c<is_quat<Q>::value,typename quat_traits<Q>::scalar_type>::type S( Q const & a ) { return quat_traits<Q>::template read_element<0>(a); }
        template <class Q> BOOST_QVM_INLINE_TRIVIAL typename enable_if_c<is_quat<Q>::value,typename quat_traits<Q>::scalar_type>::type X( Q const & a ) { return quat_traits<Q>::template read_element<1>(a); }
        template <class Q> BOOST_QVM_INLINE_TRIVIAL typename enable_if_c<is_quat<Q>::value,typename quat_traits<Q>::scalar_type>::type Y( Q const & a ) { return quat_traits<Q>::template read_element<2>(a); }
        template <class Q> BOOST_QVM_INLINE_TRIVIAL typename enable_if_c<is_quat<Q>::value,typename quat_traits<Q>::scalar_type>::type Z( Q const & a ) { return quat_traits<Q>::template read_element<3>(a); }

        template <class Q> BOOST_QVM_INLINE_TRIVIAL typename enable_if_c<is_quat<Q>::value,typename quat_traits<Q>::scalar_type &>::type S( Q & a ) { return quat_traits<Q>::template write_element<0>(a); }
        template <class Q> BOOST_QVM_INLINE_TRIVIAL typename enable_if_c<is_quat<Q>::value,typename quat_traits<Q>::scalar_type &>::type X( Q & a ) { return quat_traits<Q>::template write_element<1>(a); }
        template <class Q> BOOST_QVM_INLINE_TRIVIAL typename enable_if_c<is_quat<Q>::value,typename quat_traits<Q>::scalar_type &>::type Y( Q & a ) { return quat_traits<Q>::template write_element<2>(a); }
        template <class Q> BOOST_QVM_INLINE_TRIVIAL typename enable_if_c<is_quat<Q>::value,typename quat_traits<Q>::scalar_type &>::type Z( Q & a ) { return quat_traits<Q>::template write_element<3>(a); }

        ////////////////////////////////////////////////
        }
    }

#endif


/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * 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.
 *
 * 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 HOLDER 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.
 */
/**
 * @file math.hpp
 * @author Vaclav Blazek <vaclav.blazek@citationtech.net>
 *
 * 3D mesh operations
 */

#include <numeric>

#include <OpenMesh/Core/IO/MeshIO.hh>
#include <OpenMesh/Core/Mesh/TriMesh_ArrayKernelT.hh>
#include <OpenMesh/Tools/Decimater/DecimaterT.hh>
#include <OpenMesh/Tools/Decimater/ModQuadricT.hh>
#include <OpenMesh/Tools/Decimater/ModNormalFlippingT.hh>
#include <OpenMesh/Tools/Decimater/ModAspectRatioT.hh>
#include <OpenMesh/Tools/Decimater/ModEdgeLengthT.hh>
#include <boost/numeric/ublas/vector.hpp>

#include "math/math.hpp"

#include "meshop.hpp"
#include "detail/hybrid-decimater.hpp"

namespace geometry {

/* code moved from the window-mesh utility */
namespace ublas = boost::numeric::ublas;

namespace {

struct NormalTraits : public OpenMesh::DefaultTraits
{
  FaceAttributes( OpenMesh::Attributes::Normal );
  VertexTraits
  {
    int classLabel;
  };

};

typedef OpenMesh::TriMesh_ArrayKernelT<NormalTraits> OMMesh;
typedef OpenMesh::Decimater::DecimaterT<OMMesh> Decimator;
typedef OpenMesh::Decimater::ModQuadricT<OMMesh>::Handle HModQuadric;
typedef detail::ModQuadricHybrid<OMMesh>::Handle HModQuadricHybrid;
typedef OpenMesh::Decimater::ModNormalFlippingT<OMMesh>::Handle HModNormalFlippingT;
typedef OpenMesh::Decimater::ModAspectRatioT<OMMesh>::Handle HModAspectRatioT;
typedef OpenMesh::Decimater::ModEdgeLengthT<OMMesh>::Handle HModEdgeLengthT;

#if OM_VERSION >= 0x60000
typedef detail::ModQuadricConvexT<OMMesh>::Handle HModQuadricConvex;
typedef detail::ClassifyRestrictModT<OMMesh>::Handle HModClassRestrictT;
#endif

#ifdef GEOMETRY_HAS_OPENCV

/** Extension of ModQuadric multiplying the cost by generic 2D weight map
 */
template <class MeshT>
class ModWeightedQuadricT : public OpenMesh::Decimater::ModQuadricT<MeshT> {
public:
    // Defines the types Self, Handle, Base, Mesh, and CollapseInfo
    // and the memberfunction name()
    DECIMATING_MODULE(ModWeightedQuadricT, MeshT, WeightedQuadric);

public:
    ModWeightedQuadricT(MeshT& mesh)
        : OpenMesh::Decimater::ModQuadricT<MeshT>(mesh) {
        Base::mesh().add_property(weights_);
    }

    ~ModWeightedQuadricT() {
        Base::mesh().remove_property(weights_);
    }

    virtual float collapse_priority(const CollapseInfo& ci) {
        const float baseCost = OpenMesh::Decimater::ModQuadricT<MeshT>::collapse_priority(ci);
        if (baseCost != Base::ILLEGAL_COLLAPSE) {
            const double w = Base::mesh().property(weights_, ci.v1);
            return baseCost * w;
        } else {
            return baseCost;
        }
    }

    void setWeights(const std::vector<float>& weights) {
        MeshT& mesh = Base::mesh();
        int i = 0;
        for (auto iter = mesh.vertices_begin(); iter != mesh.vertices_end(); ++iter, ++i) {
            mesh.property(weights_, *iter) = weights[i];
        }
    }

private:
    OpenMesh::VPropHandleT<float> weights_;
};

typedef ModWeightedQuadricT<OMMesh>::Handle HModWeightedQuadric;

#endif

/** Convert mesh to OpenMesh data structure and return mesh extents (2D bounding
 *  box)
 */
math::Extents2 toOpenMesh(const geometry::Mesh &mesh, OMMesh& omMesh)
{
    math::Extents2 e(math::InvalidExtents{});

    // create OpenMesh vertices
    std::vector<OMMesh::VertexHandle> handles;
    handles.reserve(mesh.vertices.size());
    int i = 0;

    for (const auto& v : mesh.vertices) {
        handles.emplace_back(
            omMesh.add_vertex(OMMesh::Point(v(0), v(1), v(2))) );
        update(e, v);

        // copy classification data only if present
        if (mesh.vertecesClass.size()) {
            omMesh.data(handles.back()).classLabel = mesh.vertecesClass[i];
            ++i;
        }
    }

    // create OpenMesh faces
    for (const geometry::Face& face : mesh.faces)
    {
        OMMesh::VertexHandle face_handles[3] = {
            handles[face.a], handles[face.b], handles[face.c]
        };
        omMesh.add_face(face_handles, 3);
    }

    return e;
}

void fromOpenMesh(const OMMesh& omMesh, geometry::Mesh& mesh)
{
    // create our vertices
    for (auto v_it = omMesh.vertices_begin();
              v_it != omMesh.vertices_end();
              ++v_it)
    {
        const auto& pt = omMesh.point(v_it.handle());
        mesh.vertices.emplace_back(pt[0], pt[1], pt[2]);
    }

    // create our faces
    for (auto f_it = omMesh.faces_begin();
              f_it != omMesh.faces_end();
              ++f_it)
    {
        auto fv_it(omMesh.cfv_iter(f_it.handle()));
        int index[3], *p = index;
        for ( ; fv_it; ++fv_it) {
            *p++ = fv_it.handle().idx();
        }
        mesh.faces.emplace_back(index[0], index[1], index[2]);
    }
}

void lockBorder(OMMesh &omMesh, bool inner, bool outer){
    // get bounding box
    double box[2][2] = { {INFINITY, -INFINITY}, {INFINITY, -INFINITY} };

    for (auto v_it = omMesh.vertices_begin();
              v_it != omMesh.vertices_end();  ++v_it)
    {
        const auto& pt = omMesh.point(v_it.handle());
        for (int i = 0; i < 2; i++) {
            if (pt[i] < box[i][0]) box[i][0] = pt[i];
            if (pt[i] > box[i][1]) box[i][1] = pt[i];
        }
    }

    double eps = std::max(box[0][1]-box[0][0], box[1][1]-box[1][0])/(1<<10);
    double epsNormal = 0.001;

    for (auto e_it = omMesh.edges_begin();
              e_it != omMesh.edges_end();  ++e_it)
    {
        if(omMesh.is_boundary(e_it))
        {
            //LOG(info3)<<"--------";
            auto hfhandle(omMesh.halfedge_handle(e_it,0));

            auto from(omMesh.from_vertex_handle(hfhandle));
            auto to(omMesh.to_vertex_handle(hfhandle));

            //check if both points lie on the same border
            auto p1(omMesh.point(from));
            auto p2(omMesh.point(to));
            math::Point3 curNormal(
                math::normalize(math::Point3(p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2])));
            //check if previous edge or next is in the same direction
            bool dirChangeFrom(true);
            bool dirChangeTo(true);
            for(auto vih_it = omMesh.vih_iter(to); vih_it; ++vih_it) {
                if(omMesh.is_boundary(vih_it)){
                    //auto prevhf = omMesh.prev_halfedge_handle(hfhandle);
                    auto pfrom(omMesh.from_vertex_handle(vih_it));
                    auto pto(omMesh.to_vertex_handle(vih_it));
                    auto pp1(omMesh.point(pfrom));
                    auto pp2(omMesh.point(pto));
                    math::Point3 prevNormal(
                    math::normalize(math::Point3(pp2[0]-pp1[0], pp2[1]-pp1[1], pp2[2]-pp1[2])));
                    dirChangeTo = std::abs(ublas::inner_prod(prevNormal,curNormal))<(1.0f-epsNormal);
                }
            }
            for(auto voh_it = omMesh.voh_iter(from); voh_it; ++voh_it) {
                if(omMesh.is_boundary(voh_it)){
                    //auto nexthf = omMesh.next_halfedge_handle(hfhandle);
                    auto nfrom(omMesh.from_vertex_handle(voh_it));
                    auto nto(omMesh.to_vertex_handle(voh_it));
                    auto np1(omMesh.point(nfrom));
                    auto np2(omMesh.point(nto));
                    math::Point3 nextNormal(
                        math::normalize(math::Point3(np2[0]-np1[0], np2[1]-np1[1], np2[2]-np1[2])));
                    dirChangeFrom = std::abs(ublas::inner_prod(nextNormal,curNormal))<(1.0f-epsNormal);
                }
            }

            bool p1Border[4];
            bool p2Border[4];

            p1Border[0] = std::abs(p1[0]-box[0][0])<eps;
            p1Border[1] = std::abs(p1[0]-box[0][1])<eps;
            p1Border[2] = std::abs(p1[1]-box[1][0])<eps;
            p1Border[3] = std::abs(p1[1]-box[1][0])<eps;

            p2Border[0] = std::abs(p2[0]-box[0][0])<eps;
            p2Border[1] = std::abs(p2[0]-box[0][1])<eps;
            p2Border[2] = std::abs(p2[1]-box[1][0])<eps;
            p2Border[3] = std::abs(p2[1]-box[1][0])<eps;

            bool onBorder = false;
            for (int i = 0; i < 4; i++) {
                onBorder = (onBorder || (p1Border[i]==p2Border[i] && p1Border[i]));
            }

            if((inner && !onBorder) || (outer && onBorder)){
                //if they don't lie on the border, lock them
                if(dirChangeFrom){
                    omMesh.status(from).set_locked(true);
                }

                if(dirChangeTo){
                    omMesh.status(to).set_locked(true);
                }
            }
        }
    }
}

void lockCorners(OMMesh &omMesh)
{
    // get bounding box
    double box[2][2] = { {INFINITY, -INFINITY}, {INFINITY, -INFINITY} };

    for (auto v_it = omMesh.vertices_begin();
              v_it != omMesh.vertices_end();  ++v_it)
    {
        const auto& pt = omMesh.point(v_it.handle());
        for (int i = 0; i < 2; i++) {
            box[i][0] = std::min(double(pt[i]), box[i][0]);
            box[i][1] = std::max(double(pt[i]), box[i][1]);
        }
    }

    // calculate smallest distances to the four corners of the bounding box
    double dist[2][2] = { {INFINITY, INFINITY}, {INFINITY, INFINITY} };

    for (auto v_it = omMesh.vertices_begin();
              v_it != omMesh.vertices_end();  ++v_it)
    {
        const auto& pt = omMesh.point(v_it.handle());

        for (int i = 0; i < 2; i++)
        for (int j = 0; j < 2; j++)
        {
            double d = std::hypot(pt[0] - box[0][i], pt[1] - box[1][j]);
            dist[i][j] = std::min(d, dist[i][j]);
        }
    }

    omMesh.request_vertex_status();

    // lock vertices closest to the corners
    // (note: multiple vertices can have the same minimum distance to a corner)
    for (auto v_it = omMesh.vertices_begin();
              v_it != omMesh.vertices_end();  ++v_it)
    {
        const auto& pt = omMesh.point(v_it.handle());

        for (int i = 0; i < 2; i++)
        for (int j = 0; j < 2; j++)
        {
            double d = std::hypot(pt[0] - box[0][i], pt[1] - box[1][j]);
            if (std::abs(d - dist[i][j]) < 1e-12)
            {
                auto handle = v_it.handle();
                LOG(info1) << "Locking corner vertex " << handle.idx();
                omMesh.status(handle).set_locked(true);
            }
        }
    }
}

math::Extents2 prepareMeshImpl(OMMesh &omMesh, const Mesh& mesh
                               , const SimplifyOptions &options)
{
    math::Extents2 me;
    me = toOpenMesh(mesh, omMesh);
    omMesh.update_normals();

    // lock the corner and/or border vertices based on flags
    if (options.check(SimplifyOption::CORNERS)) {
        lockCorners(omMesh);
    }
    if (options.check(SimplifyOption::INNERBORDER)) {
        lockBorder(omMesh, true, false);
    }
    if (options.check(SimplifyOption::OUTERBORDER)) {
        lockBorder(omMesh, false, true);
    }
    // return accumulated extents
    return me;
}

math::Extents2 prepareMesh(OMMesh &omMesh, const Mesh& mesh
                           , const SimplifyOptions &options)
{
    // preprocess mesh - remove non-manifold edges
    if (options.check(SimplifyOption::RMNONMANIFOLDEDGES)) {
        auto pmesh(geometry::removeNonManifoldEdges(mesh));
        return prepareMeshImpl(omMesh, *pmesh, options);
    } else {
        return prepareMeshImpl(omMesh, mesh, options);
    }
}

math::Extents2 prepareMesh(OMMesh &omMesh, Mesh&& mesh
                           , const SimplifyOptions &options)
{
    // preprocess mesh - remove non-manifold edges
    if (options.check(SimplifyOption::RMNONMANIFOLDEDGES)) {
        auto pmesh(geometry::removeNonManifoldEdges(std::move(mesh)));
        return prepareMeshImpl(omMesh, *pmesh, options);
    } else {
        return prepareMeshImpl(omMesh, mesh, options);
    }
}

void prepareDecimator(Decimator &decimator
                      , const SimplifyOptions &options)
{
    if (options.alternativeVertices()) {
        if (options.concaveVertexModifier()) {
            LOGTHROW (err3, std::runtime_error) <<
                "Concave/Convex modification is not "
                "implemented for alternative verteces";
        }
        HModQuadricHybrid hModQuadricHybrid;
        decimator.add(hModQuadricHybrid);
        decimator.module(hModQuadricHybrid)
            .setAlternativeVertices(options.alternativeVertices());

        if (options.maxError()) {
            decimator.module(hModQuadricHybrid)
                .set_max_err(*options.maxError(), false);
        }
    } else if (options.concaveVertexModifier()){
#if OM_VERSION >= 0x60000
        // collapse priority based on vertex error quadric
        // adjusted by convexity of the vertex
        HModQuadricConvex hModQuadricConvex;
        decimator.add(hModQuadricConvex);
        decimator.module(hModQuadricConvex).
            set_concave_vertex_modifier(*options.concaveVertexModifier());
        if (options.maxError()) {
            decimator.module(hModQuadricConvex)
                .set_max_err(*options.maxError(), false);
        }
#else
        LOGTHROW (err3, std::runtime_error) <<
            "Concave/Convex decimater is not available on this system.";
#endif
    } else if (options.vertexWeights()) {
        HModWeightedQuadric hModWeightedQuadric;
        decimator.add(hModWeightedQuadric);
        decimator.module(hModWeightedQuadric).setWeights(*options.vertexWeights());
        if (options.maxError()) {
            decimator.module(hModWeightedQuadric).set_max_err(*options.maxError(), false);
        }
    } else {
        // collapse priority based on vertex error quadric
        HModQuadric hModQuadric;
        decimator.add(hModQuadric);
        if (options.maxError()) {
            decimator.module(hModQuadric)
                .set_max_err(*options.maxError(), false);
        }
    }

    // apply normal flipping prevention (if configured)
    if (options.check(SimplifyOption::PREVENTFACEFLIP)) {
        HModNormalFlippingT hModNormalFlippingT;
        decimator.add(hModNormalFlippingT);
        decimator.module(hModNormalFlippingT).set_max_normal_deviation(90);
    }

    // apply aspect ratio (if configured)
    if (options.minAspectRatio()) {
        HModAspectRatioT h;
        decimator.add(h);
        decimator.module(h).set_aspect_ratio(*options.minAspectRatio());
    }

    // apply max edge lenght (if configured)
    if (options.maxEdgeLength()) {
        HModEdgeLengthT h;
        decimator.add(h);
        decimator.module(h).set_edge_length(*options.maxEdgeLength());
    }
}

} // namespace

Mesh::pointer simplify(const Mesh &mesh, int faceCount
                       , const SimplifyOptions &options)
{
    OMMesh omMesh;
    prepareMesh(omMesh, mesh, options);

    Decimator decimator(omMesh);
    prepareDecimator(decimator, options);
    decimator.initialize();

    decimator.decimate_to_faces(0, faceCount);
    omMesh.garbage_collection();

    auto newMesh(std::make_shared<geometry::Mesh>());
    fromOpenMesh(omMesh, *newMesh);
    return newMesh;
}

void simplifyInPlace(Mesh& mesh, int faceCount, const SimplifyOptions& options) {
    OMMesh omMesh;
    prepareMesh(omMesh, std::move(mesh), options);

    Decimator decimator(omMesh);
    prepareDecimator(decimator, options);
    decimator.initialize();

    decimator.decimate_to_faces(0, faceCount);
    omMesh.garbage_collection();

    mesh.vertices.clear();
    mesh.faces.clear();
    fromOpenMesh(omMesh, mesh);
}

#if OM_VERSION >= 0x60000

Mesh::pointer simplifyToError(const Mesh &mesh, double maxErr
                            , const SimplifyOptions &options)
{
    OMMesh omMesh;

    prepareMesh(omMesh, mesh, options);

    Decimator decimator(omMesh);
    HModQuadric hModQuadric; // collapse priority based on vertex error quadric
    decimator.add(hModQuadric);
    decimator.module( hModQuadric ).set_max_err(maxErr, false);

    if (options.classBasedPlanarisation()) {
        HModClassRestrictT hModClassRestrictT;
        decimator.add (hModClassRestrictT);
        // Some hardcoded classes allowed for decimation
        decimator.module(hModClassRestrictT).allow(13);
        decimator.module(hModClassRestrictT).allow(14);
        decimator.module(hModClassRestrictT).allow(15);
        decimator.module(hModClassRestrictT).allow(16);
        decimator.module(hModClassRestrictT).allow(18);
        decimator.module(hModClassRestrictT).allow(9);
    }
    decimator.initialize();
    decimator.decimate_to_faces(0, 0);
    omMesh.garbage_collection();

    auto newMesh(std::make_shared<geometry::Mesh>());
    fromOpenMesh(omMesh, *newMesh);
    return newMesh;
}
#endif

namespace {

inline std::size_t gridIndexImpl(const math::Size2ll &size
                                 , const math::Point2 origin
                                 , const math::Size2f tileSize
                                 , double x, double y)
{
    long long xx((x - origin(0)) / tileSize.width);
    long long yy((y - origin(1)) / tileSize.height);

    // clamp to grid (values can be outside of grid in case non-integer
    // tileSize
    xx = math::clamp<long long>(xx, 0LL, size.width - 1);
    yy = math::clamp<long long>(yy, 0LL, size.height -1);

    return xx + yy * size.width;
}

struct Tiling {
    math::Size2ll size;
    math::Point2 origin;
    math::Size2f tileSize;
    FacesPerCell::Functor facesPerCell;

    Tiling() : tileSize() {}

    Tiling(const math::Size2ll &size, const math::Point2 &origin
           , const math::Size2f &tileSize
           , const FacesPerCell::Functor &facesPerCell)
        : size(size), origin(origin), tileSize(tileSize)
        , facesPerCell(facesPerCell)
    {}

    inline std::size_t gridIndex(double x, double y) const {
        return gridIndexImpl(size, origin, tileSize, x, y);
    };

    inline math::Point2 tileLowerLeft(int index){
        int y = index / size.width;
        int x = index % size.width;

        return math::Point2(x * tileSize.width + origin(0)
                            , y * tileSize.height + origin(1));
    }

    inline std::vector<std::size_t> intersectingCells(math::Point2 triangle[3])
        const
    {
        std::vector<std::size_t> result;

        for(uint x = 0; x < size.width; ++x){
            for(uint y = 0; y < size.height; ++y){
                math::Point2 offset(x * tileSize.width
                                    , y * tileSize.height);
                math::Point2 ll = origin + offset;
                math::Point2 ur = ll + math::Point2
                    (tileSize.width, tileSize.height);

                if(math::triangleRectangleCollision(triangle,ll,ur)){
                    result.push_back(x + y * size.width);
                }
            }
        }
        return result;
    };

    /** Calculate maximum face count in all cells of grid. If real value is
     *  lower than maximum, set maximum to real value.
     */
    std::vector<std::size_t> getMax(const std::vector<std::size_t> &current)
        const
    {
        FacesPerCell fpc(size, origin, tileSize);
        facesPerCell(fpc);

        // merge minimum values from current and expected face counts
        auto out(fpc.get());
        auto icurrent(current.begin());
        for (auto &value : out) {
            value = std::min(value, *icurrent++);
        }

        // done
        return out;
    }

    static OpenMesh::MPropHandleT<Tiling> Property;
};

OpenMesh::MPropHandleT<Tiling> Tiling::Property;

template <typename MeshType>
class ModGrid : public OpenMesh::Decimater::ModBaseT<MeshType>
{
public:
    DECIMATING_MODULE(ModGrid, MeshType, Grid);

    ModGrid(MeshType &mesh)
        : Base(mesh, true) // true -> binary module!
        , mesh_(mesh), tiling_()
    {
        mesh_.add_property(cells_);
    }

    ~ModGrid() {
        mesh_.remove_property(cells_);
        //stat();
    }

    virtual void initialize() UTILITY_OVERRIDE {
        auto &prop(mesh_.mproperty(Tiling::Property));
        if (!prop.n_elements()) {
            LOGTHROW(err2, std::runtime_error)
                << "Tiling property not set at mesh.";
        }
        tiling_ = &prop[0];

        calculateGrid();
    }

    virtual float collapse_priority(const CollapseInfo &ci) UTILITY_OVERRIDE {
        // can be both faces removed?
        if (canRemove(ci.fl) && canRemove(ci.fr)) {
            return Base::LEGAL_COLLAPSE;
        }

        return Base::ILLEGAL_COLLAPSE;
    }

    virtual void preprocess_collapse(const CollapseInfo &ci) UTILITY_OVERRIDE {
        removed(ci.fl);
        removed(ci.fr);
    }

    virtual void postprocess_collapse(const CollapseInfo &ci) UTILITY_OVERRIDE
    {
        // traverse all remaining faces of non-removed vertex
        for (auto fi(mesh_.vf_begin(ci.v1)), fe(mesh_.vf_end(ci.v1));
             fi != fe; ++fi)
        {
            const auto f(fi.handle());
            update(f, cell(f), barycenterCell(f));
        }
    }

    std::size_t desiredCount() const {
        return std::accumulate(max_.begin(), max_.end(), std::size_t(0));
    }

private:
    typedef typename MeshType::FaceHandle FaceHandle;
    typedef typename MeshType::Scalar Scalar;

    void stat() const {
        auto ioriginal(original_.begin());
        auto icurrent(current_.begin());
        for (auto max : max_) {
            LOG(info2) << *ioriginal++ << " -> " << *icurrent++
                       << " (" << max << ")";
        }
    }

    void calculateGrid() {
        current_.clear();
        current_.resize(area(tiling_->size));

        for (const auto &f : mesh_.faces()) {
            auto cellIndex(barycenterCell(f));

            // assign cell index to face
            cell(f) = cellIndex;

            // one more face at given cell
            ++current_[cellIndex];
        }

        // get face count limits
        max_ = tiling_->getMax(current_);

        // for statistics
        original_.assign(current_.begin(), current_.end());
    }

    inline std::size_t barycenterCell(FaceHandle f) const {
        // calculate barycenter of face (i.e. average x and y coordinate)
        math::Point2 p;
        int valence(0);
        for (auto vi(mesh_.cfv_iter(f)); vi; ++vi) {
            const auto &v(mesh_.point(vi.handle()));
            p(0) += v[0];
            p(1) += v[1];
            ++valence;
        }

        p(0) /= valence;
        p(1) /= valence;

        return tiling_->gridIndex(p(0), p(1));
    }

    void removed(FaceHandle fh) {
        if (fh.is_valid()) {
            --current_[cell(fh)];
        }
    }

    void update(FaceHandle fh, std::size_t oldCell, std::size_t newCell) {
        if (!fh.is_valid() || (oldCell == newCell)) {
            return;
        }

        // new cell index, update
        --current_[oldCell];
        ++current_[newCell];
        cell(fh) = newCell;
    }

    inline bool canRemove(FaceHandle fh) const {
        if (!fh.is_valid()) { return true; }

        auto index(cell(fh));
        return (current_[index] > max_[index]);
    }

    inline Scalar& cell(FaceHandle fh) {
        return mesh_.property(cells_, fh);
    }

    inline const Scalar& cell(FaceHandle fh) const {
        return mesh_.property(cells_, fh);
    }

    MeshType &mesh_;
    Tiling *tiling_;
    std::vector<std::size_t> current_;
    std::vector<std::size_t> max_;
    std::vector<std::size_t> original_;
    OpenMesh::FPropHandleT<Scalar> cells_;
};

 /*
 * OpenMesh module for decimation mesh in grid.
 * Each cell has defined maximal face count and
 * once this face count is reached, incident faces
 * can't be decimated.
 */
template <typename MeshType>
class ModGrid2 : public OpenMesh::Decimater::ModBaseT<MeshType>
{
public:
    DECIMATING_MODULE(ModGrid2, MeshType, Grid);

    ModGrid2(MeshType &mesh)
        : Base(mesh, true) // true -> binary module!
        , mesh_(mesh), tiling_()
    {
        mesh_.add_property(cells_);
    }

    ~ModGrid2() {
        mesh_.remove_property(cells_);
        //stat();
    }

    virtual void initialize() UTILITY_OVERRIDE {
        auto &prop(mesh_.mproperty(Tiling::Property));
        if (!prop.n_elements()) {
            LOGTHROW(err2, std::runtime_error)
                << "Tiling property not set at mesh.";
        }
        tiling_ = &prop[0];

        calculateGrid();
    }

    virtual float collapse_priority(const CollapseInfo &ci) UTILITY_OVERRIDE {
        // can be both faces removed?
        if (canRemove(ci.fl) && canRemove(ci.fr)) {
            return Base::LEGAL_COLLAPSE;
        }

        return Base::ILLEGAL_COLLAPSE;
    }

    virtual void preprocess_collapse(const CollapseInfo &ci) UTILITY_OVERRIDE {
        removed(ci.fl);
        removed(ci.fr);
    }

    virtual void postprocess_collapse(const CollapseInfo &ci) UTILITY_OVERRIDE
    {
        // traverse all remaining faces of non-removed vertex
        for (auto fi(mesh_.vf_begin(ci.v1)), fe(mesh_.vf_end(ci.v1));
             fi != fe; ++fi)
        {
            const auto f(fi.handle());
            update(f);
        }
    }

    std::size_t desiredCount() const {
        return std::accumulate(max_.begin(), max_.end(), std::size_t(0));
    }

private:
    typedef typename MeshType::FaceHandle FaceHandle;
    typedef typename MeshType::Scalar Scalar;
    typedef typename std::vector<std::size_t> CellList;
    typedef typename std::vector<std::set<FaceHandle>> CellFaces;

    void setCellVerticesAsFeatures(int cellIndex) const{
        for(const auto &face : cellFaces_[cellIndex]){
            for (auto vi(mesh_.cfv_iter(face)); vi; ++vi) {
                mesh_.status(vi.handle()).set_feature(true);
            }
        }
    }

    void printCells() const {
        LOG(info2) << "Cells count:";
        for (uint i=0; i<current_.size(); ++i) {
            LOG(info2) << tiling_->tileLowerLeft(i)<<" - "<<current_[i];
        }
    }

    void stat() const {
        auto ioriginal(original_.begin());
        auto icurrent(current_.begin());
        for (auto max : max_) {
            LOG(info2) << *ioriginal++ << " -> " << *icurrent++
                       << " (" << max << ")";
        }
        printCells();
    }

    void calculateGrid() {
        current_.clear();
        current_.resize(area(tiling_->size));
        cellFaces_.clear();
        cellFaces_.resize(area(tiling_->size));

        for (const auto &f : mesh_.faces()) {
            cellList(f).clear();
            update(f);
        }

        // get face count limits
        max_ = tiling_->getMax(current_);

        // for statistics
        original_.assign(current_.begin(), current_.end());
    }

    void removed(FaceHandle fh) {
        if (fh.is_valid()) {
            for(auto &cell : cellList(fh)){
                --current_[cell];
                cellFaces_[cell].erase(fh);
            }
        }
    }

    void update(FaceHandle fh) {
        if (!fh.is_valid()) {
            return;
        }

        // decrement facecount in old cells
        for(auto &cell : cellList(fh)) {
            --current_[cell];
            cellFaces_[cell].erase(fh);
        }
        // recalculate cells
        calculateCells(fh);

        // increment facecount in new cells
        for(auto &cell : cellList(fh)) {
            ++current_[cell];
            cellFaces_[cell].insert(fh);
        }
    }

    inline void calculateCells(FaceHandle fh){
        math::Point2 vertices[3];
        uint vc = 0;
        for (auto vi(mesh_.cfv_iter(fh)); vi; ++vi) {
            const auto &v(mesh_.point(vi.handle()));
            vertices[vc](0) =  v[0];
            vertices[vc](1) =  v[1];
            vc++;
        }
        cellList(fh) = tiling_->intersectingCells(vertices);
    }

    inline bool canRemove(FaceHandle fh) const {
        if (!fh.is_valid()) { return true; }
        for(auto &cell : cellList(fh)){
            if(current_[cell] <= max_[cell]){
                setCellVerticesAsFeatures(cell);
                return false;
            }
        }
        return true;
    }

    inline CellList& cellList(FaceHandle fh) {
        return mesh_.property(cells_, fh);
    }

    inline const CellList& cellList(FaceHandle fh) const {
        return mesh_.property(cells_, fh);
    }

    MeshType &mesh_;
    Tiling *tiling_;
    std::vector<std::size_t> original_;
    std::vector<std::size_t> current_;
    std::vector<std::size_t> max_;
    OpenMesh::FPropHandleT<CellList> cells_;
    CellFaces cellFaces_;
};

} // namespace

/** Converts coordinate to grid defined by reference point and cell size.
 *  Coordinates not on grid are rounded down.
 */
double gridExtentsDown(double value, double origin, double size)
{
    return std::floor((value - origin) / size) * size + origin;
}

/** Converts coordinate to grid defined by reference point and cell size.
 *  Coordinates not on grid are rounded up.
 */
double gridExtentsUp(double value, double origin, double size)
{
    return std::ceil((value - origin) / size) * size + origin;
}

math::Extents2 gridExtents(const math::Extents2 &extents
                           , const math::Point2 &alignment
                           , const math::Size2f &cellSize)
{
    return {
        gridExtentsDown(extents.ll(0), alignment(0), cellSize.width)
        , gridExtentsDown(extents.ll(1), alignment(1), cellSize.height)
        , gridExtentsUp(extents.ur(0), alignment(0), cellSize.width)
        , gridExtentsUp(extents.ur(1), alignment(1), cellSize.height)
    };
}

std::size_t FacesPerCell::cellIndex(double x, double y) const
{
    return gridIndexImpl(gridSize_, origin_, cellSize_, x, y);
}

math::Extents2 FacesPerCell::cellExtents(std::size_t index) const
{
    // convert linear index to grid index
    auto i(index % gridSize_.width);
    auto j(index / gridSize_.width);

    // return extents
    return math::Extents2(origin_(0) + i * cellSize_.width
                          , origin_(1) + j * cellSize_.height
                          , origin_(0) + (i + 1) * cellSize_.width
                          , origin_(1) + (j + 1) * cellSize_.height);
}

Mesh::pointer simplifyInGrid(const Mesh &mesh, const math::Point2 &alignment
                             , const math::Size2f &cellSize
                             , const FacesPerCell::Functor &facesPerCell
                             , const SimplifyOptions &options)
{
    LOG(info2) << "[simplify] alignment: "
               << std::setprecision(15) << alignment;
    LOG(info2) << "[simplify] cellSize: " << cellSize;

    // convert to openmesh structure
    OMMesh omMesh;
    math::Extents2 me(prepareMesh(omMesh, mesh, options));

    // calculate grid extents
    const auto ge(gridExtents(me, alignment, cellSize));

    // grid origin
    const auto gorigin(ge.ll);

    // grid size
    const math::Size2ll gsize
        ((long long) (std::round(((ge.ur(0) - ge.ll(0)) / cellSize.width)))
         , (long long)(std::round((ge.ur(1) - ge.ll(1)) / cellSize.width)));

    LOG(info2) << "[simplify] mesh extents: " << me;
    LOG(info2) << "[simplify] gridded mesh extents: "
               << std::setprecision(15) << ge;
    LOG(info2) << "[simplify] grid size: " << gsize;
    LOG(info2) << "[simplify] grid origin: " << gorigin;

    // create and add tiling as a mesh property to mesh
    omMesh.add_property(Tiling::Property);
    omMesh.mproperty(Tiling::Property).push_back();
    omMesh.mproperty(Tiling::Property)[0]
        = Tiling(gsize, gorigin, cellSize, facesPerCell);

    // create and prepare decimator
    Decimator decimator(omMesh);
    prepareDecimator(decimator, options);

    // add decimator grid module
    ModGrid<OMMesh>::Handle hModGrid;
    decimator.add(hModGrid);

    // initialized
    decimator.initialize();

    auto fc(decimator.module(hModGrid).desiredCount());
    LOG(info2) << "[simplify] Simplifying mesh to " << fc << " faces.";
    // simplifying to 0 faces, simplification will stop based on grid constraints
    decimator.decimate_to_faces(0, 0);

    omMesh.garbage_collection();

    auto newMesh(std::make_shared<geometry::Mesh>());
    fromOpenMesh(omMesh, *newMesh);
    return newMesh;
}

} // namespace geometry