//
// Software License for AMDiS
//
// Copyright (c) 2010 Dresden University of Technology 
// All rights reserved.
// Authors: Simon Vey, Thomas Witkowski et al.
//
// This file is part of AMDiS
//
// See also license.opensource.txt in the distribution.


#include <list>
#include <algorithm>
#include <math.h>

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/category.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>

#include "FixVec.h"
#include "Boundary.h"
#include "DOFAdmin.h"
#include "ElInfo.h"
#include "Error.h"
#include "FiniteElemSpace.h"
#include "Global.h"
#include "Mesh.h"
#include "Quadrature.h"
#include "AbstractFunction.h"
#include "BoundaryManager.h"
#include "Assembler.h"
#include "OpenMP.h"
#include "Operator.h"
#include "Parameters.h"
#include "Traverse.h"

// Defining the interface for MTL4
namespace mtl {

  // Let MTL4 know that DOFVector it is a column vector
  namespace traits {
    template <typename T>
    struct category< AMDiS::DOFVector<T> > 
    {
      typedef tag::dense_col_vector type;
    };
  }

  namespace ashape {
    template <typename T>
    struct ashape< AMDiS::DOFVector<T> > 
    {
      typedef cvec<typename ashape<T>::type> type;
    };
  }

  // Modelling Collection and MutableCollection
  template <typename T>
  struct Collection< AMDiS::DOFVector<T> > 
  {
    typedef T               value_type;
    typedef const T&        const_reference;
    typedef std::size_t     size_type;
  };

  template <typename T>
  struct MutableCollection< AMDiS::DOFVector<T> > 
    : public Collection< AMDiS::DOFVector<T> > 
  {
    typedef T&              reference;
  };


} // namespace mtl



namespace AMDiS {

  template<typename T>
  DOFVectorBase<T>::DOFVectorBase(const FiniteElemSpace *f, std::string n)
    : feSpace(f),
      name(n),
      elementVector(f->getBasisFcts()->getNumber()),
      boundaryManager(NULL)
  {    
    nBasFcts = feSpace->getBasisFcts()->getNumber();
    dim = feSpace->getMesh()->getDim();

    createTempData();
  }
  

  template<typename T>
  DOFVectorBase<T>::~DOFVectorBase()
  {
    for (int i = 0; i < static_cast<int>(localIndices.size()); i++) {
      delete [] localIndices[i];
      delete grdPhis[i];
      delete grdTmp[i];
      delete D2Phis[i];
    }
  }


  template<typename T>
  void DOFVectorBase<T>::createTempData()
  {
    localIndices.resize(omp_get_overall_max_threads());
    localVectors.resize(omp_get_overall_max_threads());
    grdPhis.resize(omp_get_overall_max_threads());
    grdTmp.resize(omp_get_overall_max_threads());
    D2Phis.resize(omp_get_overall_max_threads());

    for (int i = 0; i < omp_get_overall_max_threads(); i++) {
      localIndices[i] = new DegreeOfFreedom[this->nBasFcts];
      localVectors[i].change_dim(this->nBasFcts);
      grdPhis[i] = new DimVec<double>(dim, DEFAULT_VALUE, 0.0);
      grdTmp[i] = new DimVec<double>(dim, DEFAULT_VALUE, 0.0);
      D2Phis[i] = new DimMat<double>(dim, NO_INIT);
    }    
  }

  
  template<typename T>
  DOFVector<T>::DOFVector(const FiniteElemSpace* f, std::string n)
    : DOFVectorBase<T>(f, n),
      coarsenOperation(NO_OPERATION)
  {
    vec.resize(0);
    init(f, n);
  } 


  template<typename T>
  void DOFVector<T>::init(const FiniteElemSpace* f, std::string n)
  {
    this->name = n;
    this->feSpace = f;
    if (this->feSpace && this->feSpace->getAdmin())
      (this->feSpace->getAdmin())->addDOFIndexed(this);
    this->boundaryManager = new BoundaryManager(f);
  }

  template<typename T>
  DOFVector<T>::~DOFVector()
  {
    if (this->feSpace && this->feSpace->getAdmin())
      (this->feSpace->getAdmin())->removeDOFIndexed(this);

    if (this->boundaryManager)
      delete this->boundaryManager;

    vec.clear();
  }

  template<typename T>
  DOFVector<T> * DOFVector<T>::traverseVector = NULL;

  template<typename T>
  void DOFVectorBase<T>::addElementVector(T factor, 
					  const ElementVector &elVec, 
					  const BoundaryType *bound,
					  ElInfo *elInfo,
					  bool add)
  {
    FUNCNAME("DOFVector::addElementVector()");

    std::vector<DegreeOfFreedom> indices(nBasFcts);
    feSpace->getBasisFcts()->getLocalIndices(elInfo->getElement(), feSpace->getAdmin(),
					     indices);

    for (DegreeOfFreedom i = 0; i < nBasFcts; i++) {
      BoundaryCondition *condition = 
	bound ? this->getBoundaryManager()->getBoundaryCondition(bound[i]) : NULL;

      if (!(condition && condition->isDirichlet())) {
	DegreeOfFreedom irow = indices[i];

	if (add)
	  (*this)[irow] += factor * elVec[i];
	else  
	  (*this)[irow] = factor * elVec[i];	
      }
    }
  }

