/*
 * 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 <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) {
    LOG(3, "INFO: Matching is " << _Matching);

    // 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;
    }
  } else
    ELOG(3, "calculateErrorOfMatching() - Given matching's size is less than 2.");
  LOG(3, "INFO: Resulting errors for matching (L1, L2): "
      << errors.first << "," << errors.second << ".");

  return errors;
}

SphericalPointDistribution::Polygon_t 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;
}

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)
{
  LOG(4, "DEBUG: Recursing with current matching " << _matching
      << ", remaining indices " << _indices
      << ", and sought size " << _matching.size()+_matchingsize);
  //!> threshold for L1 error below which matching is immediately acceptable
  const double L1THRESHOLD = 1e-2;
  if (!_MCS.foundflag) {
    LOG(4, "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);) {
        // add index to matching
        _matching.push_back(*iter);
        LOG(5, "DEBUG: Adding " << *iter << " to matching.");
        // 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
      if (_matching.empty())
        _MCS.foundflag = true;
    } else {
      LOG(3, "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;
      }
    }
  }
}

/** Rotates a given polygon around x, y, and z axis by the given angles.
 *
 * \param _polygon polygon whose points to rotate
 * \param _q quaternion specifying the rotation of the coordinate system
 */
SphericalPointDistribution::Polygon_t rotatePolygon(
    const SphericalPointDistribution::Polygon_t &_polygon,
    const boost::math::quaternion<double> &_q)
{
  SphericalPointDistribution::Polygon_t rotated_polygon = _polygon;
  boost::math::quaternion<double> q_inverse =
      boost::math::conj(_q)/(boost::math::norm(_q));

  // apply rotation angles
  for (SphericalPointDistribution::Polygon_t::iterator iter = rotated_polygon.begin();
      iter != rotated_polygon.end(); ++iter) {
    Vector &current = *iter;
    boost::math::quaternion<double> p(0, current[0], current[1], current[2]);
    p = _q * p * q_inverse;
    LOG(5, "DEBUG: Rotated point is " << p);
    // i have no idea why but first component comes up with wrong sign
    current[0] = -p.R_component_2();
    current[1] = p.R_component_3();
    current[2] = p.R_component_4();
  }

  return rotated_polygon;
}


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());
  LOG(2, "INFO: Matching old polygon " << _polygon
      << " with new polygon " << _newpolygon);

  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);
    }
    LOG(2, "INFO: Best matching is " << MCS.bestmatching);

    // determine rotation angles to align the two point distributions with
    // respect to bestmatching
    VectorArray_t rotated_newpolygon = remainingnew;
    Vector oldCenter;
    {
      // calculate center of triangle/line/point consisting of first points of matching
      Vector newCenter;
      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;
      LOG(4, "DEBUG: oldCenter is " << oldCenter << ", newCenter is " << newCenter);

      if ((oldCenter - newCenter).NormSquared() > std::numeric_limits<double>::epsilon()*1e4) {
        // setup quaternion
        Vector RotationAxis = oldCenter;
        RotationAxis.VectorProduct(newCenter);
        Line Axis(zeroVec, RotationAxis);
        RotationAxis.Normalize();
        const double RotationAngle = oldCenter.Angle(newCenter); // /(M_PI/2.);
        LOG(5, "DEBUG: Rotate coordinate system by " << RotationAngle
            << " around axis " << RotationAxis);

        // apply rotation angles
        for (VectorArray_t::iterator iter = rotated_newpolygon.begin();
            iter != rotated_newpolygon.end(); ++iter) {
          Vector &current = *iter;
          LOG(5, "DEBUG: Original point is " << current);
          current =  Axis.rotateVector(current, RotationAngle);
          LOG(5, "DEBUG: Rotated point is " << current);
        }
      }
    }
    // rotate triangle/line/point around itself to match orientation
    if (MCS.bestmatching.size() > 1) {
      if (oldCenter.NormSquared() > std::numeric_limits<double>::epsilon()*1e4) {
        // construct RotationAxis and two points on its plane, defining the angle
        const Line RotationAxis(zeroVec, oldCenter);
        Vector oldPosition(rotated_newpolygon[*MCS.bestmatching.begin()]);
        oldPosition.ProjectOntoPlane(RotationAxis.getDirection());
        Vector newPosition(remainingold[*MCS.bestmatching.begin()]);
        newPosition.ProjectOntoPlane(RotationAxis.getDirection());

        // construct reference vector to determine direction of rotation
        Vector dreiBein(oldPosition);
        dreiBein.VectorProduct(oldCenter);
        dreiBein.Normalize();
        const double sign =
            (dreiBein.ScalarProduct(newPosition) < 0.) ? -1. : +1.;
        LOG(6, "DEBUG: oldCenter on plane is " << oldPosition
            << ", newCenter in plane is " << newPosition
            << ", and dreiBein is " << dreiBein);
        const double RotationAngle = sign * oldPosition.Angle(newPosition);
        LOG(5, "DEBUG: Rotating around self is " << RotationAngle
            << " around axis " << RotationAxis);

#ifndef NDEBUG
        // check: first bestmatching in rotated_newpolygon and remainingnew
        // should now equal
        {
          const IndexList_t::const_iterator iter = MCS.bestmatching.begin();
          Vector rotatednew = RotationAxis.rotateVector(
              rotated_newpolygon[*iter],
              RotationAngle);
          LOG(4, "CHECK: rotated first new bestmatching is " << rotatednew
              << " while old was " << remainingold[*iter]);
          ASSERT( (rotatednew - remainingold[*iter]).Norm()
              < std::numeric_limits<double>::epsilon()*1e4,
              "matchSphericalPointDistributions() - orientation rotation does not work as expected.");
        }
#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, RotationAngle);
          LOG(6, "DEBUG: Rotated point is " << current);
        }
      }
    }

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