/*
 * 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 <boost/timer.hpp>

#include "CodePatterns/Assert.hpp"
#include "Helpers/defs.hpp"
#include "CodePatterns/Log.hpp"
#include "CodePatterns/toString.hpp"
#include "CodePatterns/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 (size_t i=0; i<matrixdimension ; i++) {
    for (size_t j=0; j<= i; ++j) {
      //const double value = 10. * rand() / (double)RAND_MAX;
      //const double value = i==j ? 2. : 1.;
      if (i==j)
        matrix->set(i,i, 2.);
      else if (j+1 == i) {
        matrix->set(i,j, 1.);
        matrix->set(j,i, 1.);
      } else {
        matrix->set(i,j, 0.);
        matrix->set(j,i, 0.);
      }
    }
  }
}

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(2) << "Vector " << firstindex << " is not normalized, off by "
              << fabs(1.-(*CurrentEigenvectors[firstindex])*(*CurrentEigenvectors[secondindex])) << std::endl;
        }
      } else {
        if (fabs(scp) > MYEPSILON) {
          nonorthogonal++;
          Log() << Verbose(2) << "Scalar product between " << firstindex << " and " << secondindex
              << " is " << (*CurrentEigenvectors[firstindex])*(*CurrentEigenvectors[secondindex]) << std::endl;
        }
      }
    }
  }

  if ((nonnormalized == 0) && (nonorthogonal == 0)) {
    DoLog(1) && (DoLog(1) && (Log() << Verbose(1) << "All vectors are orthonormal to each other." << std::endl));
    return true;
  }
  if ((nonnormalized == 0) && (nonorthogonal != 0))
    DoLog(1) && (DoLog(1) && (Log() << Verbose(1) << "All vectors are normalized." << std::endl));
  if ((nonnormalized != 0) && (nonorthogonal == 0))
    DoLog(1) && (DoLog(1) && (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)
{
  DoLog(1) && (DoLog(1) && (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) {
    DoLog(2) && (DoLog(2) && (Log() << Verbose(2) << "Comparing to old " << indexiter << "th eigenvector " << *(CurrentEigenvectors[indexiter]) << std::endl));
    const double scalarproduct = (*(CurrentEigenvectors[indexiter])) * (*CurrentEigenvector);
    DoLog(2) && (DoLog(2) && (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.;
      DoLog(1) && (Log() << Verbose(1) << "Pushing (inverted) " << *CurrentEigenvector << " into parallel list [" << mostparallel_index << "]" << std::endl);
    } else {
      DoLog(1) && (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 < 10)) {
    // for every dimension
    for (size_t dim = 0; dim<subspacelimit;++dim) {
      // for every index set of this dimension
      DoLog(0) && (Log() << Verbose(0) << std::endl << std::endl);
      DoLog(0) && (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
        DoLog(0) && (Log() << Verbose(0) << std::endl);
        DoLog(1) && (Log() << Verbose(1) << "Current index set is " << *(IndexsetIter->second) << std::endl);

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

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

        // blow up eigenvectors to matrixdimensiondim column vector again
        MatrixContent *Eigenvectors = embedSubspaceMatrix(CurrentEigenvectors, *subsystem, *(IndexsetIter->second));
        DoLog(1) && (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) {
      DoLog(2) && (Log() << Verbose(2) << "Similar to " << index << "th current eigenvector " << *(CurrentEigenvectors[index]) << " are:" << std::endl);
      BOOST_FOREACH( VectorValueList::value_type &iter, ParallelEigenvectorList[index] ) {
        DoLog(2) && (Log() << Verbose(2) << *(iter.first) << std::endl);
      }
      DoLog(2) && (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) {
      DoLog(0) && (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
    DoLog(0) && (Log() << Verbose(0) << "Resulting new eigenvectors and -values, run " << run << " are:" << std::endl);
    BOOST_FOREACH( size_t index, AllIndices) {
      DoLog(0) && (Log() << Verbose(0) << *CurrentEigenvectors[index] << " with " << CurrentEigenvalues[index] << std::endl);
    }
    run++;
  }

  delete[] ParallelEigenvectorList;

  CPPUNIT_ASSERT_EQUAL(0,0);
}


/** Iterative function to generate all power sets of indices of size \a maxelements.
 *
 * @param SetofSets Container for all sets
 * @param CurrentSet pointer to current set in this container
 * @param Indices Source set of indices
 * @param maxelements number of elements of each set in final SetofSets
 * @return true - generation continued, false - current set already had
 *         \a maxelements elements
 */