  template<typename T>
  double DOFVector<T>::nrm2() const
  {
    FUNCNAME("DOFVector<T>::nrm2()");

    checkFeSpace(this->feSpace, vec);

    double nrm = 0.0;
    Iterator vecIterator(dynamic_cast<DOFIndexed<T>*>(const_cast<DOFVector<T>*>(this)), 
			 USED_DOFS);
    for (vecIterator.reset(); !vecIterator.end(); ++vecIterator)
      nrm += (*vecIterator) * (*vecIterator);

    return(sqrt(nrm));
  }

  template<typename T>
  double DOFVector<T>::squareNrm2() const
  {
    FUNCNAME("DOFVector<T>::nrm2()");

    checkFeSpace(this->feSpace, vec);
    
    double nrm = 0.0;
    Iterator vecIterator(dynamic_cast<DOFIndexed<T>*>(const_cast<DOFVector<T>*>(this)), 
			 USED_DOFS);
    for (vecIterator.reset(); !vecIterator.end(); ++vecIterator)
      nrm += (*vecIterator) * (*vecIterator);

    return nrm;
  }

  template<typename T>
  T DOFVector<T>::asum() const
  {
    FUNCNAME("DOFVector<T>::asum()");

    checkFeSpace(this->feSpace, vec);

    double nrm = 0.0;
    Iterator vecIterator(dynamic_cast<DOFIndexed<T>*>(const_cast<DOFVector<T>*>(this)), 
			 USED_DOFS);
    for (vecIterator.reset(); !vecIterator.end(); ++vecIterator)
      nrm += abs(*vecIterator);

    return(nrm);
  }

  template<typename T>
  T DOFVector<T>::sum() const
  {
    FUNCNAME("DOFVector<T>::sum()");

    checkFeSpace(this->feSpace, vec);

    double nrm = 0.0;    
    Iterator vecIterator(dynamic_cast<DOFIndexed<T>*>(const_cast<DOFVector<T>*>(this)), 
			 USED_DOFS);
    for (vecIterator.reset(); !vecIterator.end(); ++vecIterator)
      nrm += *vecIterator;

    return(nrm);
  }

  template<typename T>
  void DOFVector<T>::set(T alpha)
  {
    FUNCNAME("DOFVector<T>::set()");

    checkFeSpace(this->feSpace, vec);

    Iterator vecIterator(dynamic_cast<DOFIndexed<T>*>(this), USED_DOFS);
    for (vecIterator.reset(); !vecIterator.end(); ++vecIterator)
      *vecIterator = alpha ; 
  }


  template<typename T>
  void DOFVector<T>::copy(const DOFVector<T>& x)
  {
    FUNCNAME("DOFVector<T>::copy()");

    checkFeSpace(this->feSpace, vec);

    TEST_EXIT_DBG(static_cast<int>(x.vec.size()) >= 
		  this->feSpace->getAdmin()->getUsedSize())
      ("x.size = %d too small: admin->sizeUsed = %d\n", x.vec.size(), 
       this->feSpace->getAdmin()->getUsedSize());
    
    Iterator vecIterator(dynamic_cast<DOFIndexed<T>*>(this), USED_DOFS);
    Iterator xIterator(dynamic_cast<DOFVector<T>*>(const_cast<DOFVector<T>*>(&x)), 
		       USED_DOFS);
    for (vecIterator.reset(), xIterator.reset(); !vecIterator.end();
	 ++vecIterator, ++xIterator)      
      *vecIterator = *xIterator; 
  }


  template<typename T>
  T DOFVector<T>::min() const
  {
    FUNCNAME("DOFVector<T>::min()");
    
    checkFeSpace(this->feSpace, vec);

    T m;    
    Iterator vecIterator(const_cast<DOFIndexed<T>*>(dynamic_cast<const DOFIndexed<T>*>(this)), USED_DOFS);
    for (vecIterator.reset(), m = *vecIterator; !vecIterator.end(); ++vecIterator)
      m = std::min(m, *vecIterator);

    return m;
  }


  template<typename T>
  T DOFVector<T>::max() const 
  {
    FUNCNAME("DOFVector<T>::max()");
    
    checkFeSpace(this->feSpace, vec);

    T m;    
    Iterator vecIterator(const_cast<DOFIndexed<T>*>(dynamic_cast<const DOFIndexed<T>*>(this)), USED_DOFS);
    for (vecIterator.reset(), m = *vecIterator; !vecIterator.end(); ++vecIterator)
      m = std::max(m, *vecIterator);

    return m;

  }

  template<typename T>
  T DOFVector<T>::absMax() const
  {
    return std::max(abs(max()), abs(min()));
  }


  template<typename T>
  T DOFVector<T>::average() const
  {
    FUNCNAME("DOFVector<T>::average()");
    
    checkFeSpace(this->feSpace, vec);

    int count = 0;
    T m = 0;
    Iterator vecIterator(const_cast<DOFIndexed<T>*>(dynamic_cast<const DOFIndexed<T>*>(this)), USED_DOFS);
    for (vecIterator.reset(); !vecIterator.end(); ++vecIterator) {
      m += *vecIterator;
      count++;
    }

    return m / count;
  }


