/*
 * Project: MoleCuilder
 * Description: creates and alters molecular systems
 * Copyright (C)  2010 University of Bonn. All rights reserved.
 * Please see the LICENSE file or "Copyright notice" in builder.cpp for details.
 */

/*
 * Subspace.cpp
 *
 *  Created on: Nov 22, 2010
 *      Author: heber
 */

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

#include "CodePatterns/MemDebug.hpp"

#include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp>

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

#include "Eigenspace.hpp"
#include "Subspace.hpp"


/** Constructor for class Subspace.
 *
 * @param _s indices that make up this subspace
 * @param _FullSpace reference to the full eigenspace
 */
Subspace::Subspace(indexset & _s, Eigenspace &_FullSpace) :
  Eigenspace(_s),
  ProjectToSubspace(_FullSpace.getIndices().size(), _s.size()),
  ProjectFromSubspace(_s.size(), _FullSpace.getIndices().size()),
  FullSpace(_FullSpace),
  ZeroVector(_FullSpace.getIndices().size())
{
  // TODO: away with both of this when calculateEigenSubspace() works
  // create projection matrices
  createProjectionMatrices();

  // create eigenspace matrices
  EigenspaceMatrix = projectFullspaceMatrixToSubspace(FullSpace.getEigenspaceMatrix());
}


/** Destructor for class Subspace.
 *
 */
Subspace::~Subspace()
{}


/** Diagonalizes the subspace matrix.
 *
 */
void Subspace::calculateEigenSubspace()
{
  // project down
  createProjectionMatrices();
  // remove subsubspace directions
  //correctProjectionMatricesFromSubIndices();
  // solve
  EigenspaceMatrix = projectFullspaceMatrixToSubspace(FullSpace.getEigenspaceMatrix());
  EigenvectorMatrix = EigenspaceMatrix;
  VectorContent *EigenvalueVector = new VectorContent(EigenvectorMatrix.transformToEigenbasis());
  Eigenvalues.clear();
  for(size_t i = 0; i< EigenvalueVector->getDimension(); ++i) {
    Eigenvalues.push_back( EigenvalueVector->at(i) );
  }
  delete EigenvalueVector;

  // create mapping
  createLocalMapping();

  // swap the eigenvectors/-values to their correct sequence
  sortEigenvectors();

  // scale eigenvectors by their eigenvalues for the subsequent correction
  scaleEigenvectorsbyEigenvalue();

  // let only remain corrections to lower orders on this order
  correctEigenvectorsFromSubIndices();

  if (!EigenvectorMatrix.IsNull()) {
    // get correct eigenvalues
    getNormofEigenvectorAsEigenvalue();

    DoLog(2) && (Log() << Verbose(2) << "Eigenvector matrix is " << EigenvectorMatrix << std::endl);
    DoLog(2) && (Log() << Verbose(2) << "Eigenvalues are " << Eigenvalues << std::endl);

  }
}


/** Projects a given full matrix down into the subspace of this class.
 *
 * @param bigmatrix full matrix to project into subspace
 */
void Subspace::getSubspacematrixFromBigmatrix(const MatrixContent & bigmatrix)
{
  // check whether subsystem is big enough for both index sets
  ASSERT(Indices.size() <= bigmatrix.getRows(),
      "embedEigenspaceMatrix() - bigmatrix has less rows "+toString(bigmatrix.getRows())
      +" than needed by index set "
      +toString(Indices.size())+"!");
  ASSERT(Indices.size() <= bigmatrix.getColumns(),
      "embedEigenspaceMatrix() - bigmatrix has less columns "+toString(bigmatrix.getColumns())
      +" than needed by index set "
      +toString(Indices.size())+"!");

  // construct small matrix
  MatrixContent *subsystem =  new MatrixContent(Indices.size(), Indices.size());
  size_t localrow = 0; // local row indices for the subsystem
  size_t localcolumn = 0;
  BOOST_FOREACH( size_t rowindex, Indices) {
    localcolumn = 0;
    BOOST_FOREACH( size_t columnindex, Indices) {
      ASSERT((rowindex < bigmatrix.getRows()) && (columnindex < bigmatrix.getColumns()),
          "current index pair ("
          +toString(rowindex)+","+toString(columnindex)
          +") is out of bounds of bigmatrix ("
          +toString(bigmatrix.getRows())+","+toString(bigmatrix.getColumns())
          +")");
      subsystem->at(localrow,localcolumn) = (*Eigenvectors[rowindex]) * (bigmatrix * (*Eigenvectors[columnindex]));
      localcolumn++;
    }
    localrow++;
  }
}


