/*
 * Project: MoleCuilder
 * Description: creates and alters molecular systems
 * Copyright (C)  2014 Frederik Heber. All rights reserved.
 *
 *
 *   This file is part of MoleCuilder.
 *
 *    MoleCuilder is free software: you can redistribute it and/or modify
 *    it under the terms of the GNU General Public License as published by
 *    the Free Software Foundation, either version 2 of the License, or
 *    (at your option) any later version.
 *
 *    MoleCuilder is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *    GNU General Public License for more details.
 *
 *    You should have received a copy of the GNU General Public License
 *    along with MoleCuilder.  If not, see <http://www.gnu.org/licenses/>.
 */

/*
 * SphericalPointDistribution.cpp
 *
 *  Created on: May 30, 2014
 *      Author: heber
 */

// include config.h
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

//#include "CodePatterns/MemDebug.hpp"

#include "SphericalPointDistribution.hpp"

#include "CodePatterns/Assert.hpp"
#include "CodePatterns/IteratorAdaptors.hpp"
#include "CodePatterns/Log.hpp"
#include "CodePatterns/toString.hpp"

#include <algorithm>
#include <boost/assign.hpp>
#include <cmath>
#include <functional>
#include <iterator>
#include <limits>
#include <list>
#include <numeric>
#include <vector>
#include <map>

#include "LinearAlgebra/Line.hpp"
#include "LinearAlgebra/Plane.hpp"
#include "LinearAlgebra/RealSpaceMatrix.hpp"
#include "LinearAlgebra/Vector.hpp"

using namespace boost::assign;

// static entities
const double SphericalPointDistribution::SQRT_3(sqrt(3.0));
const double SphericalPointDistribution::warn_amplitude = 1e-2;
const double SphericalPointDistribution::L1THRESHOLD = 1e-2;
const double SphericalPointDistribution::L2THRESHOLD = 2e-1;

typedef std::vector<double> DistanceArray_t;

// class generator: taken from www.cplusplus.com example std::generate
struct c_unique {
  unsigned int current;
  c_unique() {current=0;}
  unsigned int operator()() {return current++;}
} UniqueNumber;

struct c_unique_list {
  unsigned int current;
  c_unique_list() {current=0;}
  std::list<unsigned int> operator()() {return std::list<unsigned int>(1, current++);}
} UniqueNumberList;

/** Calculate the center of a given set of points in \a _positions but only
 * for those indicated by \a _indices.
 *
 */
inline
Vector calculateGeographicMidpoint(
    const SphericalPointDistribution::VectorArray_t &_positions,
    const SphericalPointDistribution::IndexList_t &_indices)
{
  Vector Center;
  Center.Zero();
  for (SphericalPointDistribution::IndexList_t::const_iterator iter = _indices.begin();
      iter != _indices.end(); ++iter)
    Center += _positions[*iter];
  if (!_indices.empty())
    Center *= 1./(double)_indices.size();

  return Center;
}

inline
double calculateMinimumDistance(
    const Vector &_center,
    const SphericalPointDistribution::VectorArray_t &_points,
    const SphericalPointDistribution::IndexList_t & _indices)
{
  double MinimumDistance = 0.;
  for (SphericalPointDistribution::IndexList_t::const_iterator iter = _indices.begin();
      iter != _indices.end(); ++iter) {
    const double angle = _center.Angle(_points[*iter]);
    MinimumDistance += angle*angle;
  }
  return sqrt(MinimumDistance);
}

/** Calculates the center of minimum distance for a given set of points \a _points.
 *
 * According to http://www.geomidpoint.com/calculation.html this goes a follows:
 * -# Let CurrentPoint be the geographic midpoint found in Method A. this is used as the starting point for the search.
 * -# Let MinimumDistance be the sum total of all distances from the current point to all locations in 'Your Places'.
 * -# Find the total distance between each location in 'Your Places' and all other locations in 'Your Places'. If any one of these locations has a new smallest distance then that location becomes the new CurrentPoint and MinimumDistance.
 * -# Let TestDistance be PI/2 radians (6225 miles or 10018 km).
 * -# Find the total distance between each of 8 test points and all locations in 'Your Places'. The test points are arranged in a circular pattern around the CurrentPoint at a distance of TestDistance to the north, northeast, east, southeast, south, southwest, west and northwest.
 * -# If any of these 8 points has a new smallest distance then that point becomes the new CurrentPoint and MinimumDistance and go back to step 5 using that point.
 * -# If none of the 8 test points has a new smallest distance then divide TestDistance by 2 and go back to step 5 using the same point.
 * -# Repeat steps 5 to 7 until no new smallest distance can be found or until TestDistance is less than 0.00000002 radians.
 *
 * \param _points set of points
 * \return Center of minimum distance for all these points, is always of length 1
 */
