//	Copyright (c) 2016, Philipp Gergen, Andre Gaschler
//	All rights reserved.
//
//	Redistribution and use in source and binary forms, with or without
// modification,
//	are permitted provided that the following conditions are met:
//
//	* Redistributions of source code must retain the above copyright notice,
// this
//	  list of conditions and the following disclaimer.
//
//	* Redistributions in binary form must reproduce the above copyright
// notice, this
//	  list of conditions and the following disclaimer in the documentation
// and/or
//	  other materials provided with the distribution.
//
//	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 "boundingmesh/SegmenterDownsampling.h"
#include "boundingmesh/Segmenter.h"

#include <math.h>
#include <fstream>
#include <iostream>
#include <stdexcept>

#include <Eigen/SVD>

#ifdef CGAL_AVAILABLE
#include "CGAL/convex_hull_2.h"
#include "CGAL/Exact_predicates_inexact_constructions_kernel.h"

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point_2;
#endif

#define MAX(a, b) (((a) > (b)) ? (a) : (b))
#define MIN(a, b) (((a) < (b)) ? (a) : (b))
#define MAX_Real (1.79769e+308)

namespace boundingmesh {

// Returns the unit vector in the direction of the current split
Vector3 Split::directionVector() {
  if (dimension == 0) {
    return Vector3(1, 0, 0);
  } else if (dimension == 1) {
    return Vector3(0, 1, 0);
  } else {
    return Vector3(0, 0, 1);
  }
}

void VoxelSubset::mergeSplit(AppliedSplit split) {
  // Filter redundant splits:
  // Each dimension can have at most 2 splits     ....|->....<-|...
  // AppliedSplit::mergeSplits determines which plane to keep/discard
  bool add = true;
  for (std::vector<AppliedSplit>::iterator it = splits_.begin();
       it != splits_.end();) {
    int result = AppliedSplit::mergeSplitsMerge(*it, split);
    switch (result) {
      case 0:
        ++it;
        break;
      case 1:
        add = false;
        ++it;
        break;
      case 2:
        it = splits_.erase(it);
        break;
    }
  }

  if (add) splits_.push_back(split);
}

int AppliedSplit::mergeSplitsMerge(const AppliedSplit& a,
                                   const AppliedSplit& b) {
  /*
   Determines the merge action for 2 splits
   0 -> keep both
   1 -> keep only a
   2 -> keep only b
   */
  if (a.split.dimension != b.split.dimension || a.direction != b.direction)
    return 0;
  else if (a.direction && b.direction) {
    // both splits are above, take higher
    if (a.split.index > b.split.index)
      return 2;
    else
      return 1;
  } else {
    // both splits are below, take lower
    if (a.split.index > b.split.index)
      return 1;
    else
      return 2;
  }
}

// Merges two voxel subsets together by adding the voxels form both sets to one
// set and recalculating the convex hull
VoxelSubset VoxelSubset::merge(VoxelSubset other) {
  VoxelSubset ret(*this, true);
  for (int i = 0; i < indices_.size(); ++i) {
    ret.addVoxel(indices_[i]);
  }

  for (int i = 0; i < other.indices_.size(); i++) {
    ret.addVoxel(other.indices_[i]);
  }

  for (int i = 0; i < 3; i++) {
    bool hasDirection = false;
    bool hasNotDirection = false;
    for (int j = 0; j < ret.splits_.size(); j++) {
      if (ret.splits_[j].split.dimension == i) {
        if (ret.splits_[j].direction) {
          hasDirection = true;
        } else {
          hasNotDirection = true;
        }
      }
    }
    if (!hasDirection) {
      Split split(i, -10);
      ret.splits_.push_back(AppliedSplit(split, true));
    }
    if (!hasNotDirection) {
      Split split(i, voxels_->resolution(i) + 10);
      ret.splits_.push_back(AppliedSplit(split, false));
    }
  }

  for (int i = 0; i < other.splits_.size(); i++) {
    // splits_.push_back(other.splits_[i]);
    ret.mergeSplit(other.splits_[i]);
  }

  ret.calculateConvexHullVoxel();
  return ret;
}

// Partitions the given voxel subset into two parts according the the provided
// split
// This also calculates the convex hulls for both new parts
std::vector<VoxelSubset> VoxelSubset::partitionVoxel(Split split) {
  return partitionVoxel(split, true);
}

std::vector<VoxelSubset> VoxelSubset::partitionVoxel(Split split,
                                                     bool calculateHulls) {
  // Partition voxels of a set according to a plane
  std::vector<VoxelSubset> subsets;

  subsets.push_back(VoxelSubset(*this, true));
  subsets.push_back(VoxelSubset(*this, true));

  AppliedSplit above(split, true);
  AppliedSplit below(split, false);

  subsets[0].addSplit(above);
  subsets[1].addSplit(below);

  for (int i = 0; i < indices_.size(); ++i) {
    const Voxel& voxel = voxels_->voxel(indices_[i]);
    if (split.test(voxel)) {
      subsets[0].addVoxel(indices_[i]);
    } else {
      subsets[1].addVoxel(indices_[i]);
    }
  }
  // std::cout<<"split "<<split.dimension<<" "<<split.index<<" Values:
  // "<<subsets[0].evaluate()<<" "<<subsets[1].evaluate()<<std::endl;
  if (calculateHulls) {
    subsets[0].calculateConvexHullVoxel();
    subsets[1].calculateConvexHullVoxel();
  }
  return subsets;
}
// Returns a subset of all voxels from the current voxel subset which
// only contains all voxels from the surface of the original mesh
VoxelSubset VoxelSubset::getSurfaceSubset() const {
  VoxelSubset ret = VoxelSubset(*this, true);
  for (int i = 0; i < indices_.size(); ++i) {
    if (voxels_->voxel(indices_[i]).type() == SURFACE) {
      ret.addVoxel(indices_[i]);
    }
  }
  return ret;
}

int VoxelSubset::voxelCount() const { return indices_.size(); }

// Returns the number of voxels which are located at the surface
// of the original mesh in the current voxel subset
int VoxelSubset::surfaceVoxelCount() const {
  int ret = 0;
  for (int i = 0; i < indices_.size(); i++) {
    if (voxels_->voxel(indices_[i]).type() == SURFACE) {
      ret++;
    }
  }
  return ret;
}

// Function calculating the area of the convex hull using
// http://alienryderflex.com/polygon_area/
Real convexHull2DArea(std::vector<Point_2> points) {
  Real area = 0.0;
  int j = points.size() - 1;
  for (int i = 0; i < points.size(); i++) {
    area += (points[j].x() + points[i].x()) * (points[j].y() - points[i].y());
    j = i;
  }

  return area;
}

// Calculates the area of the 2D convex hull of a voxel set not looking
// at the specified direction (first return value)
// Additionally the area of the inner pixels is calculated
void write2DHullWRL(std::vector<Point_2> points, std::vector<Point_2> hull,
                    std::string filename) {
  std::ofstream file(filename.c_str());
  file << "#VRML V2.0 utf8" << std::endl << std::endl;
  file << "#Created by boundingmesh" << std::endl << std::endl;

  file << "Transform {" << std::endl;
  file << "\t translation 0 0 0" << std::endl;
  // file << "\t scale " << subset.volume() << " " << voxels_->voxelSize()
  //		<< " " << voxels_->voxelSize() << std::endl;
  file << "\t children [ " << std::endl;
  file << "\t\tShape {" << std::endl;
  file << "\t\t\tgeometry PointSet { " << std::endl;
  file << "\t\t\t\tcoord Coordinate { " << std::endl;

  file << "\t\t\t\t\tpoint [" << std::endl;
  for (unsigned int i = 0; i < points.size(); ++i) {
    file << "\t\t\t\t\t\t" << points[i].x() << " " << points[i].y() << " 0,"
         << std::endl;
  }
  file << "\t\t\t\t\t]" << std::endl;
  file << "\t\t\t\t}" << std::endl;
  file << "\t\t\t\tcolor Color {" << std::endl;
  file << "\t\t\t\t\tcolor [" << std::endl;
  for (unsigned int i = 0; i < points.size(); ++i) {
    file << "\t\t\t\t\t\t0 0 1," << std::endl;
  }
  file << "\t\t\t\t\t]" << std::endl;
  file << "\t\t\t\t}" << std::endl;
  file << "\t\t\t}" << std::endl;
  file << "\t\t}" << std::endl;

  file << "\t\tShape {" << std::endl;
  file << "\t\t\tgeometry IndexedLineSet { " << std::endl;
  file << "\t\t\t\tcoord Coordinate { " << std::endl;

  file << "\t\t\t\t\tpoint [" << std::endl;
  for (unsigned int i = 0; i < hull.size(); ++i) {
    file << "\t\t\t\t\t\t" << hull[i].x() << " " << hull[i].y() << " 0,"
         << std::endl;
  }
  file << "\t\t\t\t\t]" << std::endl;
  file << "\t\t\t\t}" << std::endl;
  file << "\t\t\t\tcolor Color {" << std::endl;
  file << "\t\t\t\t\tcolor [" << std::endl;
  for (unsigned int i = 0; i < hull.size(); ++i) {
    file << "\t\t\t\t\t\t1 0 0," << std::endl;
  }
  file << "\t\t\t\t\t]" << std::endl;
  file << "\t\t\t\t}" << std::endl;
  file << "\t\t\t\t\tcoordIndex [" << std::endl;
  for (unsigned int i = 0; i < hull.size() - 1; ++i) {
    file << "\t\t\t\t\t\t" << i << ", " << (i + 1) << ", -1," << std::endl;
  }
  file << "\t\t\t\t\t\t" << (hull.size() - 1) << ", 0, -1" << std::endl;
  file << "\t\t\t\t\t]" << std::endl;
  file << "\t\t\t\t\tcolorIndex [" << std::endl;
  for (unsigned int i = 0; i < hull.size(); ++i) {
    file << "\t\t\t\t\t\t" << i << ", " << std::endl;
  }
  file << "\t\t\t\t\t]" << std::endl;
  file << "\t\t\t}" << std::endl;
  file << "\t\t}" << std::endl;

  file << "\t]" << std::endl;
  file << "}";
}

std::pair<Real, Real> VoxelSubset::calculate2DConvexHullVolume(int direction) {
  // Determine directions of the convex hull
  int dir1 = (direction + 1) % 3;
  int dir2 = (direction + 2) % 3;

  ComputeBB();
  int len1 = bounding_box_max_[dir1] - bounding_box_min_[dir1] + 1;
  int len2 = bounding_box_max_[dir2] - bounding_box_min_[dir2] + 1;

  // Create and initialize new 2D array to store point positions
  int** points = new int*[len1];
  for (int i = 0; i < len1; i++) {
    points[i] = new int[len2];
    for (int j = 0; j < len2; j++) {
      points[i][j] = 0;
    }
  }

  // Fill the point position array
  for (int i = 0; i < indices_.size(); i++) {
    const Voxel& voxel = voxels_->voxel(indices_[i]);
    int x = voxel.coordinate(dir1) - bounding_box_min_[dir1];
    int y = voxel.coordinate(dir2) - bounding_box_min_[dir2];
    points[x][y] = 1;
  }

  // Calculate the real positions from the point array
  std::vector<Point_2> pointCoordinates;
  Vector3 origin = voxels_->origin();
  for (int i = 0; i < len1; i++) {
    for (int j = 0; j < len2; j++) {
      if (points[i][j] == 1) {
        double d1 = i * voxels_->voxelSize() + origin(dir1);
        double d2 = j * voxels_->voxelSize() + origin(dir1);

        pointCoordinates.push_back(Point_2(d1, d2));
      }
    }
  }
  // Delete unneeded points to prevent memory leaks
  for (int i = 0; i < len1; i++) {
    delete[] points[i];
  }
  delete[] points;

  // Calculate the convex hull and the area of it
  std::vector<Point_2> pointsHull;
  CGAL::convex_hull_2(pointCoordinates.begin(), pointCoordinates.end(),
                      std::back_inserter(pointsHull));
  // write2DHullWRL(pointCoordinates, pointsHull, "hull2d_" +
  // std::to_string(splits_.size()) + "_" + std::to_string(len1) + "_" +
  // std::to_string(len2) + ".wrl");
  return std::make_pair(
      fabs(convexHull2DArea(pointsHull)),
      pointCoordinates.size() * voxels_->voxelSize() * voxels_->voxelSize());
}

// Calculates the convex hull from voxel subset
// This uses one point for each of the 8 corner points of a voxel and
// then simply calls the convex hull algorithm
void VoxelSubset::calculateConvexHullVoxel() {
  std::vector<Vector3> points;
  bool useCenter = false;

  // Generate additional points within the volume
  // Required at intersection of 3 planes, within the volume
  for (int i = 0; i < splits_.size(); ++i)
    for (int j = i + 1; j < splits_.size(); ++j)
      for (int k = j + 1; k < splits_.size(); ++k) {
        if (splits_[i].split.dimension != splits_[j].split.dimension &&
            splits_[i].split.dimension != splits_[k].split.dimension &&
            splits_[j].split.dimension != splits_[k].split.dimension) {
          Vector3 voxel_pos;
          voxel_pos(splits_[i].split.dimension) = splits_[i].split.index;
          voxel_pos(splits_[j].split.dimension) = splits_[j].split.index;
          voxel_pos(splits_[k].split.dimension) = splits_[k].split.index;

          if (voxel_pos(0) < 0 || voxel_pos(0) > voxels_->resolution(0) ||
              voxel_pos(1) < 0 || voxel_pos(1) > voxels_->resolution(1) ||
              voxel_pos(2) < 0 || voxel_pos(2) > voxels_->resolution(2)) {
            continue;
          }

          bool neighbour_filled =
              voxels_->voxelAt(voxel_pos(0), voxel_pos(1), voxel_pos(2)) >= 0;
          if (voxel_pos(2) + 1 < voxels_->resolution(2))
            neighbour_filled = neighbour_filled ||
                               voxels_->voxelAt(voxel_pos(0), voxel_pos(1),
                                                voxel_pos(2) + 1) >= 0;
          if (voxel_pos(1) + 1 < voxels_->resolution(1))
            neighbour_filled = neighbour_filled ||
                               voxels_->voxelAt(voxel_pos(0), voxel_pos(1) + 1,
                                                voxel_pos(2)) >= 0;

          if (voxel_pos(1) + 1 < voxels_->resolution(1) &&
              voxel_pos(2) + 1 < voxels_->resolution(2))
            neighbour_filled = neighbour_filled ||
                               voxels_->voxelAt(voxel_pos(0), voxel_pos(1) + 1,
                                                voxel_pos(2) + 1) >= 0;
          if (voxel_pos(0) + 1 < voxels_->resolution(0))
            neighbour_filled = neighbour_filled ||
                               voxels_->voxelAt(voxel_pos(0) + 1, voxel_pos(1),
                                                voxel_pos(2)) >= 0;
          if (voxel_pos(0) + 1 < voxels_->resolution(0) &&
              voxel_pos(2) + 1 < voxels_->resolution(2))
            neighbour_filled = neighbour_filled ||
                               voxels_->voxelAt(voxel_pos(0) + 1, voxel_pos(1),
                                                voxel_pos(2) + 1) >= 0;
          if (voxel_pos(0) + 1 < voxels_->resolution(0) &&
              voxel_pos(1) + 1 < voxels_->resolution(1))
            neighbour_filled =
                neighbour_filled ||
                voxels_->voxelAt(voxel_pos(0) + 1, voxel_pos(1) + 1,
                                 voxel_pos(2)) >= 0;
          if (voxel_pos(0) + 1 < voxels_->resolution(0) &&
              voxel_pos(1) + 1 < voxels_->resolution(1) &&
              voxel_pos(2) + 1 < voxels_->resolution(2))
            neighbour_filled =
                neighbour_filled ||
                voxels_->voxelAt(voxel_pos(0) + 1, voxel_pos(1) + 1,
                                 voxel_pos(2) + 1) >= 0;
          // Consider interior if at least one neighbouring voxel is filled
          if (!neighbour_filled) {
            continue;
          }
          Vector3 offset;
          offset(splits_[i].split.dimension) =
              (splits_[i].split.index + 0.5) * voxels_->voxelSize();
          offset(splits_[j].split.dimension) =
              (splits_[j].split.index + 0.5) * voxels_->voxelSize();
          offset(splits_[k].split.dimension) =
              (splits_[k].split.index + 0.5) * voxels_->voxelSize();
          Vector3 new_point = voxels_->origin() + offset;
          points.push_back(new_point);
        }
      }

  Vector3 origin = voxels_->origin();

  if (useCenter) {
    for (int i = 0; i < indices_.size(); i++) {
      const Voxel& voxel = voxels_->voxel(indices_[i]);
      if (voxels_->voxel(indices_[i]).type() == SURFACE) {
        double xp = voxel.x() * voxels_->voxelSize() + origin(0);
        double yp = voxel.y() * voxels_->voxelSize() + origin(1);
        double zp = voxel.z() * voxels_->voxelSize() + origin(2);
        points.push_back(Vector3(xp, yp, zp));
      }
    }
  } else {
    for (int i = 0; i < indices_.size(); i++) {
      const Voxel& voxel = voxels_->voxel(indices_[i]);
      if (voxels_->voxel(indices_[i]).type() == SURFACE) {
        double xp = (voxel.x() + 0.5) * voxels_->voxelSize() + origin(0);
        double yp = (voxel.y() + 0.5) * voxels_->voxelSize() + origin(1);
        double zp = (voxel.z() + 0.5) * voxels_->voxelSize() + origin(2);
        double xm = (voxel.x() - 0.5) * voxels_->voxelSize() + origin(0);
        double ym = (voxel.y() - 0.5) * voxels_->voxelSize() + origin(1);
        double zm = (voxel.z() - 0.5) * voxels_->voxelSize() + origin(2);
        points.push_back(Vector3(xp, yp, zp));
        points.push_back(Vector3(xp, yp, zm));
        points.push_back(Vector3(xp, ym, zp));
        points.push_back(Vector3(xp, ym, zm));
        points.push_back(Vector3(xm, yp, zp));
        points.push_back(Vector3(xm, yp, zm));
        points.push_back(Vector3(xm, ym, zp));
        points.push_back(Vector3(xm, ym, zm));
      }
    }
  }
  // std::cout << "Calculating convex hull with number of points: "
  //		<< points.size() << std::endl;
  if (points.size() > 3)
    convex_hull_ = Convex(points);
  else
    convex_hull_ = Convex();

  // convex_hull_.volume = convex_hull_.ComputeVolume();
}

// Simply method for debugging purposes which outputs a given voxel subset
// to a file in WRL file format
void VoxelSubset::writeWRL(std::string filename, bool debugTriangles) {
  std::ofstream file(filename.c_str());
  file << "#VRML V2.0 utf8" << std::endl << std::endl;
  file << "#Created by boundingmesh" << std::endl << std::endl;
  file << "Transform {" << std::endl;
  file << "\t translation " << voxels_->origin()(0) << " "
       << voxels_->origin()(1) << " " << voxels_->origin()(2) << std::endl;
  file << "\t scale " << voxels_->voxelSize() << " " << voxels_->voxelSize()
       << " " << voxels_->voxelSize() << std::endl;
  file << "\t children [ " << std::endl;
  for (unsigned int i = 0; i < indices_.size(); ++i) {
    const Voxel& voxel = voxels_->voxel(indices_[i]);
    file << "\t\tTransform {" << std::endl;
    file << "\t\t\t translation " << voxel.x() << " " << voxel.y() << " "
         << voxel.z() << std::endl;
    file << "\t\t\t children [ " << std::endl;

    file << "\t\t\t\tShape {" << std::endl;
    file << "\t\t\t\t\tappearance Appearance {" << std::endl;
    file << "\t\t\t\t\t\tmaterial Material {" << std::endl;
    file << "\t\t\t\t\t\t\ttransparency 0.5" << std::endl;
    if (debugTriangles) {
      if (voxel.nTriangles() == 0)
        file << "\t\t\t\t\t\t\tdiffuseColor 1 0 0" << std::endl;
      else if (voxel.nTriangles() == 1)
        file << "\t\t\t\t\t\t\tdiffuseColor 0 1 0" << std::endl;
      else if (voxel.nTriangles() == 2)
        file << "\t\t\t\t\t\t\tdiffuseColor 0 0 1" << std::endl;
      else if (voxel.nTriangles() == 3)
        file << "\t\t\t\t\t\t\tdiffuseColor 1 1 1" << std::endl;
      else
        file << "\t\t\t\t\t\t\tdiffuseColor 0 0 0" << std::endl;

    } else {
      if (voxel.type() == SURFACE)
        file << "\t\t\t\t\t\t\tdiffuseColor 1 0 0" << std::endl;
      else if (voxel.type() == INNER)
        file << "\t\t\t\t\t\t\tdiffuseColor 0 0 1" << std::endl;
    }
    file << "\t\t\t\t\t\t}" << std::endl;
    file << "\t\t\t\t\t}" << std::endl;
    file << "\t\t\t\t\tgeometry Box {" << std::endl;
    file << "\t\t\t\t\t\tsize 1 1 1" << std::endl;
    file << "\t\t\t\t\t}" << std::endl;
    file << "\t\t\t\t}" << std::endl;
    file << "\t\t\t ]" << std::endl;
    file << "\t\t}";
  }
  file << "\t ]" << std::endl;
  file << "}";
}

SegmenterDownsampling::SegmenterDownsampling() {}

void SegmenterDownsampling::setMaximumConcavity(Real maximum_concavity) {
  maximum_concavity_ = maximum_concavity;
}

void SegmenterDownsampling::setMaxPasses(int passes) { passes_ = passes; }

void SegmenterDownsampling::setMesh(std::shared_ptr<Mesh> mesh,
                                    int min_voxel_count) {
  int count = 1;
  double length = 100;
  VoxelSet set;
  while (count < min_voxel_count * 0.9 || count > min_voxel_count * 1.1) {
    set = VoxelSet(mesh, length, true);
    count = set.nVoxels();
    // std::cout << "count: " << count << " length: " << length << std::endl;
    if (count < min_voxel_count * 0.9 || count > min_voxel_count * 1.1) {
      double a = pow((double)(min_voxel_count) / count, 1.0 / 3.0);
      length = length / a;
      // std::cout << "a: " << a << " length: " << length << std::endl;
    }
  }

  voxels_ = std::make_shared<VoxelSet>(mesh, length, true);
}

std::shared_ptr<VoxelSet> SegmenterDownsampling::getVoxels() { return voxels_; }

// Computes and stores the bounding box
void VoxelSubset::ComputeBB() {
  const size_t nVoxels = indices_.size();
  if (nVoxels == 0) return;
  bounding_box_min_[0] = voxels_->voxel(indices_[0]).x();
  bounding_box_max_[0] = voxels_->voxel(indices_[0]).x();
  bounding_box_min_[1] = voxels_->voxel(indices_[0]).y();
  bounding_box_max_[1] = voxels_->voxel(indices_[0]).y();
  bounding_box_min_[2] = voxels_->voxel(indices_[0]).z();
  bounding_box_max_[2] = voxels_->voxel(indices_[0]).z();
  Vector3 bary = Vector3::Zero();
  for (size_t p = 0; p < nVoxels; ++p) {
    const Voxel& voxel = voxels_->voxel(indices_[p]);
    bary[0] += voxel.x();
    bary[1] += voxel.y();
    bary[2] += voxel.z();

    if (bounding_box_min_[0] > voxel.x()) {
      bounding_box_min_[0] = voxel.x();
    }
    if (bounding_box_max_[0] < voxel.x()) {
      bounding_box_max_[0] = voxel.x();
    }
    if (bounding_box_min_[1] > voxel.y()) {
      bounding_box_min_[1] = voxel.y();
    }
    if (bounding_box_max_[1] < voxel.y()) {
      bounding_box_max_[1] = voxel.y();
    }
    if (bounding_box_min_[2] > voxel.z()) {
      bounding_box_min_[2] = voxel.z();
    }
    if (bounding_box_max_[2] < voxel.z()) {
      bounding_box_max_[2] = voxel.z();
    }
  }

  bary /= (double)nVoxels;
  for (int h = 0; h < 3; ++h) {
    barycenter_[h] = (short)(bary[h] + 0.5);
  }
}

void VoxelSubset::ComputePrincipalAxes() {
  const size_t nVoxels = indices_.size();
  if (nVoxels == 0) return;
  barycenterPCA_[0] = barycenterPCA_[1] = barycenterPCA_[2] = 0.0;
  for (size_t v = 0; v < nVoxels; ++v) {
    const Voxel& voxel = voxels_->voxel(indices_[v]);
    barycenterPCA_[0] += voxel.x();
    barycenterPCA_[1] += voxel.y();
    barycenterPCA_[2] += voxel.z();
  }
  barycenterPCA_ /= (double)nVoxels;

  double covMat[3][3] = {{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}};
  double x, y, z;
  for (size_t v = 0; v < nVoxels; ++v) {
    const Voxel& voxel = voxels_->voxel(indices_[v]);
    x = voxel.x() - barycenter_[0];
    y = voxel.y() - barycenter_[1];
    z = voxel.z() - barycenter_[2];
    covMat[0][0] += x * x;
    covMat[1][1] += y * y;
    covMat[2][2] += z * z;
    covMat[0][1] += x * y;
    covMat[0][2] += x * z;
    covMat[1][2] += y * z;
  }
  covMat[0][0] /= nVoxels;
  covMat[1][1] /= nVoxels;
  covMat[2][2] /= nVoxels;
  covMat[0][1] /= nVoxels;
  covMat[0][2] /= nVoxels;
  covMat[1][2] /= nVoxels;
  covMat[1][0] = covMat[0][1];
  covMat[2][0] = covMat[0][2];
  covMat[2][1] = covMat[1][2];
  Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> covMatEigen(
      &covMat[0][0]);
  Eigen::JacobiSVD<Eigen::Matrix<double, 3, 3>> svd(covMatEigen);
  // covMatEigen = Q*D*QT
  D_.setZero();
  D_.diagonal().head(svd.nonzeroSingularValues()) = svd.singularValues();
}

Real VoxelSubset::GetEigenValue(int axis) const {
  // std::cout << "Eigenvalue: " << d_[axis][axis] << " " << D_(axis, axis);
  return D_(axis, axis);
}

Vector3 VoxelSubset::GetBoundingBoxMin() const { return bounding_box_min_; }
Vector3 VoxelSubset::GetBoundingBoxMax() const { return bounding_box_max_; }

Real VoxelSubset::GetBoundingBoxSize() const {
  Real a = bounding_box_max_[0] - bounding_box_min_[0];
  Real b = bounding_box_max_[1] - bounding_box_min_[1];
  Real c = bounding_box_max_[2] - bounding_box_min_[2];
  return sqrt(a * a + b * b + c * c);
}

Real SegmenterDownsampling::computePreferredCuttingDirection(VoxelSubset& tset,
                                                             Vector3& dir) {
#if 0
    //FIXME: how should eigenvalues indicate a direction? For that, we would need eigenvectors
	Real ex = tset.GetEigenValue(0);
	Real ey = tset.GetEigenValue(1);
	Real ez = tset.GetEigenValue(2);
	Real vx = (ey - ez) * (ey - ez);
	Real vy = (ex - ez) * (ex - ez);
	Real vz = (ex - ey) * (ex - ey);
	if (vx < vy && vx < vz) {
		Real e = ey * ey + ez * ez;
		dir[0] = 1.0;
		dir[1] = 0.0;
		dir[2] = 0.0;
		return (e == 0.0) ? 0.0 : 1.0 - vx / e;
	} else if (vy < vx && vy < vz) {
		Real e = ex * ex + ez * ez;
		dir[0] = 0.0;
		dir[1] = 1.0;
		dir[2] = 0.0;
		return (e == 0.0) ? 0.0 : 1.0 - vy / e;
	} else {
		Real e = ex * ex + ey * ey;
		dir[0] = 0.0;
		dir[1] = 0.0;
		dir[2] = 1.0;
		return (e == 0.0) ? 0.0 : 1.0 - vz / e;
	}
#else
  dir[0] = 1.0;
  dir[1] = 0.0;
  dir[2] = 0.0;
  return 0;
#endif
}

inline Real computeLocalConcavity(const Real volume,
                                  const Real volumeConvexHull) {
  return fabs(volumeConvexHull - volume) / volumeConvexHull;
}
inline Real computeConcavity(Real volume, Real volumeConvexHull, Real volume0) {
  return fabs(volumeConvexHull - volume) / volume0;
}

Real SegmenterDownsampling::rateSplit(VoxelSubset& inputSet, Split split,
                                      const Vector3& preferredCuttingDirection,
                                      const Real w, const Real alpha,
                                      const Real beta, const Real delta) {
  VoxelSubset onSurfacePSet = inputSet.getSurfaceSubset();

  VoxelSubset left(0);
  VoxelSubset right(0);
  // compute convex-hulls

  std::vector<VoxelSubset> surfaceParts = onSurfacePSet.partitionVoxel(split);
  left = surfaceParts[0];
  right = surfaceParts[1];
  if (superDebug_) {
    surfaceParts[0].writeWRL("a0_voxel.wrl");
    surfaceParts[1].writeWRL("a1_voxel.wrl");
    left.getConvexHull()->writeWrl("b0_voxel.wrl");
    right.getConvexHull()->writeWrl("b1_voxel.wrl");
  }

  Real volumeLeftCH = left.convexVolume();
  Real volumeRightCH = right.convexVolume();

  // compute clipped volumes
  std::vector<VoxelSubset> parts = inputSet.partitionVoxel(split, false);
  Real volumeLeft = parts[0].volume();
  Real volumeRight = parts[1].volume();
  Real concavityLeft =
      computeConcavity(volumeLeft, volumeLeftCH, initial_volume_);
  Real concavityRight =
      computeConcavity(volumeRight, volumeRightCH, initial_volume_);
  Real localConcaviotyLeft = computeLocalConcavity(volumeLeft, volumeLeftCH);
  Real localConcavityRight = computeLocalConcavity(volumeRight, volumeRightCH);
  Real concavity = (concavityLeft + concavityRight);
  Real localConcavity = delta * (localConcaviotyLeft + localConcavityRight);

  // compute cost
  Real balance =
      alpha * pow(pow(volumeLeft - volumeRight, 2.0), 0.5) / initial_volume_;
  Real symmetry =
      beta * w * preferredCuttingDirection.dot(split.directionVector());
  Real total = concavity + balance + symmetry + localConcavity;
  if (debug_) {
    std::cout << split.dimension << " " << split.index << " " << volumeLeft
              << " " << volumeRight << " " << total << " " << concavity << " "
              << balance << " " << symmetry << " " << localConcavity
              << std::endl;
  }

  return total;
}

Real SegmenterDownsampling::rateSplit3(VoxelSubset& inputSet, Split split,
                                       const Vector3& preferredCuttingDirection,
                                       const Real w, const Real alpha,
                                       const Real beta, const Real delta) {
  int direction1 = (split.dimension + 1) % 3;
  int direction2 = (split.dimension + 2) % 3;

  VoxelSubset onSurfacePSet = inputSet.getSurfaceSubset();
  std::vector<VoxelSubset> parts = onSurfacePSet.partitionVoxel(split, false);
  std::pair<Real, Real> part00 =
      parts[0].calculate2DConvexHullVolume(direction1);
  std::pair<Real, Real> part01 =
      parts[1].calculate2DConvexHullVolume(direction1);
  std::pair<Real, Real> part10 =
      parts[0].calculate2DConvexHullVolume(direction2);
  std::pair<Real, Real> part11 =
      parts[1].calculate2DConvexHullVolume(direction2);

  Real volumeLeftCH0 = part00.first;
  Real volumeRightCH0 = part01.first;
  Real volumeLeft0 = part00.second;
  Real volumeRight0 = part01.second;
  Real volumeLeftCH1 = part10.first;
  Real volumeRightCH1 = part11.first;
  Real volumeLeft1 = part10.second;
  Real volumeRight1 = part11.second;

  Real area0 = initial_area_[direction1];
  Real concavityLeft0 = computeConcavity(volumeLeft0, volumeLeftCH0, area0);
  Real concavityRight0 = computeConcavity(volumeRight0, volumeRightCH0, area0);
  Real localConcavityLeft0 = computeLocalConcavity(volumeLeft0, volumeLeftCH0);
  Real localConcavityRight0 =
      computeLocalConcavity(volumeRight0, volumeRightCH0);

  Real area1 = initial_area_[direction2];
  Real concavityLeft1 = computeConcavity(volumeLeft1, volumeLeftCH1, area1);
  Real concavityRight1 = computeConcavity(volumeRight1, volumeRightCH1, area1);
  Real localConcavityLeft1 = computeLocalConcavity(volumeLeft1, volumeLeftCH1);
  Real localConcavityRight1 =
      computeLocalConcavity(volumeRight1, volumeRightCH1);

  Real concavity =
      MAX(concavityLeft0 + concavityRight0, concavityLeft1 + concavityRight1);
  // Real concavity = MAX(concavityLeft0 + concavityRight0, concavityLeft1 +
  // concavityRight1);
  Real localConcavity = delta * MAX(localConcavityLeft0 + localConcavityRight0,
                                    localConcavityLeft1 + localConcavityRight1);

  // compute cost
  Real balance = alpha *
                 pow(pow(parts[0].volume() - parts[1].volume(), 2.0), 0.5) /
                 initial_volume_;
  Real symmetry =
      beta * w * preferredCuttingDirection.dot(split.directionVector());
  Real total = concavity + balance + symmetry + localConcavity;
  if (debug_) {
    std::cout << split.dimension << " " << split.index << " " << total << " "
              << concavity << " " << balance << " " << symmetry << " "
              << localConcavity << std::endl;
    std::cout << "\t" << volumeLeft0 << "\t" << volumeLeftCH0 << "\t\t"
              << volumeRight0 << "\t" << volumeRightCH0 << std::endl;
    std::cout << "\t" << volumeLeft1 << "\t" << volumeLeftCH1 << "\t\t"
              << volumeRight1 << "\t" << volumeRightCH1 << std::endl;
  }

  return total;
}

void SegmenterDownsampling::computeBestClippingPlane(
    const VoxelSubset& inputPSet2, const Real volume,
    const std::vector<Split>& splits, const Vector3& preferredCuttingDirection,
    const Real w, const Real alpha, const Real beta, const Real delta,
    const int convexhullDownsampling, Split& bestSplit) {
  VoxelSubset inputPSet = inputPSet2;

  int iBest = -1;
  Real minTotal = MAX_Real;

  for (int x = 0; x < splits.size(); ++x) {
    Split split = splits[x];

    Real total = 0;
    if (heuristic_ == 1) {
      total = rateSplit(inputPSet, split, preferredCuttingDirection, w, alpha,
                        beta, delta);
    } else if (heuristic_ == 2) {
      std::cout << "Heuristic2 not compiled." << std::endl;
      throw std::invalid_argument("Heuristic2 not compiled");
    } else if (heuristic_ == 3) {
      total = rateSplit3(inputPSet, split, preferredCuttingDirection, w, alpha,
                         beta, delta);
    } else {
      std::cout << "Unknown heuristc: " << heuristic_ << std::endl;
    }

    if (total < minTotal || (total == minTotal && x < iBest)) {
      bestSplit = split;
      minTotal = total;
      iBest = x;
    }
  }
}

void SegmenterDownsampling::printDebugSlices(VoxelSubset whole_mesh) {
  for (int i = 0; i < voxels_->resolution(0); i++) {
    std::vector<VoxelSubset> parts = whole_mesh.partitionVoxel(Split(0, i));
    parts[0].writeWRL("part" + std::to_string(static_cast<long long>(i)) +
                      "_voxel.wrl");
  }
}

void SegmenterDownsampling::compute() {
  std::vector<VoxelSubset> parts;
  std::vector<VoxelSubset> inputParts;
  std::vector<VoxelSubset> temp;

  VoxelSubset whole_mesh(voxels_);
  for (int i = 0; i < voxels_->nVoxels(); ++i) {
    whole_mesh.addVoxel(i);
  }
  whole_mesh.calculateConvexHullVoxel();
  inputParts.push_back(whole_mesh);
  // printDebugSlices(whole_mesh);

  for (int pass = 0; pass < passes_ && inputParts.size() > 0; pass++) {
    std::cout << "Pass " << pass << " Number of parts: " << inputParts.size()
              << std::endl;
    if (debug_) {
      std::vector<std::shared_ptr<Mesh>> meshes;
      for (unsigned int i = 0; i < inputParts.size(); ++i) {
        meshes.push_back(inputParts[i].getConvexHull());
      }
      for (unsigned int i = 0; i < parts.size(); ++i) {
        meshes.push_back(parts[i].getConvexHull());
      }
      boundingmesh::Mesh::writeMultimeshWrl(
          meshes,
          "step" + std::to_string(static_cast<long long>(pass)) + ".wrl", true);
    }
    for (int partIndex = 0; partIndex < inputParts.size(); partIndex++) {
      VoxelSubset currentPart = inputParts[partIndex];
      Real volume = currentPart.volume();
      currentPart.ComputeBB();
      currentPart.ComputePrincipalAxes();
      if (pass == 1 && partIndex == 0) {
        // superDebug_ = true;
      }
      if (debug_) {
        currentPart.getConvexHull()->writeWrl(
            "step" + std::to_string(static_cast<long long>(pass)) + "_" +
            std::to_string(static_cast<long long>(partIndex)) + ".wrl");
        inputParts[partIndex].writeWRL(
            "step" + std::to_string(static_cast<long long>(pass)) + "_" +
                std::to_string(static_cast<long long>(partIndex)) +
                "_voxel.wrl",
            true);
      }
      Real volumeConvexHull = fabs(currentPart.convexVolume());

      if (pass == 0) {
        initial_volume_ = volumeConvexHull;
        initial_size_ = currentPart.GetBoundingBoxSize();
        initial_surface_voxels_ = currentPart.getSurfaceSubset().voxelCount();
        for (int i = 0; i < 3; i++) {
          initial_area_[i] = currentPart.calculate2DConvexHullVolume(i).first;
        }
      }

      Real concavity =
          computeConcavity(volume, volumeConvexHull, initial_volume_);
      Real error =
          (1.01 * currentPart.surfaceVoxelCount() * voxels_->voxelSize() *
           voxels_->voxelSize() * voxels_->voxelSize()) /
          initial_volume_;
      Real localConcavity =
          (volumeConvexHull > 0.0)
              ? computeLocalConcavity(volume, volumeConvexHull)
              : 0.0;
      if (debug_) {
        std::cout << "volume: " << volume
                  << " convexVolume: " << volumeConvexHull
                  << " concavity: " << concavity << " error: " << error
                  << " localConcavity: " << localConcavity << std::endl;
      }

      if (concavity > maximum_concavity_ && concavity > error) {
        Vector3 preferredCuttingDirection;
        Real w = computePreferredCuttingDirection(currentPart,
                                                  preferredCuttingDirection);

        std::vector<Split> possibleSplits =
            generateSplits(currentPart, plane_downsampling_);

        Split bestSplit;

        computeBestClippingPlane(
            currentPart, volume, possibleSplits, preferredCuttingDirection, w,
            concavity * alpha_, concavity * beta_, concavity * delta_,
            convexhull_downsampling_, bestSplit);

        if (plane_downsampling_ > 1) {
          possibleSplits =
              refineSplits(currentPart, bestSplit, plane_downsampling_);

          computeBestClippingPlane(currentPart, volume, possibleSplits,
                                   preferredCuttingDirection, w,
                                   concavity * alpha_, concavity * beta_,
                                   concavity * delta_, 1, bestSplit);
        }

        std::vector<VoxelSubset> parts = currentPart.partitionVoxel(bestSplit);
        temp.insert(temp.end(), parts.begin(), parts.end());
      } else {
        parts.push_back(currentPart);
      }
    }
    inputParts = temp;
    temp.clear();
  }
  parts.insert(parts.end(), inputParts.begin(), inputParts.end());

  std::vector<std::shared_ptr<Mesh>> meshes;
  std::vector<std::shared_ptr<Mesh>> meshes2;
  for (unsigned int i = 0; i < parts.size(); ++i) {
    if (debug_) {
      meshes2.push_back(parts[i].getConvexHull());
    }
  }
  if (debug_) {
    boundingmesh::Mesh::writeMultimeshWrl(
        meshes2, "stepfinal_beforemerge_voxelhull.wrl", true);
  }
  std::cout << "Merging convex hulls..." << std::endl;
  // parts = mergeConvexHulls(parts);
  for (long long i = 0; i < parts.size(); ++i) {
    if (debug_) {
      meshes.push_back(parts[i].getConvexHull());
    }
    parts[i].calculateConvexHull();
    if (debug_) {
      parts[i].getConvexHull()->writeWrl(
          "stepfinal_part" + std::to_string(static_cast<long long>(i)) +
          ".wrl");
    }
  }
  if (debug_) {
    boundingmesh::Mesh::writeMultimeshWrl(
        meshes, "stepfinal_aftermerge_voxelhull.wrl", true);
  }
  subsets_ = parts;
  for (unsigned int i = 0; i < subsets_.size(); ++i) {
    segmentation_.push_back(subsets_[i].getConvexHull());
  }
}

// Determines whether the bounding boxes of two voxel subsets are adjacent
// to each other or not
bool SegmenterDownsampling::arePartsAdjacent(VoxelSubset& p1, VoxelSubset& p2) {
  double maxDelta = 1;
  Vector3 p1min = p1.GetBoundingBoxMin();
  Vector3 p1max = p1.GetBoundingBoxMax();
  Vector3 p2min = p2.GetBoundingBoxMin();
  Vector3 p2max = p2.GetBoundingBoxMax();

  for (int i = 0; i < 3; i++) {
    if (p1max[i] + maxDelta < p2min[i] || p1min[i] - maxDelta > p2max[i]) {
      return false;
    }
  }

  return true;
}

// Merges adjacent convex hulls so that the error does not increase above the
// specified threshhold and
// the total number of parts decreases at the same time
std::vector<VoxelSubset> SegmenterDownsampling::mergeConvexHulls(
    std::vector<VoxelSubset> parts) {
  long long iteration = 0;
  if (parts.size() > 1) {
    bool iterate = true;

    for (int i = 0; i < parts.size(); i++) {
      parts[i].ComputeBB();
    }

    while (iterate) {
      if (parts.empty()) {
        continue;
      }
      size_t bestp1 = 0;
      size_t bestp2 = 0;
      double bestCost = MAX_Real;  // initial_volume_;
      for (size_t p1 = 0; p1 < parts.size() - 1; ++p1) {
        double volume1 = parts[p1].convexVolume();
        for (size_t p2 = p1 + 1; p2 < parts.size() - 1; ++p2) {
          double volume2 = parts[p2].convexVolume();

          if (!arePartsAdjacent(parts[p1], parts[p2])) {
            continue;
          }

          VoxelSubset merged = parts[p1].merge(parts[p2]);

          double combinedVolumeCH = merged.convexVolume();
          double combinedVolume = volume1 + volume2;
          double cost = computeConcavity(combinedVolume, combinedVolumeCH,
                                         initial_volume_);

          if (cost < bestCost) {
            bestCost = cost;
            bestp1 = p1;
            bestp2 = p2;
          }
        }
      }
      if (debug_) {
        std::cout << " best cost: " << bestCost << " gamma: " << gamma_
                  << std::endl;
      }
      if (bestCost < gamma_) {
        if (debug_) {
          parts[bestp1].getConvexHull()->writeWrl(
              "mergedpart1_" + std::to_string(iteration) + ".wrl");
          parts[bestp2].getConvexHull()->writeWrl(
              "mergedpart2_" + std::to_string(iteration) + ".wrl");
        }
        VoxelSubset merged = parts[bestp1].merge(parts[bestp2]);
        parts.erase(parts.begin() + bestp1);
        parts.erase(parts.begin() + bestp2 - 1);
        parts.push_back(merged);
        std::cout << "merged " << bestp1 << " with " << bestp2
                  << "part1 volume: " << parts[bestp1].convexVolume()
                  << " part2 volume: " << parts[bestp2].convexVolume()
                  << " combined volume: " << merged.convexVolume() << std::endl;
        iterate = true;
        if (debug_) {
          merged.getConvexHull()->writeWrl("mergedpart" +
                                           std::to_string(iteration) + ".wrl");
          std::vector<std::shared_ptr<Mesh>> meshes;
          for (unsigned int i = 0; i < parts.size(); ++i) {
            if (debug_) {
              meshes.push_back(parts[i].getConvexHull());
            }
          }
          boundingmesh::Mesh::writeMultimeshWrl(
              meshes, "merge" + std::to_string(iteration) + ".wrl", true);
        }
      } else {
        iterate = false;
      }
      iteration++;
    }
  }

  return parts;
}

std::vector<std::shared_ptr<Mesh>> SegmenterDownsampling::getSegmentation() {
  return segmentation_;
}

// Generates a lit of all possible splits considering only every nth split
// where n is the downsampling rate
std::vector<Split> SegmenterDownsampling::generateSplits(
    const VoxelSubset& subset, const int downsampling) {
  std::vector<Split> possible_splits;
  Vector3 min = subset.GetBoundingBoxMin();
  Vector3 max = subset.GetBoundingBoxMax();
  for (int dimension = 0; dimension < 3; ++dimension)
    for (int index = min(dimension) + 1; index < max(dimension);
         index += downsampling) {
      possible_splits.push_back(Split(dimension, index));
    }

  return possible_splits;
}

// Generates all splits around the split that has previously been declared as
// best split with downsampling
std::vector<Split> SegmenterDownsampling::refineSplits(
    const VoxelSubset& subset, const Split& bestSplit, const int downsampling) {
  std::vector<Split> possible_splits;
  int dimension = bestSplit.dimension;
  int start = MAX(subset.GetBoundingBoxMin()(dimension),
                  bestSplit.index - downsampling);
  int end = MIN(subset.GetBoundingBoxMax()(dimension) - 1,
                bestSplit.index + downsampling);
  for (int index = start; index <= end; index += 1) {
    possible_splits.push_back(Split(dimension, index));
  }

  return possible_splits;
}
}