  template<typename T>
  void DOFVector<T>::print() const
  {
    FUNCNAME("DOFVector<T>::print()");

    const DOFAdmin *admin = NULL;
    const char *format;

    if (this->feSpace) 
      admin = this->feSpace->getAdmin();

    MSG("Vec `%s':\n", this->name.c_str());
    int j = 0;
    if (admin) {
      if (admin->getUsedSize() > 100)
	format = "%s(%3d,%10.5le)";
      else if (admin->getUsedSize() > 10)
	format = "%s(%2d,%10.5le)";
      else
	format = "%s(%1d,%10.5le)";

      Iterator vecIterator(dynamic_cast<DOFIndexed<T>*>(const_cast<DOFVector<T>*>(this)), USED_DOFS);
      for (vecIterator.reset(); !vecIterator.end(); ++vecIterator) {
	if ((j % 3) == 0) {
	  if (j) 
	    Msg::print("\n");
	  MSG(format, "", vecIterator.getDOFIndex(), *vecIterator);
	} else {
	  Msg::print(format, " ", vecIterator.getDOFIndex(), *vecIterator);
	}
	j++;
      }
      Msg::print("\n");
    } else {
	MSG("no DOFAdmin, print whole vector.\n");
    
	for (int i = 0; i < static_cast<int>( vec.size()); i++) {
	  if ((j % 3) == 0) {
	    if (j) 
	      Msg::print("\n");
	    MSG("(%d,%10.5e)",i,vec[i]);
	  } else {
	    Msg::print(" (%d,%10.5e)",i,vec[i]);
	  }
	  j++;
	}
	Msg::print("\n");
      }
    return;
  }

  template<typename T>
  int DOFVector<T>::calcMemoryUsage() const
  {
    int result = 0;
    result += sizeof(DOFVector<T>);
    result += vec.size() * sizeof(T);

    return result;
  }

  template<typename T>
  T DOFVectorBase<T>::evalUh(const DimVec<double>& lambda, 
			     DegreeOfFreedom* dof_indices)
  {
    BasisFunction* phi = const_cast<BasisFunction*>(this->getFeSpace()->getBasisFcts());
    int nBasisFcts = phi->getNumber();
    T val = 0.0;

    for (int i = 0; i < nBasisFcts; i++)
      val += (*this)[dof_indices[i]]*(*phi->getPhi(i))(lambda);

    return val;
  }

  template<typename T>
  void DOFVector<T>::interpol(AbstractFunction<T, WorldVector<double> > *fct)
  {
    FUNCNAME("DOFVector::interpol()");

    TEST_EXIT_DBG(fct)("No function to interpolate!\n");

    interFct = fct;

    if (!this->getFeSpace()) {
      MSG("no dof admin in vec %s, skipping interpolation\n",
	  this->getName().c_str());
      return;
    }

    if (!(this->getFeSpace()->getAdmin())) {
      MSG("no dof admin in feSpace %s, skipping interpolation\n", 
	  this->getFeSpace()->getName().c_str());
      return;
    }
  
    if (!(this->getFeSpace()->getBasisFcts())) {
      MSG("no basis functions in admin of vec %s, skipping interpolation\n",
	  this->getName().c_str());
      return;
    }

    if (!(fct)) {
      MSG("function that should be interpolated only pointer to NULL, ");
      Msg::print("skipping interpolation\n");
      return;
    }

    traverseVector = this;

    TraverseStack stack;
    ElInfo *elInfo = stack.traverseFirst(this->getFeSpace()->getMesh(), -1,
					 Mesh::CALL_LEAF_EL | Mesh::FILL_COORDS);
    while (elInfo) {
      interpolFct(elInfo);
      elInfo = stack.traverseNext(elInfo);
    }
  }

  template<typename T>
  void DOFVector<T>::interpolFct(ElInfo* elinfo)
  {
    const BasisFunction *basFct = traverseVector->getFeSpace()->getBasisFcts();
    const DOFAdmin* admin = traverseVector->getFeSpace()->getAdmin();
    const T *inter_val = 
      const_cast<BasisFunction*>(basFct)->interpol(elinfo, 0, NULL,
						   traverseVector->interFct, NULL);

    DegreeOfFreedom *myLocalIndices = this->localIndices[omp_get_thread_num()];
    basFct->getLocalIndices(const_cast<Element*>(elinfo->getElement()), admin, myLocalIndices);
    int nBasFcts = basFct->getNumber();
    for (int i = 0; i < nBasFcts; i++)
      (*traverseVector)[myLocalIndices[i]] = inter_val[i];
  }

  template<typename T>
  double DOFVector<T>::Int(Quadrature* q) const
  {
    FUNCNAME("DOFVector::Int()");
  
    Mesh* mesh = this->feSpace->getMesh();

    if (!q) {
      int deg = 2 * this->feSpace->getBasisFcts()->getDegree();
      q = Quadrature::provideQuadrature(this->dim, deg);
    }

    FastQuadrature *quadFast = 
      FastQuadrature::provideFastQuadrature(this->feSpace->getBasisFcts(), *q, INIT_PHI);

    double result = 0.0;
    int nPoints = quadFast->getNumPoints();
    mtl::dense_vector<T> uh_vec(nPoints);
    TraverseStack stack;
    ElInfo *elInfo = 
      stack.traverseFirst(mesh, -1, 
			  Mesh::CALL_LEAF_EL | Mesh::FILL_COORDS | Mesh::FILL_DET);
    while (elInfo) {
      double det = elInfo->getDet();
      double normT = 0.0;
      this->getVecAtQPs(elInfo, NULL, quadFast, uh_vec);
      for (int iq = 0; iq < nPoints; iq++)
	normT += quadFast->getWeight(iq) * (uh_vec[iq]);
      result += det * normT;

      elInfo = stack.traverseNext(elInfo);
    }

    return result;  
  }