Vector SphericalPointDistribution::calculateCenterOfMinimumDistance(
    const SphericalPointDistribution::VectorArray_t &_positions,
    const SphericalPointDistribution::IndexList_t &_indices)
{
  ASSERT( _positions.size() >= _indices.size(),
      "calculateCenterOfMinimumDistance() - less positions than indices given.");
  Vector center(1.,0.,0.);

  /// first treat some special cases
  // no positions given: return x axis vector (NOT zero!)
  {
    if (_indices.empty())
      return center;
    // one position given: return it directly
    if (_positions.size() == (size_t)1)
      return _positions[0];
    // two positions on a line given: return closest point to (1.,0.,0.)
    if (fabs(_positions[0].ScalarProduct(_positions[1]) + 1.)
        < std::numeric_limits<double>::epsilon()*1e4) {
      Vector candidate;
      if (_positions[0].ScalarProduct(center) > _positions[1].ScalarProduct(center))
        candidate = _positions[0];
      else
        candidate = _positions[1];
      // non-uniqueness: all positions on great circle, normal to given line are valid
      // so, we just pick one because returning a unique point is topmost priority
      Vector normal;
      normal.GetOneNormalVector(candidate);
      Vector othernormal = candidate;
      othernormal.VectorProduct(normal);
      // now both normal and othernormal describe the plane containing the great circle
      Plane greatcircle(normal, zeroVec, othernormal);
      // check which axis is contained and pick the one not
      if (greatcircle.isContained(center)) {
        center = Vector(0.,1.,0.);
        if (greatcircle.isContained(center))
          center = Vector(0.,0.,1.);
        // now we are done cause a plane cannot contain all three axis vectors
      }
      center = greatcircle.getClosestPoint(candidate);
      // assure length of 1
      center.Normalize();
    }
  }

  // start with geographic midpoint
  center = calculateGeographicMidpoint(_positions, _indices);
  if (!center.IsZero()) {
    center.Normalize();
    LOG(4, "DEBUG: Starting with geographical midpoint of " << _positions << " under indices "
        << _indices << " is " << center);
  } else {
    // any point is good actually
    center = _positions[0];
    LOG(4, "DEBUG: Starting with first position " << center);
  }

  // calculate initial MinimumDistance
  double MinimumDistance = calculateMinimumDistance(center, _positions, _indices);
  LOG(5, "DEBUG: MinimumDistance to this center is " << MinimumDistance);

  // check all present points whether they may serve as a better center
  for (SphericalPointDistribution::IndexList_t::const_iterator iter = _indices.begin();
      iter != _indices.end(); ++iter) {
    const Vector &centerCandidate = _positions[*iter];
    const double candidateDistance = calculateMinimumDistance(centerCandidate, _positions, _indices);
    if (candidateDistance < MinimumDistance) {
      MinimumDistance = candidateDistance;
      center = centerCandidate;
      LOG(5, "DEBUG: new MinimumDistance to current test point " << MinimumDistance
          << " is " << center);
    }
  }
  LOG(5, "DEBUG: new MinimumDistance to center " << center << " is " << MinimumDistance);

  // now iterate over TestDistance
  double TestDistance = Vector(1.,0.,0.).Angle(Vector(0.,1.,0.));
//  LOG(6, "DEBUG: initial TestDistance is " << TestDistance);

  const double threshold = sqrt(std::numeric_limits<double>::epsilon());
  // check each of eight test points at N, NE, E, SE, S, SW, W, NW
  typedef std::vector<double> angles_t;
  angles_t testangles;
  testangles += 0./180.*M_PI, 45./180.*M_PI, 90./180.*M_PI, 135./180.*M_PI,
      180./180.*M_PI, 225./180.*M_PI, 270./180.*M_PI, 315./180.*M_PI;
  while (TestDistance > threshold) {
    Vector OneNormal;
    OneNormal.GetOneNormalVector(center);
    Line RotationAxis(zeroVec, OneNormal);
    Vector North = RotationAxis.rotateVector(center,TestDistance);
    Line CompassRose(zeroVec, center);
    bool updatedflag = false;
    for (angles_t::const_iterator angleiter = testangles.begin();
        angleiter != testangles.end(); ++angleiter) {
      Vector centerCandidate = CompassRose.rotateVector(North, *angleiter);
//      centerCandidate.Normalize();
      const double candidateDistance = calculateMinimumDistance(centerCandidate, _positions, _indices);
      if (candidateDistance < MinimumDistance) {
        MinimumDistance = candidateDistance;
        center = centerCandidate;
        updatedflag = true;
        LOG(5, "DEBUG: new MinimumDistance to test point at " << *angleiter/M_PI*180.
            << "° is " << MinimumDistance);
      }
    }

    // if no new point, decrease TestDistance
    if (!updatedflag) {
      TestDistance *= 0.5;
//      LOG(6, "DEBUG: TestDistance is now " << TestDistance);
    }
  }
  LOG(4, "DEBUG: Final MinimumDistance to center " << center << " is " << MinimumDistance);

  return center;
}

Vector calculateCenterOfMinimumDistance(
    const SphericalPointDistribution::PolygonWithIndices &_points)
{
  return SphericalPointDistribution::calculateCenterOfMinimumDistance(_points.polygon, _points.indices);
}

/** Calculate the center of a given set of points in \a _positions but only
 * for those indicated by \a _indices.
 *
 */
inline
Vector calculateCenter(
    const SphericalPointDistribution::VectorArray_t &_positions,
    const SphericalPointDistribution::IndexList_t &_indices)
{
//  Vector Center;
//  Center.Zero();
//  for (SphericalPointDistribution::IndexList_t::const_iterator iter = _indices.begin();
//      iter != _indices.end(); ++iter)
//    Center += _positions[*iter];
//  if (!_indices.empty())
//    Center *= 1./(double)_indices.size();
//
//  return Center;
  return SphericalPointDistribution::calculateCenterOfMinimumDistance(_positions, _indices);
}