/**

Copyright (c) 2016, Hanselmann Fabian, Heller Florian, Heizmann Heinrich, Kübler Marcel, Mehlhaus Jonas, Meißner Pascal, Qattan Mohamad, Reckling Reno, Stroh Daniel
All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.

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

3. Neither the name of the copyright holder 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.

*/

#pragma once

#include <Eigen/Geometry>
#include <cmath>
#include "common_type/Pose.hpp"
#include "common_type/Quaternion.hpp"
#include "common_type/VoteSpecifier.hpp"
#include "common_type/Point.hpp"

namespace ISM {
  class GeometryHelper {
  public:

    //Methods are all inlined since heavily used in performance relevant code.

    /**Voting related methods**/

    /**
     * Calculates an absolute pose at refPoint based on a relative orientation refToSourceQuat towards source.
     *
     * @param source Absolute position in refPoint has been calculated be combining source and the relative position of refPoint towards source.
     * @param refPoint Absolute position for which we want to calculate the corresponding orientation.
     * @param refToSourceQuat Relative orientation of searched absolute pose relative to source.
     * @return 6D pose being looked for.
     */
    static PosePtr getReferencePose(const PosePtr& source, const PointPtr& refPoint, const QuaternionPtr& objectToRefPoseQuat) {
      PosePtr result = PosePtr(new Pose());
      getReferencePose(source, refPoint, objectToRefPoseQuat, result);
      return result;
    }