  template<typename T>
  double DOFVector<T>::L1Norm(Quadrature* q) const
  {
    FUNCNAME("DOFVector::L1Norm()");
  
    Mesh* mesh = this->feSpace->getMesh();

    if (!q) {
      int deg = 2 * this->feSpace->getBasisFcts()->getDegree();
      q = Quadrature::provideQuadrature(this->dim, deg);
    }

    FastQuadrature *quadFast = 
      FastQuadrature::provideFastQuadrature(this->feSpace->getBasisFcts(), *q, INIT_PHI);

    double result = 0.0;
    int nPoints = quadFast->getNumPoints();
    mtl::dense_vector<T> uh_vec(nPoints);
    TraverseStack stack;
    ElInfo *elInfo = 
      stack.traverseFirst(mesh, -1, 
			  Mesh::CALL_LEAF_EL | Mesh::FILL_COORDS | Mesh::FILL_DET);
    while (elInfo) {
      double det = elInfo->getDet();
      double normT = 0.0;
      this->getVecAtQPs(elInfo, NULL, quadFast, uh_vec);
      for (int iq = 0; iq < nPoints; iq++)
	normT += quadFast->getWeight(iq) * abs(uh_vec[iq]);
      result += det * normT;

      elInfo = stack.traverseNext(elInfo);
    }

    return result;  
  }


  template<typename T>
  double DOFVector<T>::L2NormSquare(Quadrature* q) const
  {
    FUNCNAME("DOFVector::L2NormSquare()");

    Mesh* mesh = this->feSpace->getMesh();

    if (!q) {
      int deg = 2 * this->feSpace->getBasisFcts()->getDegree();
      q = Quadrature::provideQuadrature(this->dim, deg);
    }

    FastQuadrature *quadFast = 
      FastQuadrature::provideFastQuadrature(this->feSpace->getBasisFcts(), *q, INIT_PHI);

    double result = 0.0;
    int nPoints = quadFast->getNumPoints();
    mtl::dense_vector<T> uh_vec(nPoints);
    TraverseStack stack;
    ElInfo *elInfo = 
      stack.traverseFirst(mesh, -1, 
			  Mesh::CALL_LEAF_EL | Mesh::FILL_COORDS | Mesh::FILL_DET);
    while (elInfo) {
      double det = elInfo->getDet();
      double normT = 0.0;
      this->getVecAtQPs(elInfo, NULL, quadFast, uh_vec);
      for (int iq = 0; iq < nPoints; iq++)
	normT += quadFast->getWeight(iq) * sqr(uh_vec[iq]);
      result += det * normT;

      elInfo = stack.traverseNext(elInfo);
    }

    return result;  
  }


  template<typename T>  
  double DOFVector<T>::H1NormSquare(Quadrature *q) const
  {
    FUNCNAME("DOFVector::H1NormSquare()");

    Mesh* mesh = this->feSpace->getMesh();

    if (!q) {
      int deg = 2 * this->feSpace->getBasisFcts()->getDegree() - 2;
      q = Quadrature::provideQuadrature(this->dim, deg);
    }

    FastQuadrature *quadFast = 
      FastQuadrature::provideFastQuadrature(this->feSpace->getBasisFcts(), *q, INIT_GRD_PHI);

    double result = 0.0;
    int nPoints = quadFast->getNumPoints();
    int dimOfWorld = Global::getGeo(WORLD);
    std::vector<WorldVector<T> > grduh_vec(nPoints);
    TraverseStack stack;
    ElInfo *elInfo = 
      stack.traverseFirst(mesh, -1, 
			  Mesh::CALL_LEAF_EL | Mesh::FILL_COORDS | 
			  Mesh::FILL_DET | Mesh::FILL_GRD_LAMBDA);
    while (elInfo) {
      double det = elInfo->getDet();
      double normT = 0.0;
      this->getGrdAtQPs(elInfo, NULL, quadFast, &(grduh_vec[0]));

      for (int iq = 0; iq < nPoints; iq++) {
	double norm2 = 0.0;
	for (int j = 0; j < dimOfWorld; j++)
	  norm2 += sqr(grduh_vec[iq][j]);
	normT += quadFast->getWeight(iq) * norm2;
      }

      result += det * normT;

      elInfo = stack.traverseNext(elInfo);
    }

    return result;  
  }

  template<typename T>
  void DOFVector<T>::compressDOFIndexed(int first, int last, 
					std::vector<DegreeOfFreedom> &newDOF)
  {
    for (int i = first; i <= last; i++)
      if (newDOF[i] >= 0)
	vec[newDOF[i]] = vec[i];
  }