/** Calculate the center of a given set of points in \a _positions but only
 * for those indicated by \a _indices.
 *
 */
inline
Vector calculateCenter(
    const SphericalPointDistribution::PolygonWithIndices &_polygon)
{
  return calculateCenter(_polygon.polygon, _polygon.indices);
}

inline
DistanceArray_t calculatePairwiseDistances(
    const SphericalPointDistribution::VectorArray_t &_points,
    const SphericalPointDistribution::IndexTupleList_t &_indices
    )
{
  DistanceArray_t result;
  for (SphericalPointDistribution::IndexTupleList_t::const_iterator firstiter = _indices.begin();
      firstiter != _indices.end(); ++firstiter) {

    // calculate first center from possible tuple of indices
    Vector FirstCenter;
    ASSERT(!firstiter->empty(), "calculatePairwiseDistances() - there is an empty tuple.");
    if (firstiter->size() == 1) {
      FirstCenter = _points[*firstiter->begin()];
    } else {
      FirstCenter = calculateCenter( _points, *firstiter);
      if (!FirstCenter.IsZero())
        FirstCenter.Normalize();
    }

    for (SphericalPointDistribution::IndexTupleList_t::const_iterator seconditer = firstiter;
        seconditer != _indices.end(); ++seconditer) {
      if (firstiter == seconditer)
        continue;

      // calculate second center from possible tuple of indices
      Vector SecondCenter;
      ASSERT(!seconditer->empty(), "calculatePairwiseDistances() - there is an empty tuple.");
      if (seconditer->size() == 1) {
        SecondCenter = _points[*seconditer->begin()];
      } else {
        SecondCenter = calculateCenter( _points, *seconditer);
        if (!SecondCenter.IsZero())
          SecondCenter.Normalize();
      }

      // calculate distance between both centers
      double distance = 2.; // greatest distance on surface of sphere with radius 1.
      if ((!FirstCenter.IsZero()) && (!SecondCenter.IsZero()))
        distance = (FirstCenter - SecondCenter).NormSquared();
      result.push_back(distance);
    }
  }
  return result;
}

/** Decides by an orthonormal third vector whether the sign of the rotation
 * angle should be negative or positive.
 *
 * \return -1 or 1
 */
inline
double determineSignOfRotation(
    const Vector &_oldPosition,
    const Vector &_newPosition,
    const Vector &_RotationAxis
    )
{
  Vector dreiBein(_oldPosition);
  dreiBein.VectorProduct(_RotationAxis);
  ASSERT( !dreiBein.IsZero(), "determineSignOfRotation() - dreiBein is zero.");
  dreiBein.Normalize();
  const double sign =
      (dreiBein.ScalarProduct(_newPosition) < 0.) ? -1. : +1.;
  LOG(6, "DEBUG: oldCenter on plane is " << _oldPosition
      << ", newCenter on plane is " << _newPosition
      << ", and dreiBein is " << dreiBein);
  return sign;
}

/** Convenience function to recalculate old and new center for determining plane
 * rotation.
 */
inline
void calculateOldAndNewCenters(
    Vector &_oldCenter,
    Vector &_newCenter,
    const SphericalPointDistribution::PolygonWithIndices &_referencepositions,
    const SphericalPointDistribution::PolygonWithIndices &_currentpositions)
{
  _oldCenter = calculateCenter(_referencepositions.polygon, _referencepositions.indices);
  // C++11 defines a copy_n function ...
  _newCenter = calculateCenter( _currentpositions.polygon, _currentpositions.indices);
}
/** Returns squared L2 error of the given \a _Matching.
 *
 * We compare the pair-wise distances of each associated matching
 * and check whether these distances each match between \a _old and
 * \a _new.
 *
 * \param _old first set of returnpolygon (fewer or equal to \a _new)
 * \param _new second set of returnpolygon
 * \param _Matching matching between the two sets
 * \return pair with L1 and squared L2 error
 */
std::pair<double, double> SphericalPointDistribution::calculateErrorOfMatching(
    const VectorArray_t &_old,
    const VectorArray_t &_new,
    const IndexTupleList_t &_Matching)
{
  std::pair<double, double> errors( std::make_pair( 0., 0. ) );

  if (_Matching.size() > 1) {
    LOG(5, "INFO: Matching is " << _Matching);

    // calculate all pair-wise distances
    IndexTupleList_t keys(_old.size(), IndexList_t() );
    std::generate (keys.begin(), keys.end(), UniqueNumberList);

    const DistanceArray_t firstdistances = calculatePairwiseDistances(_old, keys);
    const DistanceArray_t seconddistances = calculatePairwiseDistances(_new, _Matching);

    ASSERT( firstdistances.size() == seconddistances.size(),
        "calculateL2ErrorOfMatching() - mismatch in pair-wise distance array sizes.");
    DistanceArray_t::const_iterator firstiter = firstdistances.begin();
    DistanceArray_t::const_iterator seconditer = seconddistances.begin();
    for (;(firstiter != firstdistances.end()) && (seconditer != seconddistances.end());
        ++firstiter, ++seconditer) {
      const double gap = fabs(*firstiter - *seconditer);
      // L1 error
      if (errors.first < gap)
        errors.first = gap;
      // L2 error
      errors.second += gap*gap;
    }
  } else {
    // check whether we have any zero centers: Combining points on new sphere may result
    // in zero centers
    for (SphericalPointDistribution::IndexTupleList_t::const_iterator iter = _Matching.begin();
        iter != _Matching.end(); ++iter) {
      if ((iter->size() != 1) && (calculateCenter( _new, *iter).IsZero())) {
        errors.first = 2.;
        errors.second = 2.;
      }
    }
  }
  LOG(4, "INFO: Resulting errors for matching (L1, L2): "
      << errors.first << "," << errors.second << ".");

  return errors;
}