    //Faster variant that avoids allocation of local Pose in every call of this method.
    static void getReferencePose(const PosePtr& source, const PointPtr& refPoint, const QuaternionPtr& objectToRefPoseQuat, PosePtr& result) {
      result->point = refPoint;
      result->quat = eigenQuatToQuat(source->quat->eigen * objectToRefPoseQuat->eigen);
    }

    /**
     * Calculate an absolute position by combining refPose and a relative position vector composed of refToObjectQuat and radius.
     *
     * @param refPose Absolute position that is combined with relative pose.
     * @param refToObjectQuat Orientation of Eigen::Vector3d connecting refPose and position being searched for.
     * @param radius Length of Eigen::Vector3d connecting refPose and position being searched for.
     * @return 3D position being looked for.
     */
    static PointPtr getSourcePoint(const PosePtr& refPose, const QuaternionPtr& refToObjectQuat, double radius) {
      PointPtr result = PointPtr(new Point());
      getSourcePoint(refPose, refToObjectQuat, radius, result);
      return result;
    }

    //Faster variant that avoids allocation of local Pose in every call of this method.
    static void getSourcePoint(const PosePtr& refPose, const QuaternionPtr& refToObjectQuat, double radius, PointPtr& result) {
      Eigen::Vector3d relativeObjectVector = refToObjectQuat->eigen._transformVector(Eigen::Vector3d::UnitX());
      Eigen::Vector3d absoluteObjectVector = refPose->quat->eigen._transformVector(relativeObjectVector);
      result->eigen = refPose->point->eigen + (absoluteObjectVector * radius);
    }

