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

/*
 * SubspaceFactorizerUnittest.cpp
 *
 *  Created on: Nov 13, 2010
 *      Author: heber
 */

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

#include <cppunit/CompilerOutputter.h>
#include <cppunit/extensions/TestFactoryRegistry.h>
#include <cppunit/ui/text/TestRunner.h>

#include <cmath>

#include <gsl/gsl_vector.h>
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>

#include "Helpers/Assert.hpp"
#include "Helpers/Log.hpp"
#include "Helpers/toString.hpp"
#include "Helpers/Verbose.hpp"
#include "LinearAlgebra/Eigenspace.hpp"
#include "LinearAlgebra/MatrixContent.hpp"
#include "LinearAlgebra/Subspace.hpp"
#include "LinearAlgebra/VectorContent.hpp"

#include "SubspaceFactorizerUnittest.hpp"

#ifdef HAVE_TESTRUNNER
#include "UnitTestMain.hpp"
#endif /*HAVE_TESTRUNNER*/

// Registers the fixture into the 'registry'
CPPUNIT_TEST_SUITE_REGISTRATION( SubspaceFactorizerUnittest );

void SubspaceFactorizerUnittest::setUp(){
  matrix = new MatrixContent(matrixdimension,matrixdimension);
  matrix->setZero();
  for (int i=0; i<matrixdimension ; i++) {
    matrix->set(i,i, 2.);
    if (i < (matrixdimension-1)) {
      matrix->set(i+1,i, 1.);
      matrix->set(i,i+1, 1.);
    }
  }
}

void SubspaceFactorizerUnittest::tearDown(){
  // delete test matrix
  delete matrix;
}

void SubspaceFactorizerUnittest::BlockTest()
{
  MatrixContent *transformation = new MatrixContent(matrixdimension,matrixdimension);
  transformation->setZero();
  for (size_t j=0; j<1; ++j)
    transformation->set(j,j, 1.);

  MatrixContent temp((*matrix)&(*transformation));
  std::cout << "Our matrix is " << *matrix << "." << std::endl;

  std::cout << "Hadamard product of " << *matrix << " with " << *transformation << " is: " << std::endl;
  std::cout << temp << std::endl;

  gsl_vector *eigenvalues = temp.transformToEigenbasis();
  VectorContent *eigenvaluesView = new VectorViewContent(gsl_vector_subvector(eigenvalues, 0, eigenvalues->size));
  std::cout << "The resulting eigenbasis is " << temp
      << "\n\t with eigenvalues " << *eigenvaluesView << std::endl;
  delete eigenvaluesView;
  gsl_vector_free(eigenvalues);
  delete (transformation);


  CPPUNIT_ASSERT_EQUAL(0,0);
}

/** For given set of row and column indices, we extract the small block matrix.
 * @param bigmatrix big matrix to extract from
 * @param Eigenvectors eigenvectors of the subspaces to obtain matrix in
 * @param columnindexset index set to pick out of all indices
 * @return small matrix with dimension equal to the number of indices for row and column.
 */