SphericalPointDistribution::Polygon_t SphericalPointDistribution::removeMatchingPoints(
    const PolygonWithIndices &_points
    )
{
  SphericalPointDistribution::Polygon_t remainingpoints;
  IndexArray_t indices(_points.indices.begin(), _points.indices.end());
  std::sort(indices.begin(), indices.end());
  LOG(4, "DEBUG: sorted matching is " << indices);
  IndexArray_t remainingindices(_points.polygon.size(), -1);
  std::generate(remainingindices.begin(), remainingindices.end(), UniqueNumber);
  IndexArray_t::iterator remainiter = std::set_difference(
      remainingindices.begin(), remainingindices.end(),
      indices.begin(), indices.end(),
      remainingindices.begin());
  remainingindices.erase(remainiter, remainingindices.end());
  LOG(4, "DEBUG: remaining indices are " << remainingindices);
  for (IndexArray_t::const_iterator iter = remainingindices.begin();
      iter != remainingindices.end(); ++iter) {
    remainingpoints.push_back(_points.polygon[*iter]);
  }

  return remainingpoints;
}

/** Recursive function to go through all possible matchings.
 *
 * \param _MCS structure holding global information to the recursion
 * \param _matching current matching being build up
 * \param _indices contains still available indices
 * \param _remainingweights current weights to fill (each weight a place)
 * \param _remainiter iterator over the weights, indicating the current position we match
 * \param _matchingsize
 */
void SphericalPointDistribution::recurseMatchings(
    MatchingControlStructure &_MCS,
    IndexTupleList_t &_matching,
    IndexList_t _indices,
    WeightsArray_t &_remainingweights,
    WeightsArray_t::iterator _remainiter,
    const unsigned int _matchingsize
    )
{
  LOG(5, "DEBUG: Recursing with current matching " << _matching
      << ", remaining indices " << _indices
      << ", and remaining weights " << _matchingsize);
  if (!_MCS.foundflag) {
    LOG(5, "DEBUG: Current matching has size " << _matching.size() << ", places left " << _matchingsize);
    if (_matchingsize > 0) {
      // go through all indices
      for (IndexList_t::iterator iter = _indices.begin();
          (iter != _indices.end()) && (!_MCS.foundflag);) {

        // check whether we can stay in the current bin or have to move on to next one
        if (*_remainiter == 0) {
          // we need to move on
          if (_remainiter != _remainingweights.end()) {
            ++_remainiter;
          } else {
            // as we check _matchingsize > 0 this should be impossible
            ASSERT( 0, "recurseMatchings() - we must not come to this position.");
          }
        }

        // advance in matching to current bin to fill in
        const size_t OldIndex = std::distance(_remainingweights.begin(), _remainiter);
        while (_matching.size() <= OldIndex) { // add empty lists of new bin is opened
          LOG(6, "DEBUG: Extending _matching.");
          _matching.push_back( IndexList_t() );
        }
        IndexTupleList_t::iterator filliniter = _matching.begin();
        std::advance(filliniter, OldIndex);

        // check whether connection between bins' indices and candidate is satisfied
        {
          adjacency_t::const_iterator finder = _MCS.adjacency.find(*iter);
          ASSERT( finder != _MCS.adjacency.end(),
              "recurseMatchings() - "+toString(*iter)+" is not in adjacency list.");
          if ((!filliniter->empty())
              && (finder->second.find(*filliniter->begin()) == finder->second.end())) {
            LOG(5, "DEBUG; Skipping index " << *iter
                << " as is not connected to current set." << *filliniter << ".");
            ++iter; // note that for loop does not contain incrementor
            continue;
          }
        }

        // add index to matching
        filliniter->push_back(*iter);
        --(*_remainiter);
        LOG(6, "DEBUG: Adding " << *iter << " to matching at " << OldIndex << ".");
        // remove index but keep iterator to position (is the next to erase element)
        IndexList_t::iterator backupiter = _indices.erase(iter);
        // recurse with decreased _matchingsize
        recurseMatchings(_MCS, _matching, _indices, _remainingweights, _remainiter, _matchingsize-1);
        // re-add chosen index and reset index to new position
        _indices.insert(backupiter, filliniter->back());
        iter = backupiter;
        // remove index from _matching to make space for the next one
        filliniter->pop_back();
        ++(*_remainiter);
      }
      // gone through all indices then exit recursion
      if (_matching.empty())
        _MCS.foundflag = true;
    } else {
      LOG(4, "INFO: Found matching " << _matching);
      // calculate errors
      std::pair<double, double> errors = calculateErrorOfMatching(
          _MCS.oldpoints, _MCS.newpoints, _matching);
      if (errors.first < L1THRESHOLD) {
        _MCS.bestmatching = _matching;
        _MCS.foundflag = true;
      } else if (_MCS.bestL2 > errors.second) {
        _MCS.bestmatching = _matching;
        _MCS.bestL2 = errors.second;
      }
    }
  }
}