    static PosePtr getSourcePose(const PosePtr& reference, const PointPtr& sourcePoint, const QuaternionPtr& refToObjectPoseQuat) {
      PosePtr result = PosePtr(new Pose());
      getSourcePose(reference, sourcePoint, refToObjectPoseQuat, result);
      return result;
    }

    //Faster variant that avoids allocation of local Pose in every call of this method.
    static void getSourcePose(const PosePtr& reference, const PointPtr& sourcePoint, const QuaternionPtr& refToObjectPoseQuat, PosePtr& result) {
      result->point = sourcePoint;
      result->quat->eigen = reference->quat->eigen * refToObjectPoseQuat->eigen;
    }

    static PointPtr applyQuatAndRadiusToPose(const PosePtr& pose, const QuaternionPtr& quat, double radius) {
      //apply transformation to viewport vector, then scale, than transform to object coordinates and add
      Eigen::Quaternion<double> rotation = pose->quat->eigen * quat->eigen;
      Eigen::Vector3d objectPoint = pose->point->eigen;

      Eigen::Vector3d resultVec = objectPoint + rotation._transformVector(Eigen::Vector3d::UnitX()) * radius;

      return vectorToPoint(resultVec);
    }

    static Quaternion selfQuat;
    static double constexpr epsylon = 1e-4;
    static bool isSelfVote(const VoteSpecifierPtr& vote)
    {
      if ( !(vote->radius > -epsylon && vote->radius < epsylon) ||
	   !quatEqual(selfQuat, *(vote->objectToRefPoseQuat)))
        {
	  return false;
        }
      return true;
    }