bool generatePowerSet(
    SetofIndexSets &SetofSets,
    SetofIndexSets::iterator &CurrentSet,
    IndexSet &Indices,
    const size_t maxelements)
{
  if (CurrentSet->size() < maxelements) {
    // allocate the needed sets
    const size_t size = Indices.size() - CurrentSet->size();
    std::vector<std::set<size_t> > SetExpanded;
    SetExpanded.reserve(size);

    // copy the current set into each
    for (size_t i=0;i<size;++i)
      SetExpanded.push_back(*CurrentSet);

    // expand each set by one index
    size_t localindex=0;
    BOOST_FOREACH(size_t iter, Indices) {
      if (CurrentSet->count(iter) == 0) {
        SetExpanded[localindex].insert(iter);
        ++localindex;
      }
    }

    // insert set at position of CurrentSet
    for (size_t i=0;i<size;++i) {
      //DoLog(1) && (Log() << Verbose(1) << "Inserting set #" << i << ": " << SetExpanded[i] << std::endl);
      SetofSets.insert(CurrentSet, SetExpanded[i]);
    }
    SetExpanded.clear();

    // and remove the current set
    //SetofSets.erase(CurrentSet);
    //CurrentSet = SetofSets.begin();

    // set iterator to a valid position again
    ++CurrentSet;
    return true;
  } else {
    return false;
  }
}

void SubspaceFactorizerUnittest::generatePowerSetTest()
{
  IndexSet AllIndices;
  for (size_t i=0;i<4;++i)
    AllIndices.insert(i);

  SetofIndexSets SetofSets;
  // note that starting off empty set is unstable
  IndexSet singleset;
  BOOST_FOREACH(size_t iter, AllIndices) {
    singleset.insert(iter);
    SetofSets.insert(singleset);
    singleset.clear();
  }
  SetofIndexSets::iterator CurrentSet = SetofSets.begin();
  while (CurrentSet != SetofSets.end()) {
    //DoLog(0) && (Log() << Verbose(0) << "Current set is " << *CurrentSet << std::endl);
    if (!generatePowerSet(SetofSets, CurrentSet, AllIndices, 2)) {
      // go to next set
      ++CurrentSet;
    }
  }

  SetofIndexSets ComparisonSet;
  // now follows a very stupid construction
  // because we can't use const arrays of some type meaningfully ...
  { IndexSet tempSet; tempSet.insert(0); ComparisonSet.insert(tempSet); }
  { IndexSet tempSet; tempSet.insert(1); ComparisonSet.insert(tempSet); }
  { IndexSet tempSet; tempSet.insert(2); ComparisonSet.insert(tempSet); }
  { IndexSet tempSet; tempSet.insert(3); ComparisonSet.insert(tempSet); }

  { IndexSet tempSet; tempSet.insert(0); tempSet.insert(1); ComparisonSet.insert(tempSet); }
  { IndexSet tempSet; tempSet.insert(0); tempSet.insert(2); ComparisonSet.insert(tempSet); }
  { IndexSet tempSet; tempSet.insert(0); tempSet.insert(3); ComparisonSet.insert(tempSet); }

  { IndexSet tempSet; tempSet.insert(1); tempSet.insert(2); ComparisonSet.insert(tempSet); }
  { IndexSet tempSet; tempSet.insert(1); tempSet.insert(3); ComparisonSet.insert(tempSet); }

  { IndexSet tempSet; tempSet.insert(2); tempSet.insert(3); ComparisonSet.insert(tempSet); }

  CPPUNIT_ASSERT_EQUAL(SetofSets, ComparisonSet);
}

bool cmd(double a, double b)
{
  return a > b;
}