SphericalPointDistribution::MatchingControlStructure::MatchingControlStructure(
    const adjacency_t &_adjacency,
    const VectorArray_t &_oldpoints,
    const VectorArray_t &_newpoints,
    const WeightsArray_t &_weights
    ) :
  foundflag(false),
  bestL2(std::numeric_limits<double>::max()),
  adjacency(_adjacency),
  oldpoints(_oldpoints),
  newpoints(_newpoints),
  weights(_weights)
{}

/** Finds combinatorially the best matching between points in \a _polygon
 * and \a _newpolygon.
 *
 * We find the matching with the smallest L2 error, where we break when we stumble
 * upon a matching with zero error.
 *
 * As points in \a _polygon may be have a weight greater 1 we have to match it to
 * multiple points in \a _newpolygon. Eventually, these multiple points are combined
 * for their center of weight, which is the only thing follow-up function see of
 * these multiple points. Hence, we actually modify \a _newpolygon in the process
 * such that the returned IndexList_t indicates a bijective mapping in the end.
 *
 * \sa recurseMatchings() for going through all matchings
 *
 * \param _polygon here, we have indices 0,1,2,...
 * \param _newpolygon and here we need to find the correct indices
 * \return list of indices: first in \a _polygon goes to first index for \a _newpolygon
 */
SphericalPointDistribution::IndexList_t SphericalPointDistribution::findBestMatching(
    const WeightedPolygon_t &_polygon
    )
{
  // transform lists into arrays
  VectorArray_t oldpoints;
  VectorArray_t newpoints;
  WeightsArray_t weights;
  for (WeightedPolygon_t::const_iterator iter = _polygon.begin();
      iter != _polygon.end(); ++iter) {
    oldpoints.push_back(iter->first);
    weights.push_back(iter->second);
  }
  newpoints.insert(newpoints.begin(), points.begin(), points.end() );
  MatchingControlStructure MCS(adjacency, oldpoints, newpoints, weights);

  // search for bestmatching combinatorially
  {
    // translate polygon into vector to enable index addressing
    IndexList_t indices(points.size());
    std::generate(indices.begin(), indices.end(), UniqueNumber);
    IndexTupleList_t matching;

    // walk through all matchings
    WeightsArray_t remainingweights = MCS.weights;
    unsigned int placesleft = std::accumulate(remainingweights.begin(), remainingweights.end(), 0);
    recurseMatchings(MCS, matching, indices, remainingweights, remainingweights.begin(), placesleft);
  }
  if (MCS.foundflag)
    LOG(3, "Found a best matching beneath L1 threshold of " << L1THRESHOLD);
  else {
    if (MCS.bestL2 < warn_amplitude)
      LOG(3, "Picking matching is " << MCS.bestmatching << " with best L2 error of "
          << MCS.bestL2);
    else if (MCS.bestL2 < L2THRESHOLD)
      ELOG(2, "Picking matching is " << MCS.bestmatching
          << " with rather large L2 error of " << MCS.bestL2);
    else
      ELOG(1, "findBestMatching() - matching "+toString(MCS.bestmatching)
          +" has L2 error of "+toString(MCS.bestL2)+" that is too large.");
  }

  // combine multiple points and create simple IndexList from IndexTupleList
  const SphericalPointDistribution::IndexList_t IndexList =
      joinPoints(points, MCS.newpoints, MCS.bestmatching);

  return IndexList;
}