    /**Pose related methods**/

    static VoteSpecifierPtr createVoteSpecifier(const PosePtr& sourcePose, const PosePtr& refPose) {
      if ((refPose->point->eigen - sourcePose->point->eigen).norm() == 0) {
	//avoid special case
	sourcePose->point->eigen.x() = (sourcePose->point->eigen.x() + 0.0000001);
      };
      Eigen::Vector3d objToRefVector = refPose->point->eigen - sourcePose->point->eigen;
      Eigen::Vector3d refToObjVector = objToRefVector * -1.0;
      Eigen::Quaternion<double> p = sourcePose->quat->eigen;
      Eigen::Quaternion<double> r = refPose->quat->eigen;

      //rotate everything relative to object pose
      Eigen::Vector3d relativeRefPoint = p.inverse()._transformVector(objToRefVector);

      Eigen::Quaternion<double> otr = vectorRotationToEigenQuat(Eigen::Vector3d::UnitX(), relativeRefPoint);
      Eigen::Quaternion<double> otrp = (p.inverse()) * r;

      //rotate everything relative to ref pose
      Eigen::Vector3d relativeObjPoint = r.inverse()._transformVector(refToObjVector);
      Eigen::Quaternion<double> rto = vectorRotationToEigenQuat(Eigen::Vector3d::UnitX(), relativeObjPoint);
      Eigen::Quaternion<double> rtop = (r.inverse()) * p;

      return VoteSpecifierPtr(
			      new VoteSpecifier(
						eigenQuatToQuat(otr),
						eigenQuatToQuat(otrp),
						eigenQuatToQuat(rto),
						eigenQuatToQuat(rtop),
						objToRefVector.norm()
						)
			      );
    }

    static PosePtr getPoseFromVote(const PosePtr& source, const VoteSpecifierPtr& vote)
    {
      PointPtr referencePoint = applyQuatAndRadiusToPose(source, vote->objectToRefQuat, vote->radius);
      return getReferencePose(source, referencePoint, vote->objectToRefPoseQuat);
    }

    static bool poseEqual(const PosePtr& p1, const PosePtr& p2) {
      bool overall = (quatEqual(p1->quat, p2->quat) && pointEqual (p1->point, p2->point));
      return overall;
    }

    static Eigen::Vector3d getDirectionVector(const PosePtr& first, const PosePtr& second) {

      Eigen::Vector3d firstToSecond = second->point->eigen - first->point->eigen;
      Eigen::Quaternion<double> firstRotation = first->quat->eigen;
      return firstRotation.inverse()._transformVector(firstToSecond);

    }

    static bool sharedNeighborhoodEvaluation(const PosePtr& p1, const PosePtr& p2, double distanceNeighbourThreshold, double angleNeighbourThreshold)
    {
      double distance_angle = getAngleBetweenQuats(p1->quat, p2->quat);
      double distance_position = getDistanceBetweenPoints(p1->point, p2->point);

      return distanceNeighbourThreshold > distance_position
	&& angleNeighbourThreshold > fabs(distance_angle);
    }

    /**Point related methods**/

    static Eigen::Vector3d pointToVector(const PointPtr& p) {
      return p->eigen;
    }

    static PointPtr vectorToPoint(const Eigen::Vector3d& v) {
      return PointPtr(new Point(v[0], v[1], v[2]));
    }

    static double getDistanceBetweenPoints(const PointPtr& p1, const PointPtr& p2) {
      return (p1->eigen - p2->eigen).norm();

    }

    static double getSquaredDistanceBetweenPoints(const PointPtr &p1, const PointPtr &p2)
    {
      return (p1->eigen - p2->eigen).squaredNorm();
    }

    static bool pointEqual(const PointPtr& p1, const PointPtr& p2)
    {
      bool xAbs = fabs(p1->eigen.x() - p2->eigen.x()) < 0.00001;
      bool yAbs = fabs(p1->eigen.y() - p2->eigen.y()) < 0.00001;
      bool zAbs = fabs(p1->eigen.z() - p2->eigen.z()) < 0.00001;
      return (xAbs && yAbs && zAbs);
    }

    /**Quaternion related methods**/

    static Eigen::Quaternion<double> quatToEigenQuat(const QuaternionPtr& q) {
      return q->eigen;
    }

    static QuaternionPtr eigenQuatToQuat(const Eigen::Quaternion<double>& q) {
      return QuaternionPtr(new Quaternion(q.w(), q.x(), q.y(), q.z()));
    }

    static QuaternionPtr normalize(const QuaternionPtr& quat) {
      return eigenQuatToQuat(quat->eigen.normalized());
    }

    static double getAngleBetweenQuats(const QuaternionPtr& q1, const QuaternionPtr& q2) {
      return rad2deg( q1->eigen.angularDistance(q2->eigen) );
    }

    static Eigen::Vector3d getAxisFromQuat(const QuaternionPtr& quat, const Eigen::Vector3d& viewport = Eigen::Vector3d::UnitX()) {
      Eigen::Quaternion<double> rotation = quat->eigen;
      return rotation._transformVector(viewport);
    }