/** Sort the eigenvectors in the order of Subspace::Indices.
 *
 */
void Subspace::sortEigenvectors()
{
  DoLog(1) && (Log() << Verbose(1) << "Sorting Eigenvectors ..." << std::endl);
  MatrixContent tempMatrix = EigenvectorMatrix;
  eigenvalueset tempValues = Eigenvalues;
  size_t localindex = 0;
  BOOST_FOREACH( size_t iter, Indices) {
    Log() << Verbose(1) << GlobalToLocal[iter] << "th eigenvector is associated to "
        << iter << " and goes to column " << localindex << "." << std::endl;
    boost::shared_ptr<VectorContent> columnvector(tempMatrix.getColumnVector(GlobalToLocal[iter]));
    *Eigenvectors[localindex] = *columnvector;
    Eigenvalues[localindex] = tempValues[GlobalToLocal[iter]];
    ++localindex;
  }
}


/** We remove the projections from lower subspaces from our Eigenspacematrix.
 * This is intended to diminish parallel changing of eigenspaces.
 */
void Subspace::correctEigenvectorsFromSubIndices()
{
  DoLog(1) && (Log() << Verbose(1) << "Correcting EigenvectorMatrix ... " << std::endl);
  BOOST_FOREACH( boost::shared_ptr<Subspace> iter, SubIndices ) {
    const MatrixContent tempMatrix =
        projectFullspaceMatrixToSubspace(
            iter->projectSubspaceMatrixToFullspace(iter->getEigenvectorMatrix())
            );
    DoLog(1) && (Log() << Verbose(1) << "Subtracting matrix from " << *iter << ":" << tempMatrix << std::endl);
    EigenvectorMatrix -= tempMatrix;
  }
  DoLog(1) && (Log() << Verbose(1) << "Corrected EigenvectorMatrix is " << EigenvectorMatrix << std::endl);

  // check for null vectors
  const size_t max = EigenvectorMatrix.getColumns();
  for(size_t i = 0; i< max; ++i) {
    if (Eigenvectors[i]->IsZero()) {
      Eigenvalues[i] = 0.;
      Eigenvectors[i]->setZero();
    }
  }
}


/** Scale the local eigenvectors each by their eigenvalue.
 *
 */
void Subspace::scaleEigenvectorsbyEigenvalue()
{
  size_t localindex = 0;
  BOOST_FOREACH( boost::shared_ptr<VectorContent> ev, Eigenvectors) {
    *ev *= Eigenvalues[localindex];
    ++localindex;
  }
}


/** Get Norm of each eigenvector to serve als eigenvalue.
 *
 */
void Subspace::getNormofEigenvectorAsEigenvalue()
{
  size_t localindex = 0;
  BOOST_FOREACH( boost::shared_ptr<VectorContent> ev, Eigenvectors) {
    Eigenvalues[localindex] = ev->Norm();
    ++localindex;
  }
}


/** We remove the projections from lower subspaces from our Eigenspacematrix.
 * This is intended to diminish parallel changing of eigenspaces.
 */