SphericalPointDistribution::IndexList_t SphericalPointDistribution::joinPoints(
    Polygon_t &_newpolygon,
    const VectorArray_t &_newpoints,
    const IndexTupleList_t &_bestmatching
    )
{
  // combine all multiple points
  IndexList_t IndexList;
  IndexArray_t removalpoints;
  unsigned int UniqueIndex = _newpolygon.size(); // all indices up to size are used right now
  VectorArray_t newCenters;
  newCenters.reserve(_bestmatching.size());
  for (IndexTupleList_t::const_iterator tupleiter = _bestmatching.begin();
      tupleiter != _bestmatching.end(); ++tupleiter) {
    ASSERT (tupleiter->size() > 0,
        "findBestMatching() - encountered tuple in bestmatching with size 0.");
    if (tupleiter->size() == 1) {
      // add point and index
      IndexList.push_back(*tupleiter->begin());
    } else {
      // combine into weighted and normalized center
      Vector Center = calculateCenter(_newpoints, *tupleiter);
      Center.Normalize();
      _newpolygon.push_back(Center);
      LOG(5, "DEBUG: Combining " << tupleiter->size() << " points to weighted center "
          << Center << " with new index " << UniqueIndex);
      // mark for removal
      removalpoints.insert(removalpoints.end(), tupleiter->begin(), tupleiter->end());
      // add new index
      IndexList.push_back(UniqueIndex++);
    }
  }
  // IndexList is now our new bestmatching (that is bijective)
  LOG(4, "DEBUG: Our new bijective IndexList reads as " << IndexList);

  // modifying _newpolygon: remove all points in removalpoints, add those in newCenters
  Polygon_t allnewpoints = _newpolygon;
  {
    _newpolygon.clear();
    std::sort(removalpoints.begin(), removalpoints.end());
    size_t i = 0;
    IndexArray_t::const_iterator removeiter = removalpoints.begin();
    for (Polygon_t::iterator iter = allnewpoints.begin();
        iter != allnewpoints.end(); ++iter, ++i) {
      if ((removeiter != removalpoints.end()) && (i == *removeiter)) {
        // don't add, go to next remove index
        ++removeiter;
      } else {
        // otherwise add points
        _newpolygon.push_back(*iter);
      }
    }
  }
  LOG(4, "DEBUG: The polygon with recentered points removed is " << _newpolygon);

  // map IndexList to new shrinked _newpolygon
  typedef std::set<unsigned int> IndexSet_t;
  IndexSet_t SortedIndexList(IndexList.begin(), IndexList.end());
  IndexList.clear();
  {
    size_t offset = 0;
    IndexSet_t::const_iterator listiter = SortedIndexList.begin();
    IndexArray_t::const_iterator removeiter = removalpoints.begin();
    for (size_t i = 0; i < allnewpoints.size(); ++i) {
      if ((removeiter != removalpoints.end()) && (i == *removeiter)) {
        ++offset;
        ++removeiter;
      } else if ((listiter != SortedIndexList.end()) && (i == *listiter)) {
        IndexList.push_back(*listiter - offset);
        ++listiter;
      }
    }
  }
  LOG(4, "DEBUG: Our new bijective IndexList corrected for removed points reads as "
      << IndexList);

  return IndexList;
}

SphericalPointDistribution::Rotation_t SphericalPointDistribution::findPlaneAligningRotation(
    const PolygonWithIndices &_referencepositions,
    const PolygonWithIndices &_currentpositions
    )
{
#ifndef NDEBUG
  bool dontcheck = false;
#endif
  // initialize to no rotation
  Rotation_t Rotation;
  Rotation.first.Zero();
  Rotation.first[0] = 1.;
  Rotation.second = 0.;

  // calculate center of triangle/line/point consisting of first points of matching
  Vector oldCenter;
  Vector newCenter;
  calculateOldAndNewCenters(
      oldCenter, newCenter,
      _referencepositions, _currentpositions);

  ASSERT( !oldCenter.IsZero() && !newCenter.IsZero(),
      "findPlaneAligningRotation() - either old "+toString(oldCenter)
      +" or new center "+toString(newCenter)+" are zero.");
  LOG(4, "DEBUG: oldCenter is " << oldCenter << ", newCenter is " << newCenter);
  if (!oldCenter.IsEqualTo(newCenter)) {
    // calculate rotation axis and angle
    Rotation.first = oldCenter;
    if (oldCenter.IsParallelTo(newCenter, 1e-6))
      Rotation.first.GetOneNormalVector(oldCenter);
    else {
      Rotation.first.VectorProduct(newCenter);
      Rotation.first.Normalize();
    }
    // construct reference vector to determine direction of rotation
    const double sign = determineSignOfRotation(newCenter, oldCenter, Rotation.first);
    Rotation.second = sign * oldCenter.Angle(newCenter);
  } else {
    // no rotation required anymore
  }

#ifndef NDEBUG
  // check: rotation brings newCenter onto oldCenter position
  if (!dontcheck) {
    Line Axis(zeroVec, Rotation.first);
    Vector test = Axis.rotateVector(newCenter, Rotation.second);
    LOG(4, "CHECK: rotated newCenter is " << test
        << ", oldCenter is " << oldCenter);
    ASSERT( (test - oldCenter).NormSquared() < std::numeric_limits<double>::epsilon()*1e4,
        "matchSphericalPointDistributions() - rotation does not work as expected by "
        +toString((test - oldCenter).NormSquared())+".");
  }
#endif

  return Rotation;
}