    static double getAngleBetweenAxes(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2) {

      double cosOfAngle = v1.dot(v2) / (v1.norm() * v2.norm());
      if(cosOfAngle < -1.0) cosOfAngle = -1.0;
      if(cosOfAngle > 1.0) cosOfAngle = 1.0;

      return rad2deg(acos(cosOfAngle));

    }

    static bool quatEqual(const QuaternionPtr& q1, const QuaternionPtr& q2)
    {
      return quatEqual(*q1, *q2);
    }

    static bool quatEqual(const Quaternion& q1, const Quaternion& q2) {
      bool qxAbs = fabs(q1.eigen.x() - q2.eigen.x()) < 0.00001;
      bool qyAbs = fabs(q1.eigen.y() - q2.eigen.y()) < 0.00001;
      bool qzAbs = fabs(q1.eigen.z() - q2.eigen.z()) < 0.00001;
      bool qwAbs = fabs(q1.eigen.w() - q2.eigen.w()) < 0.00001;
      return (qxAbs && qyAbs && qzAbs && qwAbs);
    }

    static Eigen::Quaternion<double> vectorRotationToEigenQuat(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2) {
      Eigen::Quaternion<double> q;
      q.setFromTwoVectors(v1, v2);
      return q;
    }

    static QuaternionPtr getQuatFromRPY(const QuaternionPtr& q, const double alpha, const double beta, const double gamma){
      Eigen::Quaternion<double> eq = q->eigen;
      Eigen::AngleAxisd rollAngle(deg2rad(alpha), Eigen::Vector3d::UnitX());
      Eigen::AngleAxisd pitchAngle(deg2rad(beta), Eigen::Vector3d::UnitY());
      Eigen::AngleAxisd yawAngle(deg2rad(gamma), Eigen::Vector3d::UnitZ());

      Eigen::Quaterniond rot = yawAngle * pitchAngle * rollAngle;

      return eigenQuatToQuat(eq * rot);

    }

    static QuaternionPtr getAveragePose(const std::vector<QuaternionPtr>& poseQuats) {
      Eigen::Vector3d combined(0, 0, 0);

      for (const QuaternionPtr& quat : poseQuats) {
	combined += quat->eigen._transformVector(Eigen::Vector3d::UnitX());
      }

      Eigen::Quaternion<double> ret;
      ret.setFromTwoVectors(Eigen::Vector3d::UnitX(), combined);
      return eigenQuatToQuat(ret.normalized());
    }

    static bool getNextCombination(const std::vector<unsigned int>::iterator first, std::vector<unsigned int>::iterator k, const std::vector<unsigned int>::iterator last)
    {
      if ((first == last) || (first == k) || (last == k))
	return false;

      std::vector<unsigned int>::iterator it1 = first;
      std::vector<unsigned int>::iterator it2 = last;
      it1++;
      if (last == it1)
	return false;
      it1 = last;
      it1--;
      it1 = k;
      it2--;
      while (first != it1)
        {
	  if (*--it1 < *it2)
            {
	      std::vector<unsigned int>::iterator j = k;
	      while (!(*it1 < *j)) ++j;
	      std::iter_swap(it1,j);
	      it1++;
	      j++;
	      it2 = k;
	      std::rotate(it1,j,last);
	      while (last != j)
                {
		  j++;
		  it2++;
                }
	      std::rotate(k,it2,last);
	      return true;
            }
        }
      std::rotate(first,k,last);
      return false;
    }

    /**Various geometry related helpers**/

    static double rad2deg(double rad) {
      return rad * (180.0 / M_PI);
    }

    static double deg2rad(double deg) {
      return deg * (M_PI / 180.0);
    }

    static inline bool checkIfDoubleNaN(double& c)
    {
      /*
       * distance != distance for portable way check for nan
       * but be aware:
       * http://stackoverflow.com/questions/570669/checking-if-a-double-or-float-is-nan-in-c
       */
      bool isNan = c != c;
      if (isNan)
        {
	  std::cerr << " NaN ";
        }
      return isNan;
    }

  };

}


/**
MIT License

This file is part of the Granite project which is based on Basalt.
https://github.com/DLR-RM/granite

Copyright (c) Martin Wudenka, Deutsches Zentrum für Luft- und Raumfahrt

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

/**
Original license of Basalt:
BSD 3-Clause License

This file is part of the Basalt project.
https://gitlab.com/VladyslavUsenko/granite-headers.git

Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder 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.

@file
@brief Implementation of stereographic parametrization for unit vectors
*/

#pragma once

#include <Eigen/Dense>

namespace granite {

/// @brief Stereographic projection for minimal representation of unit vectors
///
/// \image html stereographic.png
/// The projection is defined on the entire
/// sphere, except at one point: the projection point. Where it is defined, the
/// mapping is smooth and bijective. The illustrations shows 3 examples of unit
/// vectors parametrized with a point in the 2D plane. See
/// \ref project and \ref unproject functions for more details.
template <typename Scalar = double>
class StereographicParam {
 public:
  using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
  using Vec4 = Eigen::Matrix<Scalar, 4, 1>;

  using Mat24 = Eigen::Matrix<Scalar, 2, 4>;
  using Mat42 = Eigen::Matrix<Scalar, 4, 2>;

  /// @brief Project the point and optionally compute Jacobian
  ///
  /// Projection function is defined as follows:
  /// \f{align}{
  ///    \pi(\mathbf{x}) &=
  ///    \begin{bmatrix}
  ///   {\frac{x}{d + z}}
  ///    \\ {\frac{y}{d + z}}
  ///    \\ \end{bmatrix},
  ///    \\ d &= \sqrt{x^2 + y^2 + z^2}.
  /// \f}
  /// @param[in] p3d point to project [x,y,z]
  /// @param[out] d_r_d_p if not nullptr computed Jacobian of projection
  /// with respect to p3d
  /// @return 2D projection of the point which parametrizes the corresponding
  /// direction vector
  static inline Vec2 project(const Vec4& p3d, Mat24* d_r_d_p = nullptr) {
    const Scalar sqrt = p3d.template head<3>().norm();
    const Scalar norm = p3d[2] + sqrt;
    const Scalar norm_inv = Scalar(1) / norm;

    const Vec2 res(p3d[0] * norm_inv, p3d[1] * norm_inv);

    if (d_r_d_p) {
      Scalar norm_inv2 = norm_inv * norm_inv;
      Scalar tmp = -norm_inv2 / sqrt;

      (*d_r_d_p).setZero();

      (*d_r_d_p)(0, 0) = norm_inv + p3d[0] * p3d[0] * tmp;
      (*d_r_d_p)(1, 0) = p3d[0] * p3d[1] * tmp;

      (*d_r_d_p)(1, 1) = norm_inv + p3d[1] * p3d[1] * tmp;
      (*d_r_d_p)(0, 1) = p3d[0] * p3d[1] * tmp;

      (*d_r_d_p)(0, 2) = p3d[0] * norm * tmp;
      (*d_r_d_p)(1, 2) = p3d[1] * norm * tmp;

      (*d_r_d_p)(0, 3) = Scalar(0);
      (*d_r_d_p)(1, 3) = Scalar(0);
    }

    return res;
  }

  /// @brief Unproject the point and optionally compute Jacobian
  ///
  /// The unprojection function is computed as follows: \f{align}{
  ///    \pi^{-1}(\mathbf{u}) &=
  ///    \frac{2}{1 + x^2 + y^2}
  ///    \begin{bmatrix}
  ///    u \\ v \\ 1
  ///    \\ \end{bmatrix}-\begin{bmatrix}
  ///    0 \\ 0 \\ 1
  ///    \\ \end{bmatrix}.
  /// \f}
  ///
  /// @param[in] proj point to unproject [u,v]
  /// @param[out] d_r_d_p if not nullptr computed Jacobian of unprojection
  /// with respect to proj
  /// @return unprojected 3D unit vector
  static inline Vec4 unproject(const Vec2& proj, Mat42* d_r_d_p = nullptr) {
    const Scalar x2 = proj[0] * proj[0];
    const Scalar y2 = proj[1] * proj[1];
    const Scalar r2 = x2 + y2;

    const Scalar norm_inv = Scalar(2) / (Scalar(1) + r2);

    const Vec4 res(proj[0] * norm_inv, proj[1] * norm_inv, norm_inv - Scalar(1),
                   Scalar(0));

    if (d_r_d_p) {
      const Scalar norm_inv2 = norm_inv * norm_inv;
      const Scalar xy = proj[0] * proj[1];

      (*d_r_d_p)(0, 0) = (norm_inv - x2 * norm_inv2);
      (*d_r_d_p)(0, 1) = -xy * norm_inv2;

      (*d_r_d_p)(1, 0) = -xy * norm_inv2;
      (*d_r_d_p)(1, 1) = (norm_inv - y2 * norm_inv2);

      (*d_r_d_p)(2, 0) = -proj[0] * norm_inv2;
      (*d_r_d_p)(2, 1) = -proj[1] * norm_inv2;

      (*d_r_d_p)(3, 0) = Scalar(0);
      (*d_r_d_p)(3, 1) = Scalar(0);
    }

    return res;
  }
};

}  // namespace granite