MatrixContent * getSubspaceMatrix(
    MatrixContent &bigmatrix,
    VectorArray &Eigenvectors,
    const IndexSet &indexset)
{
  // check whether subsystem is big enough for both index sets
  ASSERT(indexset.size() <= bigmatrix.getRows(),
      "embedSubspaceMatrix() - bigmatrix has less rows "+toString(bigmatrix.getRows())
      +" than needed by index set "
      +toString(indexset.size())+"!");
  ASSERT(indexset.size() <= bigmatrix.getColumns(),
      "embedSubspaceMatrix() - bigmatrix has less columns "+toString(bigmatrix.getColumns())
      +" than needed by index set "
      +toString(indexset.size())+"!");

  // construct small matrix
  MatrixContent *subsystem =  new MatrixContent(indexset.size(), indexset.size());
  size_t localrow = 0; // local row indices for the subsystem
  size_t localcolumn = 0;
  BOOST_FOREACH( size_t rowindex, indexset) {
    localcolumn = 0;
    BOOST_FOREACH( size_t columnindex, indexset) {
      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++;
  }
  return subsystem;
}

/** For a given set of row and columns indices, we embed a small block matrix into a bigger space.
 *
 * @param eigenvectors current eigenvectors
 * @param rowindexset row index set
 * @param columnindexset column index set
 * @return bigmatrix with eigenvectors contained
 */
MatrixContent * embedSubspaceMatrix(
    VectorArray &Eigenvectors,
    MatrixContent &subsystem,
    const IndexSet &columnindexset)
{
  // check whether bigmatrix is at least as big as subsystem
  ASSERT(Eigenvectors.size() > 0,
      "embedSubspaceMatrix() - no Eigenvectors given!");
  ASSERT(subsystem.getRows() <= Eigenvectors[0]->getDimension(),
      "embedSubspaceMatrix() - subsystem has more rows "
      +toString(subsystem.getRows())+" than eigenvectors "
      +toString(Eigenvectors[0]->getDimension())+"!");
  ASSERT(subsystem.getColumns() <= Eigenvectors.size(),
      "embedSubspaceMatrix() - subsystem has more columns "
      +toString(subsystem.getColumns())+" than eigenvectors "
      +toString(Eigenvectors.size())+"!");
  // check whether subsystem is big enough for both index sets
  ASSERT(subsystem.getColumns() == subsystem.getRows(),
      "embedSubspaceMatrix() - subsystem is not square "
      +toString(subsystem.getRows())+" than needed by index set "
      +toString(subsystem.getColumns())+"!");
  ASSERT(columnindexset.size() == subsystem.getColumns(),
      "embedSubspaceMatrix() - subsystem has not the same number of columns "
      +toString(subsystem.getColumns())+" compared to the index set "
      +toString(columnindexset.size())+"!");

  // construct intermediate matrix
  MatrixContent *intermediatematrix = new MatrixContent(Eigenvectors[0]->getDimension(), columnindexset.size());
  size_t localcolumn = 0;
  BOOST_FOREACH(size_t columnindex, columnindexset) {
    // 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 subsystem matrix
  *intermediatematrix *= subsystem;

  // 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, columnindexset) {
    // 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;
}

/** Prints the scalar product of each possible pair that is not orthonormal.
 * We use class logger for printing.
 * @param AllIndices set of all possible indices of the eigenvectors
 * @param CurrentEigenvectors array of eigenvectors
 * @return true - all are orthonormal to each other,
 *        false - some are not orthogonal or not normalized.
 */
bool checkOrthogonality(const IndexSet &AllIndices, const VectorArray &CurrentEigenvectors)
{
  size_t nonnormalized = 0;
  size_t nonorthogonal = 0;
  // check orthogonality
  BOOST_FOREACH( size_t firstindex, AllIndices) {
    BOOST_FOREACH( size_t secondindex, AllIndices) {
      const double scp = (*CurrentEigenvectors[firstindex])*(*CurrentEigenvectors[secondindex]);
      if (firstindex == secondindex) {
        if (fabs(scp - 1.) > MYEPSILON) {
          nonnormalized++;
          Log() << Verbose(1) << "Vector " << firstindex << " is not normalized, off by "
              << fabs(1.-(*CurrentEigenvectors[firstindex])*(*CurrentEigenvectors[secondindex])) << std::endl;
        }
      } else {
        if (fabs(scp) > MYEPSILON) {
          nonorthogonal++;
          Log() << Verbose(1) << "Scalar product between " << firstindex << " and " << secondindex
              << " is " << (*CurrentEigenvectors[firstindex])*(*CurrentEigenvectors[secondindex]) << std::endl;
        }
      }
    }
  }

  if ((nonnormalized == 0) && (nonorthogonal == 0)) {
    Log() << Verbose(1) << "All vectors are orthonormal to each other." << std::endl;
    return true;
  }
  if ((nonnormalized == 0) && (nonorthogonal != 0))
    Log() << Verbose(1) << "All vectors are normalized." << std::endl;
  if ((nonnormalized != 0) && (nonorthogonal == 0))
    Log() << Verbose(1) << "All vectors are orthogonal to each other." << std::endl;
  return false;
}

/** Calculate the sum of the scalar product of each possible pair.
 * @param AllIndices set of all possible indices of the eigenvectors
 * @param CurrentEigenvectors array of eigenvectors
 * @return sum of scalar products between all possible pairs
 */
double calculateOrthogonalityThreshold(const IndexSet &AllIndices, const VectorArray &CurrentEigenvectors)
{
  double threshold = 0.;
  // check orthogonality
  BOOST_FOREACH( size_t firstindex, AllIndices) {
    BOOST_FOREACH( size_t secondindex, AllIndices) {
      const double scp = (*CurrentEigenvectors[firstindex])*(*CurrentEigenvectors[secondindex]);
      if (firstindex == secondindex) {
        threshold += fabs(scp - 1.);
      } else {
        threshold += fabs(scp);
      }
    }
  }
  return threshold;
}

/** Operator for output to std::ostream operator of an IndexSet.
 * @param ost output stream
 * @param indexset index set to output
 * @return ost output stream
 */
std::ostream & operator<<(std::ostream &ost, const IndexSet &indexset)
{
  ost << "{ ";
  for (IndexSet::const_iterator iter = indexset.begin();
      iter != indexset.end();
      ++iter)
    ost << *iter << " ";
  ost << "}";
  return ost;
}

/** Assign eigenvectors of subspace to full eigenvectors.
 * We use parallelity as relation measure.
 * @param eigenvalue eigenvalue to assign along with
 * @param CurrentEigenvector eigenvector to assign, is taken over within
 *        boost::shared_ptr
 * @param CurrentEigenvectors full eigenvectors
 * @param CorrespondenceList list to make sure that each subspace eigenvector
 *        is assigned to a unique full eigenvector
 * @param ParallelEigenvectorList list of "similar" subspace eigenvectors per
 *        full eigenvector, allocated
 */
void AssignSubspaceEigenvectors(
    double eigenvalue,
    VectorContent *CurrentEigenvector,
    VectorArray &CurrentEigenvectors,
    IndexSet &CorrespondenceList,
    VectorValueList *&ParallelEigenvectorList)
{
  Log() << Verbose(1) << "Current Eigenvector is " << *CurrentEigenvector << std::endl;

  // (for now settle with the one we are most parallel to)
  size_t mostparallel_index = SubspaceFactorizerUnittest::matrixdimension;
  double mostparallel_scalarproduct = 0.;
  BOOST_FOREACH( size_t indexiter, CorrespondenceList) {
    Log() << Verbose(2) << "Comparing to old " << indexiter << "th eigenvector " << *(CurrentEigenvectors[indexiter]) << std::endl;
    const double scalarproduct = (*(CurrentEigenvectors[indexiter])) * (*CurrentEigenvector);
    Log() << Verbose(2) << "SKP is " << scalarproduct << std::endl;
    if (fabs(scalarproduct) > mostparallel_scalarproduct) {
      mostparallel_scalarproduct = fabs(scalarproduct);
      mostparallel_index = indexiter;
    }
  }
  if (mostparallel_index != SubspaceFactorizerUnittest::matrixdimension) {
    // put into std::list for later use
    // invert if pointing in negative direction
    if ((*(CurrentEigenvectors[mostparallel_index])) * (*CurrentEigenvector) < 0) {
      *CurrentEigenvector *= -1.;
      Log() << Verbose(1) << "Pushing (inverted) " << *CurrentEigenvector << " into parallel list [" << mostparallel_index << "]" << std::endl;
    } else {
      Log() << Verbose(1) << "Pushing " << *CurrentEigenvector << " into parallel list [" << mostparallel_index << "]" << std::endl;
    }
    ParallelEigenvectorList[mostparallel_index].push_back(make_pair(boost::shared_ptr<VectorContent>(CurrentEigenvector), eigenvalue));
    CorrespondenceList.erase(mostparallel_index);
  }
}

void SubspaceFactorizerUnittest::EigenvectorTest()
{
  VectorArray CurrentEigenvectors;
  ValueArray CurrentEigenvalues;
  VectorValueList *ParallelEigenvectorList = new VectorValueList[matrixdimension];
  IndexSet AllIndices;

  // create the total index set
  for (size_t i=0;i<matrixdimension;++i)
    AllIndices.insert(i);

  // create all consecutive index subsets for dim 1 to 3
  IndexMap Dimension_to_Indexset;
  for (size_t dim = 0; dim<3;++dim) {
    for (size_t i=0;i<matrixdimension;++i) {
      IndexSet *indexset = new IndexSet;
      for (size_t j=0;j<dim+1;++j) {
        const int value = (i+j) % matrixdimension;
        //std::cout << "Putting " << value << " into " << i << "th map " << dim << std::endl;
        CPPUNIT_ASSERT_MESSAGE("index "+toString(value)+" already present in "+toString(dim)+"-dim "+toString(i)+"th indexset.", indexset->count(value) == 0);
        indexset->insert(value);
      }
      Dimension_to_Indexset.insert( make_pair(dim, boost::shared_ptr<IndexSet>(indexset)) );
      // no need to free indexset, is stored in shared_ptr and
      // will get released when Dimension_to_Indexset is destroyed
    }
  }

  // set to first guess, i.e. the unit vectors of R^matrixdimension
  BOOST_FOREACH( size_t index, AllIndices) {
    boost::shared_ptr<VectorContent> EV(new VectorContent(matrixdimension));
    EV->setZero();
    EV->at(index) = 1.;
    CurrentEigenvectors.push_back(EV);
    CurrentEigenvalues.push_back(0.);
  }

  size_t run=1; // counting iterations
  double threshold = 1.;  // containing threshold value
  while ((threshold > 1e-10) && (run < 2)) {
    // for every dimension
    for (size_t dim = 0; dim<4;++dim) {
      // for every index set of this dimension
      Log() << Verbose(0) << std::endl << std::endl;
      Log() << Verbose(0) << "Current dimension is " << dim << std::endl;
      std::pair<IndexMap::const_iterator,IndexMap::const_iterator> Bounds = Dimension_to_Indexset.equal_range(dim);
      for (IndexMap::const_iterator IndexsetIter = Bounds.first;
          IndexsetIter != Bounds.second;
          ++IndexsetIter) {
        // show the index set
        Log() << Verbose(0) << std::endl;
        Log() << Verbose(1) << "Current index set is " << *(IndexsetIter->second) << std::endl;

        // create transformation matrices from these
        MatrixContent *subsystem = getSubspaceMatrix(*matrix, CurrentEigenvectors, *(IndexsetIter->second));
        Log() << Verbose(2) << "Subsystem matrix is " << *subsystem << std::endl;

        // solve _small_ systems for eigenvalues
        VectorContent *Eigenvalues = new VectorContent(subsystem->transformToEigenbasis());
        Log() << Verbose(2) << "Eigenvector matrix is " << *subsystem << std::endl;
        Log() << Verbose(2) << "Eigenvalues are " << *Eigenvalues << std::endl;

        // blow up eigenvectors to matrixdimensiondim column vector again
        MatrixContent *Eigenvectors = embedSubspaceMatrix(CurrentEigenvectors, *subsystem, *(IndexsetIter->second));
        Log() << Verbose(1) << matrixdimension << "x" << matrixdimension << " Eigenvector matrix is " << *Eigenvectors << std::endl;

        // we don't need the subsystem anymore
        delete subsystem;

        // go through all eigenvectors in this subspace
        IndexSet CorrespondenceList((*IndexsetIter->second)); // assure one-to-one and onto assignment
        size_t localindex = 0;
        BOOST_FOREACH( size_t iter, (*IndexsetIter->second)) {
          // recognize eigenvectors parallel to existing ones
          AssignSubspaceEigenvectors(
              Eigenvalues->at(localindex),
              new VectorContent(Eigenvectors->getColumnVector(iter)),
              CurrentEigenvectors,
              CorrespondenceList,
              ParallelEigenvectorList);
          localindex++;
        }

        // free eigenvectors
        delete Eigenvectors;
        delete Eigenvalues;
      }
    }

    // print list of similar eigenvectors
    BOOST_FOREACH( size_t index, AllIndices) {
      Log() << Verbose(2) << "Similar to " << index << "th current eigenvector " << *(CurrentEigenvectors[index]) << " are:" << std::endl;
      BOOST_FOREACH( VectorValueList::value_type &iter, ParallelEigenvectorList[index] ) {
        Log() << Verbose(2) << *(iter.first) << std::endl;
      }
      Log() << Verbose(2) << std::endl;
    }

    // create new CurrentEigenvectors from averaging parallel ones.
    BOOST_FOREACH(size_t index, AllIndices) {
      CurrentEigenvectors[index]->setZero();
      CurrentEigenvalues[index] = 0.;
      BOOST_FOREACH( VectorValueList::value_type &iter, ParallelEigenvectorList[index] ) {
        *CurrentEigenvectors[index] += (*iter.first) * (iter.second);
        CurrentEigenvalues[index] += (iter.second);
      }
      *CurrentEigenvectors[index] *= 1./CurrentEigenvalues[index];
      CurrentEigenvalues[index] /= (double)ParallelEigenvectorList[index].size();
      ParallelEigenvectorList[index].clear();
    }

    // check orthonormality
    threshold = calculateOrthogonalityThreshold(AllIndices, CurrentEigenvectors);
    bool dontOrthonormalization = checkOrthogonality(AllIndices, CurrentEigenvectors);

    // orthonormalize
    if (!dontOrthonormalization) {
      Log() << Verbose(0) << "Orthonormalizing ... " << std::endl;
      for (IndexSet::const_iterator firstindex = AllIndices.begin();
          firstindex != AllIndices.end();
          ++firstindex) {
        for (IndexSet::const_iterator secondindex = firstindex;
            secondindex != AllIndices.end();
            ++secondindex) {
          if (*firstindex == *secondindex) {
            (*CurrentEigenvectors[*secondindex]) *= 1./(*CurrentEigenvectors[*secondindex]).Norm();
          } else {
            (*CurrentEigenvectors[*secondindex]) -=
                ((*CurrentEigenvectors[*firstindex])*(*CurrentEigenvectors[*secondindex]))
                *(*CurrentEigenvectors[*firstindex]);
          }
        }
      }
    }

//    // check orthonormality again
//    checkOrthogonality(AllIndices, CurrentEigenvectors);

    // show new ones
    Log() << Verbose(0) << "Resulting new eigenvectors and -values, run " << run << " are:" << std::endl;
    BOOST_FOREACH( size_t index, AllIndices) {
      Log() << Verbose(0) << *CurrentEigenvectors[index] << " with " << CurrentEigenvalues[index] << std::endl;
    }
    run++;
  }


  delete[] ParallelEigenvectorList;

  CPPUNIT_ASSERT_EQUAL(0,0);
}

void SubspaceFactorizerUnittest::SubspaceTest()
{
  Log() << Verbose(0) << std::endl << std::endl;
  // create the total index set
  Log() << Verbose(1) << "Generating full space ..." << std::endl;
  IndexSet AllIndices;
  for (size_t i=0;i<matrixdimension;++i)
    AllIndices.insert(i);
  Eigenspace FullSpace(AllIndices, *matrix);

  Log() << Verbose(1) << "Generating sub spaces ..." << std::endl;
  // create all consecutive index subsets for dim 1 to 3
  SubspaceMap Dimension_to_Indexset;
  for (size_t dim = 0; dim<3;++dim) {
    for (size_t i=0;i<matrixdimension;++i) {
      IndexSet *indexset = new IndexSet;
      for (size_t j=0;j<dim+1;++j) {
        const int value = (i+j) % matrixdimension;
        //std::cout << "Putting " << value << " into " << i << "th map " << dim << std::endl;
        CPPUNIT_ASSERT_MESSAGE("index "+toString(value)+" already present in "+toString(dim)+"-dim "+toString(i)+"th indexset.", indexset->count(value) == 0);
        indexset->insert(value);
      }
      boost::shared_ptr<Subspace> subspace(new Subspace(*indexset, FullSpace));
      Log() << Verbose(1) << "Current subspace is " << *subspace << std::endl;
      Dimension_to_Indexset.insert( make_pair(dim, boost::shared_ptr<Subspace>(subspace)) );
      delete(indexset);

      // for through next lower subspace list and add to subspaces if contained.
      if (dim != 0) {
        Log() << Verbose(1) << "Going through subspace list in dim " << dim-1 << "." << std::endl;
        BOOST_FOREACH( SubspaceMap::value_type entry, Dimension_to_Indexset.equal_range(dim-1)) {
          if (subspace->contains(*entry.second)) {
            Log() << Verbose(2) << "Adding " << *(entry.second) << " to list of contained subspaces." << std::endl;
            subspace->addSubset(entry.second);
          }
        }
      } else {
        Log() << Verbose(2) << "Subspace with dimension of 1 cannot contain other subspaces." << std::endl;
      }
    }
  }

  CPPUNIT_ASSERT_EQUAL(0,0);
}