void Subspace::correctProjectionMatricesFromSubIndices()
{
  MatrixContent subtractMatrix(ProjectToSubspace.getRows(), ProjectToSubspace.getColumns());
  DoLog(1) && (Log() << Verbose(1) << "Correcting ProjectToSubspace ... " << std::endl);
  BOOST_FOREACH( boost::shared_ptr<Subspace> iter, SubIndices ) {
    const MatrixContent tempMatrix = iter->getEigenvectorMatrix();
    const MatrixContent tempMatrix2 = iter->projectSubspaceMatrixToFullspace(tempMatrix);
    const MatrixContent tempMatrix3 = (tempMatrix2 * ProjectToSubspace);
    DoLog(1) && (Log() << Verbose(1) << "Subtracting matrix from " << *iter << ":" << tempMatrix3 << std::endl);
    subtractMatrix += tempMatrix3;
  }
  ProjectToSubspace -= subtractMatrix;
  DoLog(1) && (Log() << Verbose(1) << "Corrected ProjectToSubspace is " << ProjectToSubspace << std::endl);

  ProjectFromSubspace = ((const MatrixContent)ProjectToSubspace).transpose();
  DoLog(1) && (Log() << Verbose(1) << "Corrected ProjectFromSubspace is " << ProjectFromSubspace << std::endl);
}


/** Creates the inverse of LocalToGlobal.
 * Mapping is one-to-one and onto, hence it's simply turning around the map.
 */
void Subspace::invertLocalToGlobalMapping()
{
  GlobalToLocal.clear();
  for (mapping::const_iterator iter = LocalToGlobal.begin();
      iter != LocalToGlobal.end();
      ++iter) {
    GlobalToLocal.insert( make_pair(iter->second, iter->first) );
  }
}


/** Project calculated Eigenvectors into full space.
 *
 * @return set of projected eigenvectors.
 */
const Eigenspace::eigenvectorset & Subspace::getEigenvectorsInFullSpace()
{
  // check whether bigmatrix is at least as big as EigenspaceMatrix
  ASSERT(Eigenvectors.size() > 0,
      "embedEigenspaceMatrix() - no Eigenvectors given!");
  ASSERT(EigenspaceMatrix.getRows() <= Eigenvectors[0]->getDimension(),
      "embedEigenspaceMatrix() - EigenspaceMatrix has more rows "
      +toString(EigenspaceMatrix.getRows())+" than eigenvectors "
      +toString(Eigenvectors[0]->getDimension())+"!");
  ASSERT(EigenspaceMatrix.getColumns() <= Eigenvectors.size(),
      "embedEigenspaceMatrix() - EigenspaceMatrix has more columns "
      +toString(EigenspaceMatrix.getColumns())+" than eigenvectors "
      +toString(Eigenvectors.size())+"!");
  // check whether EigenspaceMatrix is big enough for both index sets
  ASSERT(EigenspaceMatrix.getColumns() == EigenspaceMatrix.getRows(),
      "embedEigenspaceMatrix() - EigenspaceMatrix is not square "
      +toString(EigenspaceMatrix.getRows())+" than needed by index set "
      +toString(EigenspaceMatrix.getColumns())+"!");
  ASSERT(Indices.size() == EigenspaceMatrix.getColumns(),
      "embedEigenspaceMatrix() - EigenspaceMatrix has not the same number of columns "
      +toString(EigenspaceMatrix.getColumns())+" compared to the index set "
      +toString(Indices.size())+"!");

  // construct intermediate matrix
  MatrixContent *intermediatematrix = new MatrixContent(Eigenvectors[0]->getDimension(), Indices.size());
  size_t localcolumn = 0;
  BOOST_FOREACH(size_t columnindex, Indices) {
    // create two vectors from each row and copy assign them
    boost::shared_ptr<VectorContent> srceigenvector(Eigenvectors[columnindex]);
    boost::shared_ptr<VectorContent> desteigenvector(intermediatematrix->getColumnVector(localcolumn));
    *desteigenvector = *srceigenvector;
    localcolumn++;
  }
  // matrix product with eigenbasis EigenspaceMatrix matrix
  *intermediatematrix *= EigenspaceMatrix;

  // and place at right columns into bigmatrix
  MatrixContent *bigmatrix = new MatrixContent(Eigenvectors[0]->getDimension(), Eigenvectors.size());
  bigmatrix->setZero();
  localcolumn = 0;
  BOOST_FOREACH(size_t columnindex, Indices) {
    // create two vectors from each row and copy assign them
    boost::shared_ptr<VectorContent> srceigenvector(intermediatematrix->getColumnVector(localcolumn));
    boost::shared_ptr<VectorContent> desteigenvector(bigmatrix->getColumnVector(columnindex));
    *desteigenvector = *srceigenvector;
    localcolumn++;
  }

  return Eigenvectors;
}


