/*
 * 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/toString.hpp"

#include <algorithm>
#include <cmath>
#include <limits>
#include <list>
#include <vector>
#include <map>

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

typedef std::list<unsigned int> IndexList_t;
typedef std::vector<unsigned int> IndexArray_t;
typedef std::vector<Vector> VectorArray_t;
typedef std::vector<double> DistanceArray_t;

DistanceArray_t calculatePairwiseDistances(
    const std::vector<Vector> &_points,
    const IndexList_t &_indices
    )
{
  DistanceArray_t result;
  for (IndexList_t::const_iterator firstiter = _indices.begin();
      firstiter != _indices.end(); ++firstiter) {
    for (IndexList_t::const_iterator seconditer = firstiter;
        seconditer != _indices.end(); ++seconditer) {
      if (firstiter == seconditer)
        continue;
      const double distance = (_points[*firstiter] - _points[*seconditer]).NormSquared();
      result.push_back(distance);
    }
  }
  return result;
}

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

/** 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> calculateErrorOfMatching(
    const std::vector<Vector> &_old,
    const std::vector<Vector> &_new,
    const IndexList_t &_Matching)
{
  std::pair<double, double> errors( std::make_pair( 0., 0. ) );

  if (_Matching.size() > 1) {
    // convert matching into two vectors to calculate distance among another

    // calculate all pair-wise distances
    IndexList_t keys(_Matching.size());
    std::generate (keys.begin(), keys.end(), UniqueNumber);
    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 = *firstiter - *seconditer;
      // L1 error
      if (errors.first < gap)
        errors.first = gap;
      // L2 error
      errors.second += gap*gap;
    }
  }

  return errors;
}

SphericalPointDistribution::Polygon_t removeMatchingPoints(
    const SphericalPointDistribution::Polygon_t &_points,
    const IndexList_t &_matchingindices
    )
{
  SphericalPointDistribution::Polygon_t remainingpoints;
  IndexArray_t indices(_matchingindices.begin(), _matchingindices.end());
  std::sort(indices.begin(), indices.end());
  IndexArray_t::const_iterator valueiter = indices.begin();
  SphericalPointDistribution::Polygon_t::const_iterator pointiter =
      _points.begin();
  for (unsigned int i=0; i< _points.size(); ++i, ++pointiter) {
    // skip all those in values
    if (*valueiter == i)
      ++valueiter;
    else
      remainingpoints.push_back(*pointiter);
  }

  return remainingpoints;
}

/** Rotates a given polygon around x, y, and z axis by the given angles.
 *
 * Essentially, we concentrate on the three points of the polygon to rotate
 * to the correct position. First, we rotate its center via \a angles,
 * then we rotate the "triangle" around itself/\a _RotationAxis by
 * \a _RotationAngle.
 *
 * \param _polygon polygon whose points to rotate
 * \param _angles vector with rotation angles for x,y,z axis
 * \param _RotationAxis
 * \param _RotationAngle
 */
SphericalPointDistribution::Polygon_t rotatePolygon(
    const SphericalPointDistribution::Polygon_t &_polygon,
    const std::vector<double> &_angles,
    const Line &_RotationAxis,
    const double _RotationAngle)
{
  SphericalPointDistribution::Polygon_t rotated_polygon = _polygon;
  RealSpaceMatrix rotation;
  ASSERT( _angles.size() == 3,
      "rotatePolygon() - not exactly "+toString(3)+" angles given.");
  rotation.setRotation(_angles[0] * M_PI/180., _angles[1] * M_PI/180., _angles[2] * M_PI/180.);

  // apply rotation angles
  for (SphericalPointDistribution::Polygon_t::iterator iter = rotated_polygon.begin();
      iter != rotated_polygon.end(); ++iter) {
    *iter = rotation * (*iter);
    _RotationAxis.rotateVector(*iter, _RotationAngle);
  }

  return rotated_polygon;
}

struct MatchingControlStructure {
  bool foundflag;
  double bestL2;
  IndexList_t bestmatching;
  VectorArray_t oldpoints;
  VectorArray_t newpoints;
};

/** 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 _matchingsize
 */
