/*
 * 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;

// static instances
const double SphericalPointDistribution::SQRT_3(sqrt(3.0));

DistanceArray_t calculatePairwiseDistances(
    const std::vector<Vector> &_returnpolygon,
    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 = (_returnpolygon[*firstiter] - _returnpolygon[*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 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> 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 SphericalPointDistribution::Polygon_t &_returnpolygon,
    const IndexList_t &_matchingindices
    )
{
  SphericalPointDistribution::Polygon_t remainingreturnpolygon;
  IndexArray_t indices(_matchingindices.begin(), _matchingindices.end());
  std::sort(indices.begin(), indices.end());
  LOG(4, "DEBUG: sorted matching is " << indices);
  IndexArray_t::const_iterator valueiter = indices.begin();
  SphericalPointDistribution::Polygon_t::const_iterator pointiter =
      _returnpolygon.begin();
  for (unsigned int i=0; i< _returnpolygon.size(); ++i, ++pointiter) {
    // skip all those in values
    if (*valueiter == i)
      ++valueiter;
    else
      remainingreturnpolygon.push_back(*pointiter);
  }
  LOG(4, "DEBUG: remaining indices are " << remainingreturnpolygon);

  return remainingreturnpolygon;
}

struct MatchingControlStructure {
  bool foundflag;
  double bestL2;
  IndexList_t bestmatching;
  VectorArray_t oldreturnpolygon;
  VectorArray_t newreturnpolygon;
};

/** 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.oldreturnpolygon, _MCS.newreturnpolygon, _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 remainingreturnpolygon;
  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.oldreturnpolygon.insert(MCS.oldreturnpolygon.begin(), _polygon.begin(),_polygon.end() );
    MCS.newreturnpolygon.insert(MCS.newreturnpolygon.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 returnpolygon 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
    SphericalPointDistribution::Polygon_t rotated_newpolygon;
    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 = newCenter;
        RotationAxis.VectorProduct(oldCenter);
        RotationAxis.Normalize();
        const double RotationAngle = oldCenter.Angle(newCenter)/(M_PI/2.);
//            RotationAxis.Angle(oldCenter) - RotationAxis.Angle(newCenter);
        boost::math::quaternion<double> q
            (RotationAngle, RotationAxis[0], RotationAxis[1], RotationAxis[2]);
        LOG(5, "DEBUG: RotationAxis is " << RotationAxis
            << ", RotationAngle is " << RotationAngle);
        LOG(5, "DEBUG: Quaternion describing rotation is " << q);
#ifndef NDEBUG
        {
        // check that rotation works in DEBUG mode
        boost::math::quaternion<double> p(0., newCenter[0], newCenter[1], newCenter[2]);
        boost::math::quaternion<double> q_inverse =
            boost::math::conj(q)/(boost::math::norm(q));
        p = q * p * q_inverse;
        boost::math::quaternion<double> identity(1,0,0,0);
        ASSERT( boost::math::norm(q*q_inverse - identity) < std::numeric_limits<double>::epsilon()*1e4,
            "matchSphericalPointDistributions() - quaternion does not rotate newCenter "
            +toString(q*q_inverse)+" into oldCenter "+toString(identity)+".");
        boost::math::quaternion<double> comparison(0., -oldCenter[0], oldCenter[1], oldCenter[2]);
        ASSERT( boost::math::norm(p - comparison) < std::numeric_limits<double>::epsilon()*1e4,
            "matchSphericalPointDistributions() - quaternion does not rotate newCenter "
            +toString(p)+" into oldCenter "+toString(comparison)+".");
        }
#endif

        // rotate spherical distribution
        rotated_newpolygon = rotatePolygon(_newpolygon, q);
        LOG(5, "DEBUG: Rotated new polygon is " << rotated_newpolygon);
      } else {
        rotated_newpolygon = _newpolygon;
      }
    }
    // rotate triangle/line/point around itself to match orientation
    const Line RotationAxis(zeroVec, oldCenter);
    const double RotationAngle =
        oldCenter.Angle(remainingold[0])
        - oldCenter.Angle(remainingnew[*MCS.bestmatching.begin()]);
    LOG(5, "DEBUG: Rotate around self is " << RotationAngle
        << " around axis " << RotationAxis);

    for (SphericalPointDistribution::Polygon_t::iterator iter = rotated_newpolygon.begin();
        iter != rotated_newpolygon.end(); ++iter) {
      RotationAxis.rotateVector(*iter, RotationAngle);
    }

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