/** Getter for Subspace::SubIndices.
 *
 * @return const reference to Subspace::SubIndices.
 */
const Subspace::subset & Subspace::getSubIndices() const
{
  return SubIndices;
}

/** Remove a subset of indices from the SubIndices.
 *
 * @param _s subset to remove
 * @return true - removed, false - not found
 */
bool Subspace::removeSubset(boost::shared_ptr<Subspace> &_s)
{
  if (SubIndices.count(_s) != 0) {
    SubIndices.erase(_s);
    return true;
  } else {
    return false;
  }
}

/** Add a subspace set to SubIndices.
 *
 * @param _s subset to add
 * @return true - not present before, false - has already been present
 */
bool Subspace::addSubset(boost::shared_ptr<Subspace> & _s)
{
  if (SubIndices.count(_s) != 0)
    return false;
  else {
    SubIndices.insert(_s);
    return true;
  }
}


/** Simply a getter for Eigenvectors, as they are stored as subspace eigenvectors.
 *
 * @return set of eigenvectors in subspace
 */
const MatrixContent & Subspace::getEigenvectorMatrixInFullSpace()
{
  // check whether bigmatrix is at least as big as EigenspaceMatrix
  ASSERT(EigenspaceMatrix.getRows() <= FullSpace.getEigenspaceMatrix().getRows(),
      "embedEigenspaceMatrix() - EigenspaceMatrix has more rows "
      +toString(EigenspaceMatrix.getRows())+" than FullSpace::EigenspaceMatrix "
      +toString(Eigenvectors[0]->getDimension())+"!");
  ASSERT(EigenspaceMatrix.getColumns() <= FullSpace.getEigenspaceMatrix().getColumns(),
      "embedEigenspaceMatrix() - EigenspaceMatrix has more columns "
      +toString(EigenspaceMatrix.getColumns())+" than FullSpace::EigenspaceMatrix "
      +toString(Eigenvectors.size())+"!");
  // check whether EigenspaceMatrix is big enough for both index sets
  ASSERT(EigenspaceMatrix.getColumns() == EigenspaceMatrix.getRows(),
      "embedEigenspaceMatrix() - EigenspaceMatrix is not square "
      +toString(EigenspaceMatrix.getRows())+" than needed by index set "
      +toString(EigenspaceMatrix.getColumns())+"!");
  ASSERT(Indices.size() == EigenspaceMatrix.getColumns(),
      "embedEigenspaceMatrix() - EigenspaceMatrix has not the same number of columns "
      +toString(EigenspaceMatrix.getColumns())+" compared to the index set "
      +toString(Indices.size())+"!");

  // construct intermediate matrix
  MatrixContent *bigmatrix = new MatrixContent(
      FullSpace.getEigenspaceMatrix().getRows(),
      FullSpace.getEigenspaceMatrix().getColumns());
  *bigmatrix = ProjectToSubspace * EigenspaceMatrix * ProjectFromSubspace;

//  MatrixContent *intermediatematrix = new MatrixContent(FullSpace.getEigenspaceMatrix().getRows(), Indices.size());
//  size_t localcolumn = 0;
//  BOOST_FOREACH(size_t columnindex, Indices) {
//    // create two vectors from each row and copy assign them
//    boost::shared_ptr<VectorContent> srceigenvector(Eigenvectors[columnindex]);
//    boost::shared_ptr<VectorContent> desteigenvector(intermediatematrix->getColumnVector(localcolumn));
//    *desteigenvector = *srceigenvector;
//    localcolumn++;
//  }
//  // matrix product with eigenbasis EigenspaceMatrix matrix
//  *intermediatematrix *= EigenspaceMatrix;
//
//  // and place at right columns into bigmatrix
//  MatrixContent *bigmatrix = new MatrixContent(FullSpace.getEigenspaceMatrix().getRows(), Eigenvectors.size());
//  bigmatrix->setZero();
//  localcolumn = 0;
//  BOOST_FOREACH(size_t columnindex, Indices) {
//    // create two vectors from each row and copy assign them
//    boost::shared_ptr<VectorContent> srceigenvector(intermediatematrix->getColumnVector(localcolumn));
//    boost::shared_ptr<VectorContent> desteigenvector(bigmatrix->getColumnVector(columnindex));
//    *desteigenvector = *srceigenvector;
//    localcolumn++;
//  }

  return *bigmatrix;
}