/**
* This file is part of Intrinsic3D.
*
* Copyright (c) 2019, NVIDIA CORPORATION. All rights reserved.
* Copyright (c) 2019, Technical University of Munich. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
*    * Redistributions of source code must retain the above copyright
*      notice, this list of conditions and the following disclaimer.
*    * Redistributions in binary form must reproduce the above copyright
*      notice, this list of conditions and the following disclaimer in the
*      documentation and/or other materials provided with the distribution.
*    * Neither the name of NVIDIA CORPORATION 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 "AS IS" AND ANY
* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
* PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
* PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
* OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <nv/keyframe_selection.h>

#include <iostream>
#include <sstream>
#include <string>
#include <fstream>
#include <iomanip>

#include <Eigen/Geometry>
#include <opencv2/imgproc.hpp>

namespace nv
{

    KeyframeSelection::KeyframeSelection(int window_size) :
        window_size_(window_size)
	{
	}

	KeyframeSelection::~KeyframeSelection()
	{
	}

	void KeyframeSelection::reset()
	{
		frame_scores_.clear();
		is_keyframe_.clear();
	}

	void KeyframeSelection::add(const cv::Mat &color)
	{
        // compute frame score (measure for image blur)
        double score = estimateBlur(color);

		frame_scores_.push_back(score);
	}

	void KeyframeSelection::selectKeyframes()
	{
		int n = (int)frame_scores_.size();

        // select best frame within a fixed size window
        is_keyframe_.resize(n, false);

        int num_windows = n / window_size_;
        if (n % window_size_ > 0)
            ++num_windows;

        for (int j = 0; j < num_windows; ++j)
        {
            // determine window start and end indices
            int id_win_beg = j * window_size_;
            int id_win_end = std::min(id_win_beg + window_size_, n);
            // find frame with best score within window
            double score_max = 0.0;
            int id_max = id_win_beg;
            for (int id = id_win_beg; id < id_win_end; ++id)
            {
                if (frame_scores_[id] > score_max)
                {
                    score_max = frame_scores_[id];
                    id_max = id;
                }
            }
            // assign isKeyframe to frame ids
            for (int id = id_win_beg; id < id_win_end; ++id)
            {
                is_keyframe_[id] = (id == id_max);
            }
        }
	}

	bool KeyframeSelection::isKeyframe(int id) const
	{
		if (!frameExists(id))
			return false;
		return is_keyframe_[id];
	}

    size_t KeyframeSelection::countKeyframes() const
    {
        size_t cnt = 0;
        for (auto is_kf : is_keyframe_)
        {
            if (is_kf)
                ++cnt;
        }
        return cnt;
    }

    void KeyframeSelection::drawScore(int id, cv::Mat &image) const
	{
		if (!frameExists(id))
			return;

        std::string text = "score: " + std::to_string(frame_scores_[id]);
        cv::putText(image, text, cv::Point(10, 50), cv::FONT_HERSHEY_COMPLEX, 0.9, cv::Scalar(0, 255, 0), 2);
	}

	bool KeyframeSelection::load(const std::string &filename)
    {
		reset();

		// open input file
		std::ifstream file(filename.c_str());
		if (!file.is_open())
			return false;

        // load window size
		std::string line;
		if (std::getline(file, line))
		{
			if (line.empty())
				return false;
            std::istringstream iss(line);
            int window_size;
            if (!(iss >> window_size))
                return false;
            window_size_ = window_size;
		}

		// load frames scores and isKeyframe from file
		while (std::getline(file, line))
		{
			if (line.empty())
				continue;
			std::istringstream iss(line);
			double score;
            bool is_kf;
            if (!(iss >> score >> is_kf))
				break;
			frame_scores_.push_back(score);
            is_keyframe_.push_back(is_kf);
		}

		// close file
		file.close();

		return true;
	}

	bool KeyframeSelection::save(const std::string &filename) const
	{
		if (frame_scores_.empty() || is_keyframe_.empty() || frame_scores_.size() != is_keyframe_.size())
			return false;

		// open output file
		std::ofstream file(filename.c_str());
		if (!file.is_open())
			return false;

        // write window size into output file
        file << window_size_ << std::endl;

		// write frame scores and isKeyframe into output file
		file << std::fixed << std::setprecision(6);
        for (size_t i = 0; i < is_keyframe_.size(); ++i)
		{
			double score = frame_scores_[i];
			bool isKf = is_keyframe_[i];
            file << score << " " << static_cast<int>(isKf) << std::endl;
		}
		// close file
		file.close();

		return true;
	}

	bool KeyframeSelection::frameExists(int id) const
	{
        if (id < 0 || id >= frame_scores_.size() || id >= is_keyframe_.size())
			return false;
		else
			return true;
	}

	double KeyframeSelection::estimateBlur(const cv::Mat &color) const
	{
		if (color.empty())
			return 0.0;
		cv::Mat gray;
		if (color.channels() == 1)
			gray = color;
		else if (color.channels() == 3)
			cv::cvtColor(color, gray, cv::COLOR_BGR2GRAY);
		else
			return 0.0;

		// convert grayscale image to float
        cv::Mat gray_f;
        gray.convertTo(gray_f, CV_32FC1, 1.0 / 255.0);

        // estimate blur using no-reference perceptual blur metric (Crete2007)
        return estimateBlurCrete(gray_f);
	}

	double KeyframeSelection::estimateBlurCrete(const cv::Mat &gray) const
	{
		// no-reference perceptual blur metric (Crete2007)

		int w = gray.cols;
		int h = gray.rows;

		// create blur kernels for strong low-pass filter
		cv::Mat hv = cv::Mat::ones(9, 1, CV_32F)  * (1.0 / 9.0);
		cv::Mat hh;
		cv::transpose(hv, hh);

		// blur image using a strong low-pass filter
        cv::Mat b_ver, b_hor;
        cv::filter2D(gray, b_ver, CV_32FC1, hv);
        cv::filter2D(gray, b_hor, CV_32FC1, hh);

		// compute absolute difference images (vertical)
        cv::Mat d_f_ver = cv::Mat::zeros(h, w, CV_32FC1);
        cv::Mat d_b_ver = cv::Mat::zeros(h, w, CV_32FC1);
		for (int y = 1; y < h; ++y)
		{
			for (int x = 0; x < w; ++x)
			{
                d_f_ver.at<float>(y, x) = std::abs(gray.at<float>(y, x) - gray.at<float>(y - 1, x));
                d_b_ver.at<float>(y, x) = std::abs(b_ver.at<float>(y, x) - b_ver.at<float>(y - 1, x));
			}
		}

		// compute absolute difference images (horizontal)
        cv::Mat d_f_hor = cv::Mat::zeros(h, w, CV_32FC1);
        cv::Mat d_b_hor = cv::Mat::zeros(h, w, CV_32FC1);
		for (int y = 0; y < h; ++y)
		{
			for (int x = 1; x < w; ++x)
			{
                d_f_hor.at<float>(y, x) = std::abs(gray.at<float>(y, x) - gray.at<float>(y, x - 1));
                d_b_hor.at<float>(y, x) = std::abs(b_hor.at<float>(y, x) - b_hor.at<float>(y, x - 1));
			}
		}

		// compute vertical and horizontal variation
        cv::Mat v_ver = cv::Mat::zeros(h, w, CV_32FC1);
        cv::Mat v_hor = cv::Mat::zeros(h, w, CV_32FC1);
		for (int y = 0; y < h; ++y)
		{
			for (int x = 0; x < w; ++x)
			{
                v_ver.at<float>(y, x) = std::max(0.0f, d_f_ver.at<float>(y, x) - d_b_ver.at<float>(y, x));
                v_hor.at<float>(y, x) = std::max(0.0f, d_f_hor.at<float>(y, x) - d_b_hor.at<float>(y, x));
			}
		}

		// compute sum of coefficients
        double s_f_ver = cv::sum(d_f_ver).val[0];
        double s_v_ver = cv::sum(v_ver).val[0];
        double s_f_hor = cv::sum(d_f_hor).val[0];
        double s_v_hor = cv::sum(v_hor).val[0];

		// normalize results
        double b_f_ver = (s_f_ver - s_v_ver) / s_f_ver;
        double b_f_hor = (s_f_hor - s_v_hor) / s_f_hor;

		// compute final score: select more annyoing blur
        double blur_score = std::max(b_f_ver, b_f_hor);

		// invert score for 1.0=best and 0.0=worst
        blur_score = 1.0 - blur_score;

        return blur_score;
	}

} // namespace nv


#include "CppSimulation.hpp"

#include <boost/numeric/odeint.hpp>
#include <functional>
#include <algorithm>
#include <vector>
#include <exception>

namespace pysim{

struct EarlyBreakException : public std::exception {
    virtual const char* what() const throw() {
        return "A comparison resulted in an early break of the simulation";
    }
}earlybreak;

Simulation::Simulation(void)
    :currentTime(0) {
}

Simulation::~Simulation(void) {
}

void Simulation::doGenericStep(const std::vector< double > &state,
                               std::vector< double > &der,
                               double time) {
    // Copy the state variables to the actual systems
    auto si = states.cbegin();
    for (auto i = state.cbegin(); i != state.cend(); ++i) {
        **si++ = *i;
    }

    // Copy state outputs from all systems to their respective inputs
    // Since they are states, and constant input to this function this
    // is done before the timestep calulations.
    for ( auto syst = systems.begin(); syst != systems.end(); ++syst ) {
		(*syst)->preStep();
        (*syst)->copystateoutputs();
		(*syst)->copyoutputs();
    }

    // Do the time step for all systems, and copy the variable
    // outputs after the time step.
    for ( auto syst = systems.begin(); syst != systems.end(); ++syst ) {
        (*syst)->doStep(time);
        (*syst)->copyoutputs();
    }

    // Copy the systems derivate variables to the
    // solvers (the input to this function).
    auto di = der.begin();
    for ( auto i = ders.cbegin(); i != ders.cend(); ++i ) {
        *(di++) = **i;
    }
}

void Simulation::observer(const std::vector<double> &state, double time) {
    currentTime = time;

    // Copy the state variables to the actual systems
    auto si = states.cbegin();
    for (auto i = state.cbegin(); i != state.cend(); ++i) {
        **si++ = *i;
    }

    for ( auto syst = systems.cbegin(); syst != systems.end(); ++syst ) {
        (*syst)->postStep();
        (*syst)->doStoreStep(time);
    }
    for (   auto syst = discreteSystems.cbegin();
            syst != discreteSystems.end();
            ++syst) {
        (*syst)->postStep();
        (*syst)->doStoreStep(time);
    }

    bool compare_break = false;
    for (auto syst = systems.cbegin(); syst != systems.end(); ++syst) {
        if ((*syst)->do_comparison()) {
            compare_break = true;
        }
    }
    if (compare_break) {
        throw earlybreak;
    }
}

void Simulation::prepare_first_sim() {
    // Copy system states to this objects list
    for ( auto syst = systems.cbegin(); syst != systems.end(); ++syst ) {
        (*syst)->preSim();

        std::vector<double*> sp = (*syst)->getStatePointers();
        std::copy(sp.cbegin(), sp.cend(), std::back_inserter(states));

        std::vector<double*> dp = (*syst)->getDerPointers();
        std::copy(dp.cbegin(), dp.cend(), std::back_inserter(ders));

        if (ders.size() != states.size()) {
            throw std::runtime_error("Unequal states and ders");
        }

    }
    for (   auto syst = discreteSystems.cbegin();
            syst != discreteSystems.end();
            ++syst) {
        (*syst)->preSim();
    }
}

bool myfn(SimulatableSystemInterface* i, SimulatableSystemInterface* j) {
    return i->getNextUpdateTime() < j->getNextUpdateTime();
}

void Simulation::simulate(double duration,
                          double dt,
                          char* solvername,
                          double abs_err,
                          double rel_err,
                          bool dense_output) {
    if (currentTime == 0.0) {
        prepare_first_sim();
    }
    double endtime = currentTime + duration;

    if (discreteSystems.size() > 0) {
        auto si = std::min_element(discreteSystems.begin(),
                                   discreteSystems.end(),
                                   myfn);
        double inter_endtime = (*si)->getNextUpdateTime();
        while (inter_endtime < endtime) {
            if (inter_endtime > 0) {
                setup_odeint(inter_endtime, dt, solvername,
                             abs_err, rel_err, dense_output);
            }
            do {
                (*si)->doStep(currentTime);
                std::vector<double*> states = (*si)->getStatePointers();
                std::vector<double*> ders = (*si)->getDerPointers();

                //Copy states to der
                auto state_iter = states.begin();
                auto der_iter = ders.begin();
                while (state_iter != states.end()) {
                    **state_iter++ = **der_iter++;
                }

                (*si)->copystateoutputs();
                (*si)->copyoutputs();
                si = std::min_element(discreteSystems.begin(),
                                      discreteSystems.end(),
                                      myfn);
            } while ((*si)->getNextUpdateTime() == inter_endtime);

            si = std::min_element(discreteSystems.begin(),
                                  discreteSystems.end(),
                                  myfn);
            inter_endtime = (*si)->getNextUpdateTime();
        }
    }
    setup_odeint(endtime, dt, solvername, abs_err, rel_err, dense_output);
}

void Simulation::setup_odeint(double endtime,
                              double dt,
                              char* solvername,
                              double abs_err,
                              double rel_err,
                              bool dense_output) {
    using namespace boost::numeric::odeint;
    using std::vector;

    namespace pl = std::placeholders;
    auto ff = std::bind(&Simulation::doGenericStep,
                        this,
                        pl::_1, pl::_2, pl::_3);
    auto sf = std::bind(&Simulation::observer, this, pl::_1, pl::_2);

    vector< double > state;
    for ( auto i = states.cbegin(); i != states.cend(); ++i ) {
        state.push_back(**i);
    }
    try {
        if (strcmp(solvername, "rk4") == 0) {
            runge_kutta4<vector< double > > stepper;
            integrate_const(stepper, ff, state, currentTime, endtime, dt, sf);
        } else if (strcmp(solvername, "ck54") == 0) {
            typedef runge_kutta_cash_karp54<vector<double> > error_stepper_type;
            auto cs = make_controlled <error_stepper_type>(abs_err, rel_err);
            integrate_adaptive(cs, ff, state, currentTime, endtime, dt, sf);
        } else if (strcmp(solvername, "dp5") == 0) {
            typedef runge_kutta_dopri5<vector<double> > error_stepper_type;
            if (dense_output) {
                auto cs = make_dense_output(abs_err, rel_err, error_stepper_type());
                integrate_const(cs, ff, state, currentTime, endtime, dt, sf);
            } else {
                auto cs = make_controlled <error_stepper_type>(abs_err, rel_err);
                integrate_adaptive(cs, ff, state, currentTime, endtime, dt, sf);
            }
        } else {
            char errmsg[50];
            snprintf(errmsg, sizeof(errmsg), "Unknown solver: %s", solvername);
            throw std::invalid_argument(errmsg);
        }
    } catch (EarlyBreakException &eb) {
        printf("%s\n",eb.what());
    }
}

void Simulation::addSystem(SimulatableSystemInterface* system) {
    if (system->getDiscrete()) {
        discreteSystems.push_back(system);
    } else {
        systems.push_back(system);
    }
}

}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_DETAIL_RANGE_GENERATOR_INCLUDE
#define MTL_DETAIL_RANGE_GENERATOR_INCLUDE

#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/glas_tag.hpp>
#include <boost/numeric/mtl/utility/complexity.hpp>
#include <boost/numeric/mtl/detail/base_cursor.hpp>
#include <boost/mpl/less.hpp>

namespace mtl { namespace traits { namespace detail {

    /// Range generator that traverses all elements of some densely stored collection
    /** - Or contiguous parts of such collection
        - Works for matrices and vectors when derived from contiguous_memory_block
    **/
    template <typename Collection, typename Cursor, typename Complexity>
    struct dense_element_range_generator
    {
	typedef Complexity          complexity;
	typedef Cursor              type;
	static int const            level = 1;

	type begin(Collection const& collection)
	{
	    return collection.elements();
	}
	type end(Collection const& collection)
	{
	    return collection.elements() + collection.used_memory();
	}
    };

    /// Range generator that traverses all elements of some collection stored in strides
    template <typename Collection, typename Ref, typename Traversor>
    struct strided_element_range_generator
    {
	typedef complexity_classes::linear  complexity;
	typedef Traversor                   type;
	static int const                    level = 1;

	type begin(Ref& c)
	{
	    return type(c.address_data(), c.stride());
	}
	type end(Ref& c)
	{
		using mtl::size;
	    return type(c.address_data() + size(c) + c.stride(), c.stride());
	}
    };

    // Like above over all elements but in terms of offsets
    // Also with reference to collection in cursor
    template <typename Matrix, typename Cursor, typename Complexity>
    struct all_offsets_range_generator
    {
	typedef Complexity          complexity;
	typedef Cursor              type;
	static int const            level = 1;

	type begin(Matrix const& matrix) const
	{
	    return type(matrix, 0);
	}
	type end(Matrix const& matrix) const
	{
	    return type(matrix, matrix.nnz());
	}
    };

    // Cursor to some submatrix (e.g. row, column, block matrix, block row)
    // This cursor is intended to be used by range generators to iterate
    // over subsets of the submatrix this cursor refers to.
    // For instance if this cursor refers to a row then a range
    // can iterate over the elements in this row.
    // If this cursor refers to a block then a range can iterate over the rows in this block.
    // The level of a generated cursor must be of course at least one level less
    // The tag serves to dispatching between row and column cursors
    template <typename Matrix, typename Tag, int Level>
    struct sub_matrix_cursor
      : mtl::detail::base_cursor<std::size_t>
    {
	typedef sub_matrix_cursor                        self;
	typedef mtl::detail::base_cursor<std::size_t>    base;
	typedef Matrix                                   ref_type;
	static int const                                 level = Level;

	sub_matrix_cursor(std::size_t i, Matrix const& c)
	    : base(i), ref(c)
	{}

	self operator+(std::size_t offset) const
	{
	    return self(key + offset, ref);
	}

	// otherwise base_cursor returns an int and ranged for doesn't work
	// for getting the key of base_cursor use this->value()
	self operator*() const { return *this; }

	Matrix const& ref;
    };

    // Key for canonically referring to its elements with row and column index
    template <typename Matrix>
    struct matrix_element_key
    {
	typedef typename Collection<Matrix>::size_type           size_type;
	typedef matrix_element_key                               self;

	matrix_element_key(Matrix const& ref, std::size_t r, std::size_t c) : ref(ref)
	{
	    indices[0]= size_type(r); indices[1]= size_type(c);
	}

	bool operator==(const self& cc) const { return &ref == &cc.ref && indices[0] == cc.indices[0] && indices[1] == cc.indices[1]; }
	bool operator!=(const self& cc) const { return !(*this == cc); }

	size_type indices[2];
	Matrix const& ref;
    };

    // Cursor for canonically referring to its elements with row and column index
    // Increments row for pos==0 and column for pos==1
    // Referring operator returns matrix_element_key
    template <typename Matrix, int pos>
    struct matrix_element_cursor
    {
	typedef typename Collection<Matrix>::size_type           size_type;
	typedef matrix_element_cursor                            self;
	typedef matrix_element_key<Matrix>                       key_type;
	static int const            level = 2;

	matrix_element_cursor(Matrix const& ref, size_type r, size_type c) : ref(ref)
	{
	    indices[0]= r; indices[1]= c;
	}

	key_type operator*() const { return key_type(ref, indices[0], indices[1]); }

	self& operator++() { ++indices[pos]; return *this; }
	self operator++(int) { self tmp(*this); ++indices[pos]; return tmp; }
	template <typename T> self& operator+=(T n) { indices[pos] += size_type(n); return *this; }
	template <typename T> self& operator+(T n) const { self tmp = *this; tmp += n; return tmp; }

	self& operator--() { indices[pos]--; return *this; }
	self operator--(int) { self tmp(*this); indices[pos]--; return tmp; }
	template <typename T> self& operator-=(T n) { indices[pos] -= size_type(n); return *this; }
	template <typename T> self& operator-(T n) const { self tmp = *this; tmp -= n; return tmp; }

	bool operator==(const self& cc) const { return &ref == &cc.ref && indices[0] == cc.indices[0] && indices[1] == cc.indices[1]; }
	bool operator!=(const self& cc) const { return !(*this == cc); }

	size_type indices[2];
	Matrix const& ref;
    };

    template <typename Matrix, typename Complexity, int Level>
    struct all_rows_range_generator
    {
	typedef Complexity                                       complexity;
	static int const                                         level = Level;
	typedef Matrix                                           ref_type;
	typedef sub_matrix_cursor<Matrix, glas::tag::row, Level> type;
	typedef typename Collection<Matrix>::size_type           size_type;

	type begin(Matrix const& c) const
	{
	    return type(0, c); // return type(c.begin_row(), c); get rid of obsolete stuff
	}
	type end(Matrix const& c) const
	{
		using mtl::num_rows; using mtl::mat::num_rows;
	    return type(int(num_rows(c)), c); //return type(c.end_row(), c);
	}
	type lower_bound(Matrix const& c, size_type position) const
	{
		using mtl::num_rows;
	    return type(std::min(num_rows(c), position), c);
	}
    };

    template <typename Cursor>
    struct all_cols_in_row_range_generator
    {
	typedef complexity_classes::linear                       complexity;
	static int const                                         level = 1;
	typedef typename Cursor::ref_type                        ref_type;
	typedef typename Collection<ref_type>::size_type         size_type;

	typedef matrix_element_cursor<ref_type, 1>               type;

	type begin(Cursor const& c) const { return type(c.ref, c.value(), size_type(0)); }
	type end(Cursor const& c) const { using mtl::num_cols; return type(c.ref, c.value(), num_cols(c.ref)); }
	type lower_bound(Cursor const& c, size_type position) const
	{
		using mtl::num_cols;
	    return type(c.ref, c.value, std::min(num_cols(c.ref), position));
	}
    };

    template <typename Matrix, typename Complexity, int Level>
    struct all_cols_range_generator
    {
	typedef Complexity          complexity;
	static int const            level = Level;
	typedef sub_matrix_cursor<Matrix, glas::tag::col, Level> type;
	typedef typename Collection<Matrix>::size_type           size_type;

	type begin(Matrix const& c) const
	{
	    return type(0, c); // return type(c.begin_col(), c);
	}
	type end(Matrix const& c) const
	{
		using mtl::num_cols;
	    return type(num_cols(c), c); // return type(c.end_col(), c);
	}
	type lower_bound(Matrix const& c, size_type position) const
	{
		using mtl::num_cols;
	    return type(std::min(num_cols(c), position), c);
	}
    };

    template <typename Cursor>
    struct all_rows_in_col_range_generator
    {
	typedef complexity_classes::linear                       complexity;
	static int const            level = 1;
	typedef typename Cursor::ref_type                        ref_type;
	typedef typename Collection<ref_type>::size_type         size_type;

	typedef matrix_element_cursor<ref_type, 0> type;

	type begin(Cursor const& c) const { return type(c.ref, 0, c.value()); }
	type end(Cursor const& c) const { using mtl::num_rows; return type(c.ref, num_rows(c.ref), c.value()); }
	type lower_bound(Cursor const& c, size_type position) const
	{
		using mtl::num_rows;
	    return type(c.ref, std::min(num_rows(c.ref), position), c.value());
	}
    };

    // Use RangeGenerator for Collection by applying to .ref
    template <typename Coll, typename RangeGenerator>
    struct referred_range_generator
    {
	typedef typename RangeGenerator::complexity complexity;
	static int const                            level = RangeGenerator::level;
	typedef typename RangeGenerator::type       type;
	typedef typename Collection<Coll>::size_type  size_type;

	type begin(const Coll& c)
	{
	    return RangeGenerator().begin(c.ref);
	}

	type end(const Coll& c)
	{
	    return RangeGenerator().end(c.ref);
	}
	type lower_bound(const Coll& c, size_type position)
	{
	    return RangeGenerator().lower_bound(c.ref, position);
	}
    };

} // namespace detail

    namespace range {

	template <typename Range1, typename Range2>
	struct min
	    : public boost::mpl::if_<
	            boost::mpl::less<
	                   typename Range1::complexity,
	                   typename Range2::complexity>
	          , Range1
	          , Range2
	      >
	{};
    }

}} // namespace mtl::traits

#endif // MTL_DETAIL_RANGE_GENERATOR_INCLUDE