void recurseMatchings(
    MatchingControlStructure &_MCS,
    IndexList_t &_matching,
    IndexList_t _indices,
    unsigned int _matchingsize)
{
  //!> threshold for L1 error below which matching is immediately acceptable
  const double L1THRESHOLD = 1e-2;
  if (!_MCS.foundflag) {
    if (_matching.size() < _matchingsize) {
      // go through all indices
      for (IndexList_t::iterator iter = _indices.begin();
          iter != _indices.end();) {
        // add index to matching
        _matching.push_back(*iter);
        // 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, _matchingsize-1);
        // re-add chosen index and reset index to new position
        _indices.insert(backupiter, _matching.back());
        iter = backupiter;
        // remove index from _matching to make space for the next one
        _matching.pop_back();
      }
      // gone through all indices then exit recursion
      _MCS.foundflag = true;
    } else {
      // calculate errors
      std::pair<double, double> errors = calculateErrorOfMatching(
          _MCS.oldpoints, _MCS.newpoints, _matching);
      if (errors.first < L1THRESHOLD) {
        _MCS.bestmatching = _matching;
        _MCS.foundflag = true;
      }
      if (_MCS.bestL2 > errors.second) {
        _MCS.bestmatching = _matching;
        _MCS.bestL2 = errors.second;
      }
    }
  }
}

SphericalPointDistribution::Polygon_t
SphericalPointDistribution::matchSphericalPointDistributions(
    const SphericalPointDistribution::Polygon_t &_polygon,
    const SphericalPointDistribution::Polygon_t &_newpolygon
    )
{
  SphericalPointDistribution::Polygon_t remainingpoints;
  VectorArray_t remainingold(_polygon.begin(), _polygon.end());
  VectorArray_t remainingnew(_newpolygon.begin(), _newpolygon.end());

  if (_polygon.size() > 0) {
    MatchingControlStructure MCS;
    MCS.foundflag = false;
    MCS.bestL2 = std::numeric_limits<double>::max();
    MCS.oldpoints.insert(MCS.oldpoints.begin(), _polygon.begin(),_polygon.end() );
    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);
      IndexList_t matching;

      // walk through all matchings
      const unsigned int matchingsize = _polygon.size();
      ASSERT( matchingsize <= indices.size(),
          "SphericalPointDistribution::matchSphericalPointDistributions() - not enough new points to choose for matching to old ones.");
      recurseMatchings(MCS, matching, indices, matchingsize);
    }

    // determine rotation angles to align the two point distributions with
    // respect to bestmatching
    std::vector<double> angles(3);
    Vector newCenter;
    {
      // calculate center of triangle/line/point consisting of first points of matching
      Vector oldCenter;
      IndexList_t::const_iterator iter = MCS.bestmatching.begin();
      unsigned int i = 0;
      for (; (i<3) && (i<MCS.bestmatching.size()); ++i, ++iter) {
        oldCenter += remainingold[i];
        newCenter += remainingnew[*iter];
      }
      oldCenter *= 1./(double)i;
      newCenter *= 1./(double)i;

      Vector direction(0.,0.,0.);
      for(size_t i=0;i<NDIM;++i) {
        // create new rotation axis
        direction[i] = 1.;
        const Line axis (zeroVec, direction);
        // calculate rotation angle for this axis
        const double alpha = direction.Angle(oldCenter) - direction.Angle(newCenter);
        // perform rotation
        axis.rotateVector(newCenter, alpha);
        // store angle
        angles[i] = alpha;
        // reset direction component for next iteration
        direction[i] = 0.;
      }
    }
    const Line RotationAxis(zeroVec, newCenter);
    const double RotationAngle =
        newCenter.Angle(remainingold[0])
        - newCenter.Angle(remainingnew[*MCS.bestmatching.begin()]);

    // rotate _newpolygon
    SphericalPointDistribution::Polygon_t rotated_newpolygon =
        rotatePolygon(_newpolygon, angles, RotationAxis, RotationAngle);

    // remove all points in matching and return remaining ones
    return removeMatchingPoints(rotated_newpolygon, MCS.bestmatching);
  } else
    return _newpolygon;
}