void SubspaceFactorizerUnittest::SubspaceTest()
{
  Eigenspace::eigenvectorset CurrentEigenvectors;
  Eigenspace::eigenvalueset CurrentEigenvalues;

  setVerbosity(0);

  boost::timer Time_generatingfullspace;
  DoLog(0) && (Log() << Verbose(0) << std::endl << std::endl);
  // create the total index set
  IndexSet AllIndices;
  for (size_t i=0;i<matrixdimension;++i)
    AllIndices.insert(i);
  Eigenspace FullSpace(AllIndices, *matrix);
  DoLog(1) && (Log() << Verbose(1) << "Generated full space: " << FullSpace << std::endl);
  DoLog(0) && (Log() << Verbose(0) << "Full space generation took " << Time_generatingfullspace.elapsed() << " seconds." << std::endl);

  // generate first set of eigenvectors
  // 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.);
  }

  boost::timer Time_generatingsubsets;
  DoLog(0) && (Log() << Verbose(0) << "Generating sub sets ..." << std::endl);
  SetofIndexSets SetofSets;
  // note that starting off empty set is unstable
  IndexSet singleset;
  BOOST_FOREACH(size_t iter, AllIndices) {
    singleset.insert(iter);
    SetofSets.insert(singleset);
    singleset.clear();
  }
  SetofIndexSets::iterator CurrentSet = SetofSets.begin();
  while (CurrentSet != SetofSets.end()) {
    //DoLog(2) && (Log() << Verbose(2) << "Current set is " << *CurrentSet << std::endl);
    if (!generatePowerSet(SetofSets, CurrentSet, AllIndices, subspacelimit)) {
      // go to next set
      ++CurrentSet;
    }
  }
  DoLog(0) && (Log() << Verbose(0) << "Sub set generation took " << Time_generatingsubsets.elapsed() << " seconds." << std::endl);

  // create a subspace to each set and and to respective level
  boost::timer Time_generatingsubspaces;
  DoLog(0) && (Log() << Verbose(0) << "Generating sub spaces ..." << std::endl);
  SubspaceMap Dimension_to_Indexset;
  BOOST_FOREACH(std::set<size_t> iter, SetofSets) {
    boost::shared_ptr<Subspace> subspace(new Subspace(iter, FullSpace));
    DoLog(1) && (Log() << Verbose(1) << "Current subspace is " << *subspace << std::endl);
    Dimension_to_Indexset.insert( make_pair(iter.size(), boost::shared_ptr<Subspace>(subspace)) );
  }

  for (size_t dim = 1; dim<=subspacelimit;++dim) {
    BOOST_FOREACH( SubspaceMap::value_type subspace, Dimension_to_Indexset.equal_range(dim)) {
      if (dim != 0) { // from level 1 and onward
        BOOST_FOREACH( SubspaceMap::value_type entry, Dimension_to_Indexset.equal_range(dim-1)) {
          if (subspace.second->contains(*entry.second)) {
            // if contained then add ...
            subspace.second->addSubset(entry.second);
            // ... and also its containees as they are all automatically contained as well
            BOOST_FOREACH(boost::shared_ptr<Subspace> iter, entry.second->SubIndices) {
              subspace.second->addSubset(iter);
            }
          }
        }
      }
    }
  }
  DoLog(0) && (Log() << Verbose(0) << "Sub space generation took " << Time_generatingsubspaces.elapsed() << " seconds." << std::endl);

  // create a file handle for the eigenvalues
  std::ofstream outputvalues("eigenvalues.dat", std::ios_base::trunc);
  ASSERT(outputvalues.good(),
      "SubspaceFactorizerUnittest::EigenvectorTest() - failed to open eigenvalue file!");
  outputvalues << "# iteration ";
  BOOST_FOREACH(size_t iter, AllIndices) {
    outputvalues << "\teigenvalue" << iter;
  }
  outputvalues << std::endl;

  DoLog(0) && (Log() << Verbose(0) << "Solving ..." << std::endl);
  boost::timer Time_solving;
  size_t run=1; // counting iterations
  double threshold = 1.;  // containing threshold value
  while ((threshold > MYEPSILON) && (run < 20)) {
    // for every dimension
    for (size_t dim = 1; dim <= subspacelimit;++dim) {
      // for every index set of this dimension
      DoLog(1) && (Log() << Verbose(1) << std::endl << std::endl);
      DoLog(1) && (Log() << Verbose(1) << "Current dimension is " << dim << std::endl);
      std::pair<SubspaceMap::const_iterator,SubspaceMap::const_iterator> Bounds = Dimension_to_Indexset.equal_range(dim);
      for (SubspaceMap::const_iterator IndexsetIter = Bounds.first;
          IndexsetIter != Bounds.second;
          ++IndexsetIter) {
        Subspace& subspace = *(IndexsetIter->second);
        // show the index set
        DoLog(2) && (Log() << Verbose(2) << std::endl);
        DoLog(2) && (Log() << Verbose(2) << "Current subspace is " << subspace << std::endl);

        // solve
        subspace.calculateEigenSubspace();

        // note that assignment to global eigenvectors all remains within subspace
      }
    }

    // print list of similar eigenvectors
    DoLog(2) && (Log() << Verbose(2) << std::endl);
    BOOST_FOREACH( size_t index, AllIndices) {
      DoLog(2) && (Log() << Verbose(2) << "Similar to " << index << "th current eigenvector " << *(CurrentEigenvectors[index]) << " are:" << std::endl);
      BOOST_FOREACH( SubspaceMap::value_type iter, Dimension_to_Indexset) {
        const VectorContent & CurrentEV = (iter.second)->getEigenvectorParallelToFullOne(index);
        if (!CurrentEV.IsZero())
          Log() << Verbose(2)
          << "dim" << iter.first
          << ", subspace{" << (iter.second)->getIndices()
          << "}: "<<CurrentEV << std::endl;
      }
      DoLog(2) && (Log() << Verbose(2) << std::endl);
    }

    // create new CurrentEigenvectors from averaging parallel ones.
    BOOST_FOREACH(size_t index, AllIndices) {
      CurrentEigenvectors[index]->setZero();
      CurrentEigenvalues[index] = 0.;
      size_t count = 0;
      BOOST_FOREACH( SubspaceMap::value_type iter, Dimension_to_Indexset) {
        const VectorContent CurrentEV = (iter.second)->getEigenvectorParallelToFullOne(index);
        *CurrentEigenvectors[index] += CurrentEV; // * (iter.second)->getEigenvalueOfEigenvectorParallelToFullOne(index);
        CurrentEigenvalues[index] += (iter.second)->getEigenvalueOfEigenvectorParallelToFullOne(index);
        if (!CurrentEV.IsZero())
          count++;
      }
      *CurrentEigenvectors[index] *= 1./CurrentEigenvalues[index];
      //CurrentEigenvalues[index] /= (double)count;
    }

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

    // orthonormalize
    if (!dontOrthonormalization) {
      DoLog(1) && (Log() << Verbose(1) << "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);

    // put obtained eigenvectors into full space
    FullSpace.setEigenpairs(CurrentEigenvectors, CurrentEigenvalues);

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

    // and next iteration
    DoLog(0) && (Log() << Verbose(0) << "\titeration #" << run << std::endl);
    run++;
  }
  DoLog(0) && (Log() << Verbose(0) << "Solving took " << Time_solving.elapsed() << " seconds." << std::endl);
  // show final ones
  DoLog(0) && (Log() << Verbose(0) << "Resulting new eigenvectors and -values, run " << run << " are:" << std::endl);
  outputvalues << run;
  BOOST_FOREACH( size_t index, AllIndices) {
    DoLog(0) && (Log() << Verbose(0) << *CurrentEigenvectors[index] << " with " << CurrentEigenvalues[index] << std::endl);
    outputvalues << "\t" << CurrentEigenvalues[index];
  }
  outputvalues << std::endl;
  outputvalues.close();

  setVerbosity(2);

  DoLog(0) && (Log() << Verbose(0) << "Solving full space ..." << std::endl);
  boost::timer Time_comparison;
  MatrixContent tempFullspaceMatrix = FullSpace.getEigenspaceMatrix();
  gsl_vector *eigenvalues = tempFullspaceMatrix.transformToEigenbasis();
  tempFullspaceMatrix.sortEigenbasis(eigenvalues);
  DoLog(0) && (Log() << Verbose(0) << "full space solution took " << Time_comparison.elapsed() << " seconds." << std::endl);

  // compare all
  sort(CurrentEigenvalues.begin(),CurrentEigenvalues.end()); //, cmd);
  for (size_t i=0;i<eigenvalues->size; ++i) {
    CPPUNIT_ASSERT_MESSAGE(toString(i)+"ths eigenvalue differs:"
        +toString(CurrentEigenvalues[i])+" != "+toString(gsl_vector_get(eigenvalues,i)),
        fabs((CurrentEigenvalues[i] - gsl_vector_get(eigenvalues,i))/CurrentEigenvalues[i]) < 1e-3);
  }

  CPPUNIT_ASSERT_EQUAL(0,0);
}