  template<typename T>
  Flag DOFVectorBase<T>::getAssembleFlag()
  {
    Flag fillFlag(0);
    
    for (std::vector<Operator*>::iterator op = this->operators.begin(); 
	 op != this->operators.end(); ++op)
      fillFlag |= (*op)->getFillFlag();

    return fillFlag;
  }

  template<typename T>
  void DOFVectorBase<T>::finishAssembling()
  {
    // call the operatos cleanup procedures
    for (std::vector<Operator*>::iterator it = this->operators.begin();
	 it != this->operators.end(); ++it)
      (*it)->finishAssembling();
  }

  template<typename T>
  DOFVector<T>& DOFVector<T>::operator=(const DOFVector<T>& rhs)
  {
    this->feSpace = rhs.feSpace;
    this->dim = rhs.dim;
    this->nBasFcts = rhs.nBasFcts;
    vec = rhs.vec;
    this->elementVector.change_dim(this->nBasFcts);
    interFct = rhs.interFct;
    coarsenOperation = rhs.coarsenOperation;
    this->operators = rhs.operators;
    this->operatorFactor = rhs.operatorFactor;
    
    if (rhs.boundaryManager) {
      if (this->boundaryManager) 
	delete this->boundaryManager; 

      this->boundaryManager = new BoundaryManager(*rhs.boundaryManager);
    } else {
      this->boundaryManager = NULL;
    }

#ifdef HAVE_PARALLEL_DOMAIN_AMDIS
    this->rankDofs = rhs.rankDofs;
#endif

    return *this;
  }

  template<typename T>
  const DOFVector<T>& operator*=(DOFVector<T>& x, T scal)
  {
    FUNCNAME("DOFVector<T>::operator*=(DOFVector<T>& x, T scal)");

    TEST_EXIT_DBG(x.getFeSpace() && x.getFeSpace()->getAdmin())
      ("pointer is NULL: %8X, %8X\n", x.getFeSpace(), x.getFeSpace()->getAdmin());

    typename DOFVector<T>::Iterator vecIterator(dynamic_cast<DOFIndexed<T>*>(&x), 
						USED_DOFS);
    for (vecIterator.reset(); !vecIterator.end(); ++vecIterator)
      (*vecIterator) *= scal; 

    return x;
  }


  template<typename T>
  const DOFVector<T>& operator+=(DOFVector<T>& x, const DOFVector<T>& y)
  {
    FUNCNAME("DOFVector<T>::operator+=(DOFVector<T>& x, const DOFVector<T>& y)");
    
    TEST_EXIT_DBG(x.getFeSpace() && y.getFeSpace())
      ("feSpace is NULL: %8X, %8X\n", x.getFeSpace(), y.getFeSpace());
    TEST_EXIT_DBG(x.getFeSpace()->getAdmin() &&
	      (x.getFeSpace()->getAdmin() == y.getFeSpace()->getAdmin()))
      ("no admin or different admins: %8X, %8X\n",
       x.getFeSpace()->getAdmin(), y.getFeSpace()->getAdmin());
    TEST_EXIT_DBG(x.getSize() == y.getSize())("different sizes\n");
    
    typename DOFVector<T>::Iterator xIterator(dynamic_cast<DOFIndexed<T>*>(&x), USED_DOFS);
    typename DOFVector<T>::Iterator yIterator(dynamic_cast<DOFIndexed<T>*>(const_cast<DOFVector<T>*>(&y)), USED_DOFS);
    for (xIterator.reset(), yIterator.reset(); !xIterator.end();
	 ++xIterator, ++yIterator)
      *xIterator += *yIterator; 

    return x;
  }

  template<typename T>
  const DOFVector<T>& operator-=(DOFVector<T>& x, const DOFVector<T>& y)
  {
    FUNCNAME("DOFVector<T>::operator-=(DOFVector<T>& x, const DOFVector<T>& y)");

    TEST_EXIT_DBG(x.getFeSpace() && y.getFeSpace())
      ("feSpace is NULL: %8X, %8X\n", x.getFeSpace(), y.getFeSpace());
    TEST_EXIT_DBG(x.getFeSpace()->getAdmin() &&
	      (x.getFeSpace()->getAdmin() == y.getFeSpace()->getAdmin()))
      ("no admin or different admins: %8X, %8X\n",
       x.getFeSpace()->getAdmin(), y.getFeSpace()->getAdmin());
    TEST_EXIT_DBG(x.getSize() == y.getSize())("different sizes\n");
    
    typename DOFVector<T>::Iterator xIterator(dynamic_cast<DOFIndexed<T>*>(&x), USED_DOFS);
    typename DOFVector<T>::Iterator yIterator(dynamic_cast<DOFIndexed<T>*>(const_cast<DOFVector<T>*>(&y)), USED_DOFS);
    for (xIterator.reset(), yIterator.reset(); !xIterator.end();
	 ++xIterator, ++yIterator)
      *xIterator -= *yIterator; 

    return x;
  }