/** Creates the projection matrix from Subspace::FullSpace::EigenvectorMatrix.
 * We simply copy the respective eigenvectors from Subspace::FullSpace into
 * Subspace::Eigenvectors.
 */
void Subspace::createProjectionMatrices()
{
  // first ProjectToSubspace

  // generate column vectors
  std::vector< boost::shared_ptr<VectorContent> > ColumnVectors;
  for (size_t i = 0; i < Indices.size(); ++i) {
    boost::shared_ptr<VectorContent> p(ProjectToSubspace.getColumnVector(i));
    ColumnVectors.push_back( p );
  }

  // then copy from real eigenvectors
  size_t localindex = 0;
  BOOST_FOREACH(size_t iter, Indices) {
    *ColumnVectors[localindex] = FullSpace.getEigenvector(iter);
    ++localindex;
  }
  DoLog(2) && (Log() << Verbose(2) << "ProjectionToSubspace matrix is " << ProjectToSubspace << std::endl);

  // then ProjectFromSubspace is just transposed
  ProjectFromSubspace = ((const MatrixContent)ProjectToSubspace).transpose();
  DoLog(2) && (Log() << Verbose(2) << "ProjectFromSubspace matrix is " << ProjectFromSubspace << std::endl);
}


/** Creates the subspace matrix by projecting down the given \a _fullmatrix.
 *  We just perform \f$M = P^t \cdot A \cdot P\f$, when P are the projection matrix,
 *  A the full matrix and M the subspace matrix.
 *
 *  @param _fullmatrix full matrix A to project into subspace
 *  @return subspace matrix M
 */
const MatrixContent Subspace::projectFullspaceMatrixToSubspace(const MatrixContent &_fullmatrix) const
{
  ASSERT((FullSpace.getIndices().size() == _fullmatrix.getRows())
      && (FullSpace.getIndices().size() == _fullmatrix.getColumns()),
      "Subspace::projectSubspaceMatrixToFullspace() - dimensions of this subspace "
      +toString(Indices.size())+" and the given matrix ("
      +toString(_fullmatrix.getRows())+" x "+toString(_fullmatrix.getColumns())+") differ!");
  // construct small matrix
  MatrixContent tempMatrix = ProjectFromSubspace * _fullmatrix * ProjectToSubspace;
  return tempMatrix;
  DoLog(2) && (Log() << Verbose(2) << "returned subspace matrix is " << tempMatrix << std::endl);
}


/** Creates a full space matrix which is the projection of given \a _subspacematrix
 * from the subspace.
 *
 * @param _subspacematrix subspace matrix
 * @return full matrix of the Subspace::EigenspaceMatrix projected into
 *         Subspace::FullSpace.
 */
const MatrixContent Subspace::projectSubspaceMatrixToFullspace(const MatrixContent &_subspacematrix) const
{
  ASSERT((Indices.size() == _subspacematrix.getRows()) && (Indices.size() == _subspacematrix.getColumns()),
      "Subspace::projectSubspaceMatrixToFullspace() - dimensions of this subspace "
      +toString(Indices.size())+" and the given matrix ("
      +toString(_subspacematrix.getRows())+" x "+toString(_subspacematrix.getColumns())+") differ!");
  return (ProjectToSubspace * _subspacematrix * ProjectFromSubspace);
}


/** We associate local Eigenvectors with ones from FullSpace by a parallelity
 * criterion.
 */
