/*
 * 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/math/quaternion.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"

// static entities
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 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;
}

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::VectorArray_t &_referencepositions,
    const SphericalPointDistribution::VectorArray_t &_currentpositions,
    const SphericalPointDistribution::IndexList_t &_bestmatching)
{
  const size_t NumberIds = std::min(_bestmatching.size(), (size_t)3);
  SphericalPointDistribution::IndexList_t continuousIds(NumberIds, -1);
  std::generate(continuousIds.begin(), continuousIds.end(), UniqueNumber);
  _oldCenter = calculateCenter(_referencepositions, continuousIds);
  // C++11 defines a copy_n function ...
  SphericalPointDistribution::IndexList_t::const_iterator enditer = _bestmatching.begin();
  std::advance(enditer, NumberIds);
  SphericalPointDistribution::IndexList_t firstbestmatchingIds(NumberIds, -1);
  std::copy(_bestmatching.begin(), enditer, firstbestmatchingIds.begin());
  _newCenter = calculateCenter( _currentpositions, firstbestmatchingIds);
}
/** 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 points (fewer or equal to \a _new)
 * \param _new second set of points
 * \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 VectorArray_t &_points,
    const IndexList_t &_matchingindices
    )
{
  SphericalPointDistribution::Polygon_t remainingpoints;
  IndexArray_t indices(_matchingindices.begin(), _matchingindices.end());
  std::sort(indices.begin(), indices.end());
  LOG(4, "DEBUG: sorted matching is " << indices);
  IndexArray_t remainingindices(_points.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[*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 same position
        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);
        // 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;
      }
    }
  }
}

/** 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,
    Polygon_t &_newpolygon
    )
{
  MatchingControlStructure MCS;
  MCS.foundflag = false;
  MCS.bestL2 = std::numeric_limits<double>::max();
  // transform lists into arrays
  for (WeightedPolygon_t::const_iterator iter = _polygon.begin();
      iter != _polygon.end(); ++iter) {
    MCS.oldpoints.push_back(iter->first);
    MCS.weights.push_back(iter->second);
  }
  MCS.newpoints.insert(MCS.newpoints.begin(), _newpolygon.begin(),_newpolygon.end() );

  // search for bestmatching combinatorially
  {
    // translate polygon into vector to enable index addressing
    IndexList_t indices(_newpolygon.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
      ASSERT(0, "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(_newpolygon, 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 VectorArray_t &_referencepositions,
    const VectorArray_t &_currentpositions,
    const IndexList_t &_bestmatching
    )
{
  bool dontcheck = false;
  // 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, _bestmatching);

  if ((!oldCenter.IsZero()) && (!newCenter.IsZero())) {
    LOG(4, "DEBUG: oldCenter is " << oldCenter << ", newCenter is " << newCenter);
    oldCenter.Normalize();
    newCenter.Normalize();
    if (!oldCenter.IsEqualTo(newCenter)) {
      // calculate rotation axis and angle
      Rotation.first = oldCenter;
      Rotation.first.VectorProduct(newCenter);
      Rotation.second = oldCenter.Angle(newCenter); // /(M_PI/2.);
    } else {
      // no rotation required anymore
    }
  } else {
    LOG(4, "DEBUG: oldCenter is " << oldCenter << ", newCenter is " << newCenter);
    if ((oldCenter.IsZero()) && (newCenter.IsZero())) {
      // either oldCenter or newCenter (or both) is directly at origin
      if (_bestmatching.size() == 2) {
        // line case
        Vector oldPosition = _currentpositions[*_bestmatching.begin()];
        Vector newPosition = _referencepositions[0];
        // check whether we need to rotate at all
        if (!oldPosition.IsEqualTo(newPosition)) {
          Rotation.first = oldPosition;
          Rotation.first.VectorProduct(newPosition);
          // orientation will fix the sign here eventually
          Rotation.second = oldPosition.Angle(newPosition);
        } else {
          // no rotation required anymore
        }
      } else {
        // triangle case
        // both triangles/planes have same center, hence get axis by
        // VectorProduct of Normals
        Plane newplane(_referencepositions[0], _referencepositions[1], _referencepositions[2]);
        VectorArray_t vectors;
        for (IndexList_t::const_iterator iter = _bestmatching.begin();
            iter != _bestmatching.end(); ++iter)
          vectors.push_back(_currentpositions[*iter]);
        Plane oldplane(vectors[0], vectors[1], vectors[2]);
        Vector oldPosition = oldplane.getNormal();
        Vector newPosition = newplane.getNormal();
        // check whether we need to rotate at all
        if (!oldPosition.IsEqualTo(newPosition)) {
          Rotation.first = oldPosition;
          Rotation.first.VectorProduct(newPosition);
          Rotation.first.Normalize();

          // construct reference vector to determine direction of rotation
          const double sign = determineSignOfRotation(oldPosition, newPosition, Rotation.first);
          Rotation.second = sign * oldPosition.Angle(newPosition);
          LOG(5, "DEBUG: Rotating plane normals by " << Rotation.second
              << " around axis " << Rotation.first);
        } else {
          // else do nothing
        }
      }
    } else {
      // TODO: we can't do anything here, but this case needs to be dealt with when
      // we have no ideal geometries anymore
      if ((oldCenter-newCenter).Norm() > warn_amplitude)
        ELOG(2, "oldCenter is " << oldCenter << ", yet newCenter is " << newCenter);
      // else they are considered close enough
      dontcheck = true;
    }
  }

#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 VectorArray_t &remainingold,
    const VectorArray_t &remainingnew,
    const IndexList_t &_bestmatching)
{
  // 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, _bestmatching);

  Vector oldPosition = remainingnew[*_bestmatching.begin()];
  Vector newPosition = remainingold[0];
  LOG(6, "DEBUG: oldPosition is " << oldPosition << " and newPosition is " << newPosition);
  if (!oldPosition.IsEqualTo(newPosition)) {
    if ((!oldCenter.IsZero()) && (!newCenter.IsZero())) {
      oldCenter.Normalize();  // note weighted sum of normalized weight is not normalized
      Rotation.first = oldCenter;
      LOG(6, "DEBUG: Picking normalized oldCenter as Rotation.first " << oldCenter);
      oldPosition.ProjectOntoPlane(Rotation.first);
      newPosition.ProjectOntoPlane(Rotation.first);
      LOG(6, "DEBUG: Positions after projection are " << oldPosition << " and " << newPosition);
    } else {
      if (_bestmatching.size() == 2) {
        // line situation
        try {
          Plane oldplane(oldPosition, oldCenter, newPosition);
          Rotation.first = oldplane.getNormal();
          LOG(6, "DEBUG: Plane is " << oldplane << " and normal is " << Rotation.first);
        } catch (LinearDependenceException &e) {
          LOG(6, "DEBUG: Vectors defining plane are linearly dependent.");
          // oldPosition and newPosition are on a line, just flip when not equal
          if (!oldPosition.IsEqualTo(newPosition)) {
            Rotation.first.Zero();
            Rotation.first.GetOneNormalVector(oldPosition);
            LOG(6, "DEBUG: For flipping we use Rotation.first " << Rotation.first);
            assert( Rotation.first.ScalarProduct(oldPosition) < std::numeric_limits<double>::epsilon()*1e4);
  //              Rotation.second = M_PI;
          } else {
            LOG(6, "DEBUG: oldPosition and newPosition are equivalent.");
          }
        }
      } else {
        // triangle situation
        Plane oldplane(remainingold[0], remainingold[1], remainingold[2]);
        Rotation.first = oldplane.getNormal();
        LOG(6, "DEBUG: oldPlane is " << oldplane << " and normal is " << Rotation.first);
        oldPosition.ProjectOntoPlane(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;
}


SphericalPointDistribution::Polygon_t
SphericalPointDistribution::matchSphericalPointDistributions(
    const SphericalPointDistribution::WeightedPolygon_t &_polygon,
    SphericalPointDistribution::Polygon_t &_newpolygon
    )
{
  SphericalPointDistribution::Polygon_t remainingpoints;
  VectorArray_t remainingold;
  for (WeightedPolygon_t::const_iterator iter = _polygon.begin();
      iter != _polygon.end(); ++iter)
    remainingold.push_back(iter->first);
  LOG(2, "INFO: Matching old polygon " << _polygon
      << " with new polygon " << _newpolygon);

  if (_polygon.size() == _newpolygon.size()) {
    // same number of points desired as are present? Do nothing
    LOG(2, "INFO: There are no vacant points to return.");
    return remainingpoints;
  }

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

    // _newpolygon has changed, so now convert to array
    VectorArray_t remainingnew(_newpolygon.begin(), _newpolygon.end());

    // 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
    VectorArray_t rotated_newpolygon = remainingnew;
    {
      Rotation_t Rotation = findPlaneAligningRotation(
          remainingold,
          remainingnew,
          bestmatching);
      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 = rotated_newpolygon.begin();
          iter != rotated_newpolygon.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
      {
        Vector oldCenter;
        Vector rotatednewCenter;
        calculateOldAndNewCenters(
            oldCenter, rotatednewCenter,
            remainingold, rotated_newpolygon, bestmatching);
        // NOTE: Center must not necessarily lie on the sphere with norm 1, hence, we
        // have to normalize it just as before, as oldCenter and newCenter lengths may differ.
        if ((!oldCenter.IsZero()) && (!rotatednewCenter.IsZero())) {
          oldCenter.Normalize();
          rotatednewCenter.Normalize();
          LOG(4, "CHECK: rotatednewCenter is " << rotatednewCenter
              << ", oldCenter is " << oldCenter);
          ASSERT( (rotatednewCenter - oldCenter).NormSquared() < std::numeric_limits<double>::epsilon()*1e4,
              "matchSphericalPointDistributions() - rotation does not work as expected by "
              +toString((rotatednewCenter - oldCenter).NormSquared())+".");
        }
      }
#endif
    }
    /// the second (orientation) rotation aligns the planes such that the
    /// points themselves coincide
    if (bestmatching.size() > 1) {
      Rotation_t Rotation = findPointAligningRotation(
          remainingold,
          rotated_newpolygon,
          bestmatching);

      // 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();
        Vector rotatednew = RotationAxis.rotateVector(
            rotated_newpolygon[*iter],
            Rotation.second);
        LOG(4, "CHECK: rotated first new bestmatching is " << rotatednew
            << " while old was " << remainingold[0]);
        ASSERT( (rotatednew - remainingold[0]).Norm() < warn_amplitude,
            "matchSphericalPointDistributions() - orientation rotation ends up off by more than "
            +toString(warn_amplitude)+".");
      }
#endif

      for (VectorArray_t::iterator iter = rotated_newpolygon.begin();
          iter != rotated_newpolygon.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(rotated_newpolygon, bestmatching);
    LOG(2, "INFO: Remaining points are " << remainingpoints);
    return remainingpoints;
  } else
    return _newpolygon;
}