  template<typename T>
  const DOFVector<T>& operator*=(DOFVector<T>& x, const DOFVector<T>& y)
  {
    FUNCNAME("DOFVector<T>::operator*=(DOFVector<T>& x, const DOFVector<T>& y)");
    
    TEST_EXIT_DBG(x.getFeSpace() && y.getFeSpace())
      ("feSpace is NULL: %8X, %8X\n", x.getFeSpace(), y.getFeSpace());
    TEST_EXIT_DBG(x.getFeSpace()->getAdmin() &&
	      (x.getFeSpace()->getAdmin() == y.getFeSpace()->getAdmin()))
      ("no admin or different admins: %8X, %8X\n",
       x.getFeSpace()->getAdmin(), y.getFeSpace()->getAdmin());
    TEST_EXIT_DBG(x.getSize() == y.getSize())("different sizes\n");
    
    typename DOFVector<T>::Iterator xIterator(dynamic_cast<DOFIndexed<T>*>(&x), USED_DOFS);
    typename DOFVector<T>::Iterator yIterator(dynamic_cast<DOFIndexed<T>*>(const_cast<DOFVector<T>*>(&y)), USED_DOFS);
    for (xIterator.reset(), yIterator.reset(); !xIterator.end();
	 ++xIterator, ++yIterator)
      *xIterator *= *yIterator; 

    return x;
  }

  template<typename T>
  T operator*(DOFVector<T>& x, DOFVector<T>& y)
  {
    FUNCNAME("DOFVector<T>::operator*(DOFVector<T>& x, DOFVector<T>& y)");
    const DOFAdmin *admin = NULL;

    TEST_EXIT(x.getFeSpace() && y.getFeSpace())
      ("feSpace is NULL: %8X, %8X\n", x.getFeSpace(), y.getFeSpace());
    TEST_EXIT((admin = x.getFeSpace()->getAdmin()) && (admin == y.getFeSpace()->getAdmin()))
      ("no admin or different admins: %8X, %8X\n",
       x.getFeSpace()->getAdmin(), y.getFeSpace()->getAdmin());
    TEST_EXIT(x.getSize() == y.getSize())("different sizes\n");

    T dot = 0;

    typename DOFVector<T>::Iterator xIterator(dynamic_cast<DOFIndexed<T>*>(&x), USED_DOFS);
    typename DOFVector<T>::Iterator yIterator(dynamic_cast<DOFIndexed<T>*>(&y), USED_DOFS);
    for (xIterator.reset(), yIterator.reset(); !xIterator.end();
	 ++xIterator, ++yIterator) 
      dot += (*xIterator) * (*yIterator);

    return dot;
  }

  template<typename T>
  void mv(MatrixTranspose transpose, const DOFMatrix &a, const DOFVector<T>&x,
	  DOFVector<T> &result, bool add)
  {
    // Unfortunately needed
    using namespace mtl;

    FUNCNAME("DOFVector<T>::mv()");

    TEST_EXIT(a.getRowFeSpace() && a.getColFeSpace() && 
	      x.getFeSpace() && result.getFeSpace())
      ("getFeSpace() is NULL: %8X, %8X, %8X, %8X\n", 
       a.getRowFeSpace(), a.getColFeSpace(), x.getFeSpace(), result.getFeSpace());

    const DOFAdmin *rowAdmin = a.getRowFeSpace()->getAdmin();
    const DOFAdmin *colAdmin = a.getColFeSpace()->getAdmin();

    TEST_EXIT((rowAdmin && colAdmin) && 
	      (((transpose == NoTranspose) && (colAdmin == x.getFeSpace()->getAdmin()) && 
		(rowAdmin == result.getFeSpace()->getAdmin()))||
	       ((transpose == Transpose) && (rowAdmin == x.getFeSpace()->getAdmin()) && 
		(colAdmin == result.getFeSpace()->getAdmin()))))
      ("no admin or different admins: %8X, %8X, %8X, %8X\n",
       a.getRowFeSpace()->getAdmin(), a.getColFeSpace()->getAdmin(), 
       x.getFeSpace()->getAdmin(), result.getFeSpace()->getAdmin());


    if (transpose == NoTranspose)
      mult(a.getBaseMatrix(), x, result);
    else if (transpose == Transpose)
      mult(trans(const_cast<DOFMatrix::base_matrix_type&>(a.getBaseMatrix())), x, result);
    else 
      ERROR_EXIT("transpose = %d\n", transpose);
  }

  template<typename T>
  void axpy(double alpha, const DOFVector<T>& x, DOFVector<T>& y)
  {
    FUNCNAME("DOFVector<T>::axpy()");

    TEST_EXIT(x.getFeSpace() && y.getFeSpace())
      ("feSpace is NULL: %8X, %8X\n", x.getFeSpace(), y.getFeSpace());

    const DOFAdmin *admin = x.getFeSpace()->getAdmin();

    TEST_EXIT((admin) && (admin == y.getFeSpace()->getAdmin()))
      ("no admin or different admins: %8X, %8X\n",
       x.getFeSpace()->getAdmin(), y.getFeSpace()->getAdmin());
    TEST_EXIT(static_cast<int>(x.getSize()) >= admin->getUsedSize())
      ("size = %d too small: admin->size = %d\n", x.getSize(), 
       admin->getUsedSize());
    TEST_EXIT(static_cast<int>(y.getSize()) >= admin->getUsedSize())
      ("y.size = %d too small: admin->size = %d\n", y.getSize(), 
       admin->getUsedSize());

      // This is the old implementation of the mv-multiplication. It have been changed
      // because of the OpenMP-parallelization:
//     typename DOFVector<T>::Iterator xIterator(dynamic_cast<DOFIndexed<T>*>(const_cast<DOFVector<T>*>(&x)), USED_DOFS);
//     typename DOFVector<T>::Iterator yIterator(dynamic_cast<DOFIndexed<T>*>(&y), USED_DOFS);
//     for(xIterator.reset(), yIterator.reset();
// 	!xIterator.end();
// 	++xIterator, ++yIterator) 
//       {  
// 	*yIterator += alpha * (*xIterator); 
//       };

      int i;
      int maxI = y.getSize();

#ifdef _OPENMP
#pragma omp parallel for schedule(dynamic, 25000) default(shared) private(i)
#endif
      for (i = 0; i < maxI; i++)
	if (!admin->isDofFree(i))
	  y[i] += alpha * x[i];
  }