void Subspace::createLocalMapping()
{
  // first LocalToGlobal
  LocalToGlobal.clear();
  IndexSet CorrespondenceList(Indices); // is to ensure one-to-one mapping

  for (size_t localindex = 0; localindex< Indices.size(); ++localindex) {
    boost::shared_ptr<VectorContent> &CurrentEigenvector = Eigenvectors[localindex];
    VectorContent tempCurrentEigenvector = ProjectToSubspace * (*CurrentEigenvector);
    Log() << Verbose(1) << "Current Eigenvector is " << *CurrentEigenvector
        << " --(projected)-> " << tempCurrentEigenvector << std::endl;
    if (Eigenvalues[localindex] == 0) {
      DoLog(2) && (Log() << Verbose(2) << "Is null, skipping." << std::endl);
      continue;
    }

    // (for now settle with the one we are most parallel to)
    size_t mostparallel_index = FullSpace.getIndices().size();
    double mostparallel_scalarproduct = 0.;
    BOOST_FOREACH( size_t indexiter, CorrespondenceList) {
      DoLog(2) && (Log() << Verbose(2) << "Comparing to old " << indexiter << "th eigenvector " << FullSpace.getEigenvector(indexiter) << std::endl);
      const double scalarproduct = (FullSpace.getEigenvector(indexiter)) * ( tempCurrentEigenvector );
      DoLog(2) && (Log() << Verbose(2) << "SKP is " << scalarproduct << std::endl);
      if (fabs(scalarproduct) > fabs(mostparallel_scalarproduct)) {
        mostparallel_scalarproduct = scalarproduct;
        mostparallel_index = indexiter;
      }
    }
    // and make the assignment if we found one
    if (mostparallel_index != FullSpace.getIndices().size()) {
      // put into std::list for later use
      // invert if pointing in negative direction
      if (mostparallel_scalarproduct < 0) {
        *CurrentEigenvector *= -1.;
        DoLog(1) && (Log() << Verbose(1) << "Associating (inverted) " << *CurrentEigenvector << " with " << mostparallel_index << "th's fullspace eigenvector." << std::endl);
      } else {
        DoLog(1) && (Log() << Verbose(1) << "Associating " << *CurrentEigenvector << " with " << mostparallel_index << "th's fullspace eigenvector." << std::endl);
      }
      ASSERT( LocalToGlobal.count(localindex) == 0,
          "Subspace::createLocalMapping() - localindex "+toString(localindex)+" already assigned to "
          +toString(LocalToGlobal[localindex])+" !=? "+toString(mostparallel_index)+".");
      LocalToGlobal.insert( make_pair(localindex, mostparallel_index) );
      CorrespondenceList.erase(mostparallel_index);
    }
  }

  // then invert mapping
  GlobalToLocal.clear();
  BOOST_FOREACH(mapping::value_type iter, LocalToGlobal) {
    ASSERT(GlobalToLocal.count(iter.second) == 0,
        "Subspace::createLocalMapping() - LocalToGlobal is not bijective, key "
        +toString(iter.second)+" present more than once!");
    GlobalToLocal.insert( make_pair(iter.second, iter.first) );
  }
}


/** Get the local eigenvector that is most parallel to the \a i'th full one.
 * We just the internal mapping Subspace::GlobalToLocal.
 * @param i index of global eigenvector
 * @return local eigenvector, most parallel
 */
const VectorContent Subspace::getEigenvectorParallelToFullOne(size_t i)
{
  if (GlobalToLocal.count(i) == 0) {
    return ZeroVector;
  } else {
    return ProjectToSubspace*(*Eigenvectors[GlobalToLocal[i]]);
  }
}


/** Get the local eigenvalue of the eigenvector that is most parallel to the
 * \a i'th full one.
 * We just the internal mapping Subspace::GlobalToLocal.
 * @param i index of global eigenvector
 * @return eigenvalue of local eigenvector, most parallel
 */
const double Subspace::getEigenvalueOfEigenvectorParallelToFullOne(size_t i)
{
  if (GlobalToLocal.count(i) == 0) {
    return 0.;
  } else {
    return Eigenvalues[GlobalToLocal[i]];
  }
}