SphericalPointDistribution::Rotation_t SphericalPointDistribution::findPointAligningRotation(
    const PolygonWithIndices &remainingold,
    const PolygonWithIndices &remainingnew)
{
  // initialize rotation to zero
  Rotation_t Rotation;
  Rotation.first.Zero();
  Rotation.first[0] = 1.;
  Rotation.second = 0.;

  // recalculate center
  Vector oldCenter;
  Vector newCenter;
  calculateOldAndNewCenters(
      oldCenter, newCenter,
      remainingold, remainingnew);

  Vector oldPosition = remainingnew.polygon[*remainingnew.indices.begin()];
  Vector newPosition = remainingold.polygon[0];
  LOG(6, "DEBUG: oldPosition is " << oldPosition << " (length: "
      << oldPosition.Norm() << ") and newPosition is " << newPosition << " length(: "
      << newPosition.Norm() << ")");

  if (!oldPosition.IsEqualTo(newPosition)) {
    // we rotate at oldCenter and around the radial direction, which is again given
    // by oldCenter.
    Rotation.first = oldCenter;
    Rotation.first.Normalize();  // note weighted sum of normalized weight is not normalized
    LOG(6, "DEBUG: Using oldCenter " << oldCenter << " as rotation center and "
        << Rotation.first << " as axis.");
    oldPosition -= oldCenter;
    newPosition -= oldCenter;
    oldPosition = (oldPosition - oldPosition.Projection(Rotation.first));
    newPosition = (newPosition - newPosition.Projection(Rotation.first));
    LOG(6, "DEBUG: Positions after projection are " << oldPosition << " and " << newPosition);

    // construct reference vector to determine direction of rotation
    const double sign = determineSignOfRotation(oldPosition, newPosition, Rotation.first);
    Rotation.second = sign * oldPosition.Angle(newPosition);
  } else {
    LOG(6, "DEBUG: oldPosition and newPosition are equivalent, hence no orientating rotation.");
  }

  return Rotation;
}

void SphericalPointDistribution::initSelf(const int _NumberOfPoints)
{
  switch (_NumberOfPoints)
  {
    case 0:
      points = get<0>();
      adjacency = getConnections<0>();
      break;
    case 1:
      points = get<1>();
      adjacency = getConnections<1>();
      break;
    case 2:
      points = get<2>();
      adjacency = getConnections<2>();
      break;
    case 3:
      points = get<3>();
      adjacency = getConnections<3>();
      break;
    case 4:
      points = get<4>();
      adjacency = getConnections<4>();
      break;
    case 5:
      points = get<5>();
      adjacency = getConnections<5>();
      break;
    case 6:
      points = get<6>();
      adjacency = getConnections<6>();
      break;
    case 7:
      points = get<7>();
      adjacency = getConnections<7>();
      break;
    case 8:
      points = get<8>();
      adjacency = getConnections<8>();
      break;
    case 9:
      points = get<9>();
      adjacency = getConnections<9>();
      break;
    case 10:
      points = get<10>();
      adjacency = getConnections<10>();
      break;
    case 11:
      points = get<11>();
      adjacency = getConnections<11>();
      break;
    case 12:
      points = get<12>();
      adjacency = getConnections<12>();
      break;
    case 14:
      points = get<14>();
      adjacency = getConnections<14>();
      break;
    default:
      ASSERT(0, "SphericalPointDistribution::initSelf() - cannot deal with the case "
          +toString(_NumberOfPoints)+".");
  }
  LOG(3, "DEBUG: Ideal polygon is " << points);
}