  template<typename T>
  const DOFVector<T>& operator*(const DOFVector<T>& v, double d)
  {
    static DOFVector<T> result;
    return mult(d, v, result);
  }

  template<typename T>
  const DOFVector<T>& operator*(double d, const DOFVector<T>& v)
  {
    static DOFVector<T> result;
    return mult(d, v, result);
  }

  template<typename T>
  const DOFVector<T>& operator+(const DOFVector<T> &v1 , const DOFVector<T> &v2)
  {
    static DOFVector<T> result;
    return add(v1, v2, result);
  }


  template<typename T>
  void xpay(double alpha, 
	    const DOFVector<T>& x, 
	    DOFVector<T>& y)
  {
    FUNCNAME("DOFVector<T>::xpay()");

    TEST_EXIT(x.getFeSpace() && y.getFeSpace())
      ("feSpace is NULL: %8X, %8X\n", x.getFeSpace(), y.getFeSpace());

    const DOFAdmin *admin = x.getFeSpace()->getAdmin();

    TEST_EXIT(admin && (admin == y.getFeSpace()->getAdmin()))
      ("no admin or different admins: %8X, %8X\n",
       x.getFeSpace()->getAdmin(), y.getFeSpace()->getAdmin());
    TEST_EXIT(static_cast<int>(x.getSize()) >= admin->getUsedSize())
      ("size = %d too small: admin->size = %d\n", x.getSize(), 
       admin->getUsedSize());
    TEST_EXIT(static_cast<int>(y.getSize()) >= admin->getUsedSize())
      ("y.size = %d too small: admin->size = %d\n", y.getSize(), 
       admin->getUsedSize());

    // This is the old implementation of the mv-multiplication. It have been changed
    // because of the OpenMP-parallelization:
    //     typename DOFVector<T>::Iterator xIterator(dynamic_cast<DOFIndexed<T>*>(const_cast<DOFVector<T>*>(&x)), USED_DOFS);
    //     typename DOFVector<T>::Iterator yIterator(dynamic_cast<DOFIndexed<T>*>(&y), USED_DOFS);
    //     for(xIterator.reset(), yIterator.reset();
    // 	!xIterator.end();
    // 	++xIterator, ++yIterator) 
    //       {  
    // 	*yIterator = alpha *(*yIterator)+ (*xIterator); 
    //       };

      int i;
      int maxI = y.getSize();

#ifdef _OPENMP
#pragma omp parallel for schedule(dynamic, 25000) default(shared) private(i)
#endif
      for (i = 0; i < maxI; i++)
	if (!admin->isDofFree(i))
	  y[i] = alpha * y[i] + x[i];
  }

  template<typename T>
  inline const DOFVector<T>& mult(double scal,
				  const DOFVector<T>& v,
				  DOFVector<T>& result)
  {
    typename DOFVector<T>::Iterator vIterator(dynamic_cast<DOFIndexed<T>*>(const_cast<DOFVector<T>*>(&v)), USED_DOFS);
    typename DOFVector<T>::Iterator rIterator(dynamic_cast<DOFIndexed<T>*>(&result), USED_DOFS);
    for (vIterator.reset(), rIterator.reset();
	 !vIterator.end(); ++vIterator, ++rIterator) 
      *rIterator = scal * (*vIterator);

    return result;
  }

  template<typename T>
  inline const DOFVector<T>& add(const DOFVector<T>& v,
				 double scal,
				 DOFVector<T>& result)
  {
    typename DOFVector<T>::Iterator vIterator(dynamic_cast<DOFIndexed<T>*>(const_cast<DOFVector<T>*>(&v)), USED_DOFS);
    typename DOFVector<T>::Iterator rIterator(dynamic_cast<DOFIndexed<T>*>(&result), USED_DOFS);
    for (vIterator.reset(), rIterator.reset();
	!vIterator.end(); ++vIterator, ++rIterator) 
      *rIterator = (*vIterator) + scal;

    return result;
  }


  template<typename T>
  inline const DOFVector<T>& add(const DOFVector<T>& v1, 
				 const DOFVector<T>& v2,
				 DOFVector<T>& result)
  {
    typename DOFVector<T>::Iterator v1Iterator(dynamic_cast<DOFIndexed<T>*>(const_cast<DOFVector<T>*>(&v1)), USED_DOFS);
    typename DOFVector<T>::Iterator v2Iterator(dynamic_cast<DOFIndexed<T>*>(const_cast<DOFVector<T>*>(&v2)), USED_DOFS);
    typename DOFVector<T>::Iterator rIterator(dynamic_cast<DOFIndexed<T>*>(&result), USED_DOFS);
    for (v1Iterator.reset(), v2Iterator.reset(), rIterator.reset();
	 !v1Iterator.end(); ++v1Iterator, ++v2Iterator, ++rIterator) 
      *rIterator = (*v1Iterator) + (*v2Iterator);

    return result;
  }


  template<typename T>
  void DOFVectorBase<T>::getLocalVector(const Element *el, 
					mtl::dense_vector<T>& d) const
  {
    FUNCNAME("DOFVectorBase<T>::getLocalVector()");

    TEST_EXIT_DBG(feSpace->getMesh() == el->getMesh())
      ("Element is defined on a different mesh than the DOF vector!\n");

    const DOFAdmin* admin = feSpace->getAdmin();

#ifdef _OPENMP
    std::vector<DegreeOfFreedom> localIndices(nBasFcts);
    feSpace->getBasisFcts()->getLocalIndices(el, admin, &(localIndices[0]));
#else
    const DegreeOfFreedom *localIndices = 
      feSpace->getBasisFcts()->getLocalIndices(el, admin, NULL);
#endif

    for (int i = 0; i < nBasFcts; i++)
      d[i] = (*this)[localIndices[i]];
  }


  template<typename T>
  void DOFVectorBase<T>::getVecAtQPs(const ElInfo *elInfo, 
				     const Quadrature *quad,
				     const FastQuadrature *quadFast,
				     mtl::dense_vector<T>& vecAtQPs) const
  {
    FUNCNAME("DOFVector<T>::getVecAtQPs()");
  
    TEST_EXIT_DBG(quad || quadFast)("neither quad nor quadFast defined\n");

    if (quad && quadFast) {
      TEST_EXIT_DBG(quad == quadFast->getQuadrature())
	("quad != quadFast->quadrature\n");
    }

    TEST_EXIT_DBG(!quadFast || quadFast->getBasisFunctions() == feSpace->getBasisFcts())
      ("invalid basis functions");

    Element *el = elInfo->getElement();
    const Quadrature *quadrature = quadFast ? quadFast->getQuadrature() : quad;
    const BasisFunction *basFcts = feSpace->getBasisFcts();
    int nPoints = quadrature->getNumPoints();
    int nBasFcts  = basFcts->getNumber();

    vecAtQPs.change_dim(nPoints);

    const mtl::dense_vector<T>& localVec = localVectors[omp_get_thread_num()];
    getLocalVector(el, const_cast<mtl::dense_vector<T>& >(localVec));

    for (int i = 0; i < nPoints; i++) {
      vecAtQPs[i] = 0.0;
      for (int j = 0; j < nBasFcts; j++)
	vecAtQPs[i] += localVec[j] * 
	  (quadFast ? 
	   (quadFast->getPhi(i, j)) : 
	   ((*(basFcts->getPhi(j)))(quad->getLambda(i))));      
    }
  }



  // Some free functions used in MTL4

  template <typename T>
  inline std::size_t size(const AMDiS::DOFVector<T>& v)
  {
    return v.getSize();
  }

  template <typename T>
  inline std::size_t num_rows(const AMDiS::DOFVector<T>& v)
  {
    return v.getSize();
  }


  template <typename T>
  inline std::size_t num_cols(const AMDiS::DOFVector<T>& v)
  {
    return 1;
  }
 

  template <typename T>
  inline void set_to_zero(AMDiS::DOFVector<T>& v)
  {
    using math::zero;
    T  ref, my_zero(zero(ref));

    std::fill(v.begin(), v.end(), my_zero);
  }


  template<typename T>
  double DOFVector<T>::DoubleWell(Quadrature* q) const
  {
    FUNCNAME("DOFVector::DoubleWell()");

    Mesh* mesh = this->feSpace->getMesh();

    if (!q) {
      int deg = 2 * this->feSpace->getBasisFcts()->getDegree();
      q = Quadrature::provideQuadrature(this->dim, deg);
    }

    FastQuadrature *quadFast = 
      FastQuadrature::provideFastQuadrature(this->feSpace->getBasisFcts(), *q, INIT_PHI);

    double result = 0.0;
    int nPoints = quadFast->getNumPoints();
    mtl::dense_vector<T> uh_vec(nPoints);
    TraverseStack stack;
    ElInfo *elInfo = 
      stack.traverseFirst(mesh, -1,
			  Mesh::CALL_LEAF_EL | Mesh::FILL_COORDS | Mesh::FILL_DET);
    while (elInfo) {
      double det = elInfo->getDet();
      double normT = 0.0;
      this->getVecAtQPs(elInfo, NULL, quadFast, uh_vec);
      for (int iq = 0; iq < nPoints; iq++)
	normT += quadFast->getWeight(iq) * sqr(uh_vec[iq]) * sqr(1.0 - uh_vec[iq]);
      result += det * normT;

      elInfo = stack.traverseNext(elInfo);
    }

    return result;    
  }
}