SphericalPointDistribution::Polygon_t
SphericalPointDistribution::getRemainingPoints(
    const WeightedPolygon_t &_polygon,
    const int _N)
{
  SphericalPointDistribution::Polygon_t remainingpoints;

  // initialze to given number of points
  initSelf(_N);
  LOG(2, "INFO: Matching old polygon " << _polygon
      << " with new polygon " << points);

  // check whether any points will remain vacant
  int RemainingPoints = _N;
  for (WeightedPolygon_t::const_iterator iter = _polygon.begin();
      iter != _polygon.end(); ++iter)
    RemainingPoints -= iter->second;
  if (RemainingPoints == 0)
    return remainingpoints;

  if (_N > 0) {
    IndexList_t bestmatching = findBestMatching(_polygon);
    LOG(2, "INFO: Best matching is " << bestmatching);

    const size_t NumberIds = std::min(bestmatching.size(), (size_t)3);
    // create old set
    PolygonWithIndices oldSet;
    oldSet.indices.resize(NumberIds, -1);
    std::generate(oldSet.indices.begin(), oldSet.indices.end(), UniqueNumber);
    for (WeightedPolygon_t::const_iterator iter = _polygon.begin();
        iter != _polygon.end(); ++iter)
      oldSet.polygon.push_back(iter->first);

    // _newpolygon has changed, so now convert to array with matched indices
    PolygonWithIndices newSet;
    SphericalPointDistribution::IndexList_t::const_iterator beginiter = bestmatching.begin();
    SphericalPointDistribution::IndexList_t::const_iterator enditer = bestmatching.begin();
    std::advance(enditer, NumberIds);
    newSet.indices.resize(NumberIds, -1);
    std::copy(beginiter, enditer, newSet.indices.begin());
    std::copy(points.begin(),points.end(), std::back_inserter(newSet.polygon));

    // determine rotation angles to align the two point distributions with
    // respect to bestmatching:
    // we use the center between the three first matching points
    /// the first rotation brings these two centers to coincide
    PolygonWithIndices rotatednewSet = newSet;
    {
      Rotation_t Rotation = findPlaneAligningRotation(oldSet, newSet);
      LOG(5, "DEBUG: Rotating coordinate system by " << Rotation.second
          << " around axis " << Rotation.first);
      Line Axis(zeroVec, Rotation.first);

      // apply rotation angle to bring newCenter to oldCenter
      for (VectorArray_t::iterator iter = rotatednewSet.polygon.begin();
          iter != rotatednewSet.polygon.end(); ++iter) {
        Vector &current = *iter;
        LOG(6, "DEBUG: Original point is " << current);
        current =  Axis.rotateVector(current, Rotation.second);
        LOG(6, "DEBUG: Rotated point is " << current);
      }

#ifndef NDEBUG
      // check: rotated "newCenter" should now equal oldCenter
      // we don't check in case of two points as these lie on a great circle
      // and the center cannot stably be recalculated. We may reactivate this
      // when we calculate centers only once
      if (oldSet.indices.size() > 2) {
        Vector oldCenter;
        Vector rotatednewCenter;
        calculateOldAndNewCenters(
            oldCenter, rotatednewCenter,
            oldSet, rotatednewSet);
        oldCenter.Normalize();
        rotatednewCenter.Normalize();
        // check whether centers are anti-parallel (factor -1)
        // then we have the "non-unique poles" situation: points lie on great circle
        // and both poles are valid solution
        if (fabs(oldCenter.ScalarProduct(rotatednewCenter) + 1.)
            < std::numeric_limits<double>::epsilon()*1e4)
          rotatednewCenter *= -1.;
        LOG(4, "CHECK: rotatednewCenter is " << rotatednewCenter
            << ", oldCenter is " << oldCenter);
        const double difference = (rotatednewCenter - oldCenter).NormSquared();
        ASSERT( difference < std::numeric_limits<double>::epsilon()*1e4,
            "matchSphericalPointDistributions() - rotation does not work as expected by "
            +toString(difference)+".");
      }
#endif
    }
    /// the second (orientation) rotation aligns the planes such that the
    /// points themselves coincide
    if (bestmatching.size() > 1) {
      Rotation_t Rotation = findPointAligningRotation(oldSet, rotatednewSet);

      // construct RotationAxis and two points on its plane, defining the angle
      Rotation.first.Normalize();
      const Line RotationAxis(zeroVec, Rotation.first);

      LOG(5, "DEBUG: Rotating around self is " << Rotation.second
          << " around axis " << RotationAxis);

#ifndef NDEBUG
      // check: first bestmatching in rotated_newpolygon and remainingnew
      // should now equal
      {
        const IndexList_t::const_iterator iter = bestmatching.begin();

        // check whether both old and newPosition are at same distance to oldCenter
        Vector oldCenter = calculateCenter(oldSet);
        const double distance = fabs(
              (oldSet.polygon[0] - oldCenter).NormSquared()
              - (rotatednewSet.polygon[*iter] - oldCenter).NormSquared()
            );
        LOG(4, "CHECK: Squared distance between oldPosition and newPosition "
            << " with respect to oldCenter " << oldCenter << " is " << distance);
//        ASSERT( distance < warn_amplitude,
//            "matchSphericalPointDistributions() - old and newPosition's squared distance to oldCenter differs by "
//            +toString(distance));

        Vector rotatednew = RotationAxis.rotateVector(
            rotatednewSet.polygon[*iter],
            Rotation.second);
        LOG(4, "CHECK: rotated first new bestmatching is " << rotatednew
            << " while old was " << oldSet.polygon[0]);
        const double difference = (rotatednew - oldSet.polygon[0]).NormSquared();
        ASSERT( difference < distance+1e-8,
            "matchSphericalPointDistributions() - orientation rotation ends up off by "
            +toString(difference)+", more than "
            +toString(distance+1e-8)+".");
      }
#endif

      for (VectorArray_t::iterator iter = rotatednewSet.polygon.begin();
          iter != rotatednewSet.polygon.end(); ++iter) {
        Vector &current = *iter;
        LOG(6, "DEBUG: Original point is " << current);
        current = RotationAxis.rotateVector(current, Rotation.second);
        LOG(6, "DEBUG: Rotated point is " << current);
      }
    }

    // remove all points in matching and return remaining ones
    SphericalPointDistribution::Polygon_t remainingpoints =
        removeMatchingPoints(rotatednewSet);
    LOG(2, "INFO: Remaining points are " << remainingpoints);
    return remainingpoints;
  } else
    return points;
}

SphericalPointDistribution::Polygon_t
SphericalPointDistribution::getSimplePolygon(const int _NumberOfPoints) const
{
  Polygon_t returnpolygon;

	switch (_NumberOfPoints)
	{
	  case 0:
	    returnpolygon = get<0>();
	    break;
	  case 1:
	    returnpolygon = get<1>();
	    break;
	  case 2:
	    returnpolygon = get<2>();
	    break;
	  case 3:
	    returnpolygon = get<3>();
	    break;
	  case 4:
	    returnpolygon = get<4>();
	    break;
	  case 5:
	    returnpolygon = get<5>();
	    break;
	  case 6:
	    returnpolygon = get<6>();
	    break;
	  case 7:
	    returnpolygon = get<7>();
	    break;
	  case 8:
	    returnpolygon = get<8>();
	    break;
	  case 9:
	    returnpolygon = get<9>();
	    break;
	  case 10:
	    returnpolygon = get<10>();
	    break;
	  case 11:
	    returnpolygon = get<11>();
	    break;
	  case 12:
	    returnpolygon = get<12>();
	    break;
	  case 14:
	    returnpolygon = get<14>();
	    break;
	  default:
	    ASSERT(0, "SphericalPointDistribution::initSelf() - cannot deal with the case "
	        +toString(_NumberOfPoints)+".");
	}

	return returnpolygon;
}