/******************************************************************************
 *
 * AMDiS - Adaptive multidimensional simulations
 *
 * Copyright (C) 2013 Dresden University of Technology. All Rights Reserved.
 * Web: https://fusionforge.zih.tu-dresden.de/projects/amdis
 *
 * Authors:
 * Simon Vey, Thomas Witkowski, Andreas Naumann, Simon Praetorius, et al.
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * This file is part of AMDiS
 *
 * See also license.opensource.txt in the distribution.
 *
 ******************************************************************************/


#include "FirstOrderTerm.h"
#include "DOFVector.h"

using namespace std;

namespace AMDiS {

  // =========== VecAtQP_FOT ==========

  VecAtQP_FOT::VecAtQP_FOT(DOFVectorBase<double> *dv,
			   AbstractFunction<double, double> *af,
			   WorldVector<double> *wv)
    : FirstOrderTerm(af ? af->getDegree() : dv->getFeSpace()->getBasisFcts()->getDegree()), vec(dv), f(af), b(wv)
  {
    TEST_EXIT(dv)("No vector!\n");

    auxFeSpaces.insert(dv->getFeSpace());
  }

  VecAtQP_FOT::VecAtQP_FOT(DOFVectorBase<double> *dv,
			   AbstractFunction<double, double> *af,
			   int bIdx)
    : FirstOrderTerm(af ? af->getDegree() : dv->getFeSpace()->getBasisFcts()->getDegree()), vec(dv), f(af)
  {
    TEST_EXIT(dv)("No vector!\n");

    bOne = bIdx;
    auxFeSpaces.insert(dv->getFeSpace());
  }

  void VecAtQP_FOT::initElement(const ElInfo* elInfo,
				SubAssembler* subAssembler,
				Quadrature *quad)
  {
    getVectorAtQPs(vec, elInfo, subAssembler, quad, vecAtQPs);
  }

  void VecAtQP_FOT::initElement(const ElInfo* smallElInfo,
				const ElInfo* largeElInfo,
				SubAssembler* subAssembler,
				Quadrature *quad)
  {
    getVectorAtQPs(vec, smallElInfo, largeElInfo, subAssembler, quad, vecAtQPs);
  }

  void VecAtQP_FOT::getLb(const ElInfo *elInfo,
			  vector<mtl::dense_vector<double> >& Lb) const
  {
    const DimVec<WorldVector<double> > &grdLambda = elInfo->getGrdLambda();
    const int nPoints = static_cast<int>(Lb.size());

    if (bOne > -1) {
      if (f) {
	for (int iq = 0; iq < nPoints; iq++)
	  lb_one(grdLambda, Lb[iq], (*f)(vecAtQPs[iq]));
      } else {
	for (int iq = 0; iq < nPoints; iq++)
	  lb_one(grdLambda, Lb[iq], vecAtQPs[iq]);
      }
    } else if (b) {
      if (f) {
	for (int iq = 0; iq < nPoints; iq++)
	  lb(grdLambda, *b, Lb[iq], (*f)(vecAtQPs[iq]));
      } else {
	for (int iq = 0; iq < nPoints; iq++)
	  lb(grdLambda, *b, Lb[iq], vecAtQPs[iq]);
      }
    } else {
      if (f) {
	for (int iq = 0; iq < nPoints; iq++)
	  l1(grdLambda, Lb[iq], (*f)(vecAtQPs[iq]));
      } else {
	for (int iq = 0; iq < nPoints; iq++)
	  l1(grdLambda, Lb[iq], vecAtQPs[iq]);
      }
    }
  }

  void VecAtQP_FOT::eval( int nPoints,
			  const mtl::dense_vector<double>& uhAtQP,
			  const mtl::dense_vector<WorldVector<double> >& grdUhAtQP,
			  const mtl::dense_vector<WorldMatrix<double> >& D2UhAtQP,
			  mtl::dense_vector<double>& result,
			  double fac)
  {
    int dow = Global::getGeo(WORLD);

    if (num_rows(grdUhAtQP) > 0) {
      for (int iq = 0; iq < nPoints; iq++) {
	double factor = (f ? (*f)(vecAtQPs[iq]) : vecAtQPs[iq]);
	double resultQP = 0.0;
	for (int i = 0; i < dow; i++)
	  resultQP += grdUhAtQP[iq][i];

	result[iq] += fac * factor * resultQP;
      }
    }
  }


  // =========== CoordsAtQP_FOT ===========

  void CoordsAtQP_FOT::initElement(const ElInfo* elInfo,
				   SubAssembler* subAssembler,
				   Quadrature *quad)
  {
    subAssembler->getCoordsAtQPs(elInfo, quad, coordsAtQPs);
  }

  void CoordsAtQP_FOT::getLb(const ElInfo *elInfo,
			     vector<mtl::dense_vector<double> >& Lb) const
  {
    const DimVec<WorldVector<double> > &grdLambda = elInfo->getGrdLambda();
    const int nPoints = static_cast<int>(Lb.size());

    for (int iq = 0; iq < nPoints; iq++)
      l1(grdLambda, Lb[iq], (*g)(coordsAtQPs[iq]));
  }

  void CoordsAtQP_FOT::eval(int nPoints,
			    const mtl::dense_vector<double>& uhAtQP,
			    const mtl::dense_vector<WorldVector<double> >& grdUhAtQP,
			    const mtl::dense_vector<WorldMatrix<double> >& D2UhAtQP,
			    mtl::dense_vector<double>& result,
			    double f)
  {
    int dow = Global::getGeo(WORLD);

    if (num_rows(grdUhAtQP) > 0) {
      for (int iq = 0; iq < nPoints; iq++) {
	double factor = (*g)(coordsAtQPs[iq]);
	double resultQP = 0.0;
	for (int i = 0; i < dow; i++)
	  resultQP += grdUhAtQP[iq][i];

	result[iq] += f * factor * resultQP;
      }
    }
  }


  // ========== VecCoordsAtQP_FOT ==========

  void VecCoordsAtQP_FOT::initElement(const ElInfo* elInfo,
				      SubAssembler* subAssembler,
				      Quadrature *quad)
  {
    subAssembler->getCoordsAtQPs(elInfo, quad, coordsAtQPs);
  }

  void VecCoordsAtQP_FOT::getLb(const ElInfo *elInfo,
				vector<mtl::dense_vector<double> >& Lb) const
  {
    const DimVec<WorldVector<double> > &grdLambda = elInfo->getGrdLambda();
    const int nPoints = static_cast<int>(Lb.size());

    for (int iq = 0; iq < nPoints; iq++)
      lb(grdLambda, *b, Lb[iq], (*g)(coordsAtQPs[iq]));
  }

  void VecCoordsAtQP_FOT::eval( int nPoints,
				const mtl::dense_vector<double>& uhAtQP,
				const mtl::dense_vector<WorldVector<double> >& grdUhAtQP,
				const mtl::dense_vector<WorldMatrix<double> >& D2UhAtQP,
				mtl::dense_vector<double>& result,
			        double f)
  {
    int dow = Global::getGeo(WORLD);

    if (num_rows(grdUhAtQP) > 0) {
      for (int iq = 0; iq < nPoints; iq++) {
	double factor = (*g)(coordsAtQPs[iq]);
	double resultQP = 0.0;
	for (int i = 0; i < dow; i++)
	  resultQP += grdUhAtQP[iq][i];

	result[iq] += f * factor * resultQP;
      }
    }
  }


  // ========== VectorGradient_FOT ==========

  VectorGradient_FOT::VectorGradient_FOT(DOFVectorBase<double> *dv,
					 AbstractFunction<WorldVector<double>, WorldVector<double> > *af)
    : FirstOrderTerm(af ? af->getDegree() : dv->getFeSpace()->getBasisFcts()->getDegree()),
      vec(dv), f(af)
  {
    FUNCNAME("VectorGradient_FOT::VectorGradient_FOT()");

    TEST_EXIT(dv)("No vector!\n");

    auxFeSpaces.insert(dv->getFeSpace());
  }

  void VectorGradient_FOT::initElement(const ElInfo* elInfo,
				       SubAssembler* subAssembler,
				       Quadrature *quad)
  {
    getGradientsAtQPs(vec, elInfo, subAssembler, quad, gradAtQPs);
  }

  void VectorGradient_FOT::getLb(const ElInfo *elInfo,
				 vector<mtl::dense_vector<double> >& Lb) const
  {
    const DimVec<WorldVector<double> > &grdLambda = elInfo->getGrdLambda();
    const int nPoints = static_cast<int>(Lb.size());

    if (f)
      for (int iq = 0; iq < nPoints; iq++)
        lb(grdLambda, (*f)(gradAtQPs[iq]), Lb[iq], 1.0);
    else
      for (int iq = 0; iq < nPoints; iq++)
        lb(grdLambda, gradAtQPs[iq], Lb[iq], 1.0);
  }

  void VectorGradient_FOT::eval(int nPoints,
				const mtl::dense_vector<double>& uhAtQP,
				const mtl::dense_vector<WorldVector<double> >& grdUhAtQP,
				const mtl::dense_vector<WorldMatrix<double> >& D2UhAtQP,
				mtl::dense_vector<double>& result,
				double factor)
  {
    if (num_rows(grdUhAtQP) > 0) {
      if (f) {
	for (int iq = 0; iq < nPoints; iq++) {
	  WorldVector<double> b = (*f)(gradAtQPs[iq]);
	  result[iq] += b * grdUhAtQP[iq] * factor;
	}
      } else {
	for (int iq = 0; iq < nPoints; iq++)
	  result[iq] += gradAtQPs[iq] * grdUhAtQP[iq] * factor;
      }
    }
  }


  // =========== VectorFct_FOT ==========

  VectorFct_FOT::VectorFct_FOT(DOFVectorBase<double> *dv,
			       AbstractFunction<WorldVector<double>, double> *fct)
    : FirstOrderTerm(fct->getDegree()), vec(dv), vecFct(fct)
  {
    TEST_EXIT(dv)("No vector!\n");

    auxFeSpaces.insert(dv->getFeSpace());
  }

  void VectorFct_FOT::initElement(const ElInfo* elInfo,
				  SubAssembler* subAssembler,
				  Quadrature *quad)
  {
    getVectorAtQPs(vec, elInfo, subAssembler, quad, vecAtQPs);
  }

  void VectorFct_FOT::getLb(const ElInfo *elInfo,
			    vector<mtl::dense_vector<double> >& Lb) const
  {
    const DimVec<WorldVector<double> > &grdLambda = elInfo->getGrdLambda();
    const int nPoints = static_cast<int>(Lb.size());

    for (int iq = 0; iq < nPoints; iq++)
      lb(grdLambda, (*vecFct)(vecAtQPs[iq]), Lb[iq], 1.0);
  }

  void VectorFct_FOT::eval( int nPoints,
			    const mtl::dense_vector<double>& uhAtQP,
			    const mtl::dense_vector<WorldVector<double> >& grdUhAtQP,
			    const mtl::dense_vector<WorldMatrix<double> >& D2UhAtQP,
			    mtl::dense_vector<double>& result,
			    double factor)
  {
    if (num_rows(grdUhAtQP) > 0) {
      for (int iq = 0; iq < nPoints; iq++) {
	WorldVector<double> b = (*vecFct)(vecAtQPs[iq]);
	result[iq] += b * grdUhAtQP[iq] * factor;
      }
    }
  }


  // =========== VecFctAtQP_FOT ==========

  void VecFctAtQP_FOT::initElement(const ElInfo* elInfo,
				   SubAssembler* subAssembler,
				   Quadrature *quad)
  {
    subAssembler->getCoordsAtQPs(elInfo, quad, coordsAtQPs);
  }


  void VecFctAtQP_FOT::getLb(const ElInfo *elInfo,
			     vector<mtl::dense_vector<double> >& Lb) const
  {
    const DimVec<WorldVector<double> > &grdLambda = elInfo->getGrdLambda();
    const int nPoints = static_cast<int>(Lb.size());

    for (int iq = 0; iq < nPoints; iq++)
      lb(grdLambda, (*g)(coordsAtQPs[iq]), Lb[iq], 1.0);
  }

  void VecFctAtQP_FOT::eval(int nPoints,
			    const mtl::dense_vector<double>& uhAtQP,
			    const mtl::dense_vector<WorldVector<double> >& grdUhAtQP,
			    const mtl::dense_vector<WorldMatrix<double> >& D2UhAtQP,
			    mtl::dense_vector<double>& result,
			    double f)
  {
    int dow = Global::getGeo(WORLD);

    if (num_rows(grdUhAtQP) > 0) {
      for (int iq = 0; iq < nPoints; iq++) {
	double resultQP = 0.0;
	const WorldVector<double> &b = (*g)(coordsAtQPs[iq]);
	for (int i = 0; i < dow; i++)
	  resultQP += b[i] * grdUhAtQP[iq][i];
	result[iq] += f * resultQP;
      }
    }
  }


  // =========== VecGrad_FOT ===========

  VecGrad_FOT::VecGrad_FOT(DOFVectorBase<double> *dv1, DOFVectorBase<double> *dv2,
			   BinaryAbstractFunction<WorldVector<double>, double, WorldVector<double> > *fct)
    : FirstOrderTerm(fct->getDegree()), vec1(dv1), vec2(dv2), vecFct(fct)
  {
    TEST_EXIT(dv1)("No first vector!\n");
    TEST_EXIT(dv2)("No second vector!\n");

    auxFeSpaces.insert(dv1->getFeSpace());
    auxFeSpaces.insert(dv2->getFeSpace());
  }

  void VecGrad_FOT::initElement(const ElInfo* elInfo,
				SubAssembler* subAssembler,
				Quadrature *quad)
  {
    getVectorAtQPs(vec1, elInfo, subAssembler, quad, vecAtQPs);
    getGradientsAtQPs(vec2, elInfo, subAssembler, quad, gradAtQPs);
  }

  void VecGrad_FOT::initElement(const ElInfo* smallElInfo,
				const ElInfo* largeElInfo,
				SubAssembler* subAssembler,
				Quadrature *quad)
  {
    getVectorAtQPs(vec1, smallElInfo, largeElInfo, subAssembler, quad, vecAtQPs);
    getGradientsAtQPs(vec2, smallElInfo, largeElInfo, subAssembler, quad, gradAtQPs);
  }

  void VecGrad_FOT::getLb(const ElInfo *elInfo,
			  vector<mtl::dense_vector<double> >& Lb) const
  {
    const DimVec<WorldVector<double> > &grdLambda = elInfo->getGrdLambda();
    const int nPoints = static_cast<int>(Lb.size());

    for (int iq = 0; iq < nPoints; iq++)
      lb(grdLambda, (*vecFct)(vecAtQPs[iq], gradAtQPs[iq]), Lb[iq], 1.0);
  }

  void VecGrad_FOT::eval( int nPoints,
			  const mtl::dense_vector<double>& uhAtQP,
			  const mtl::dense_vector<WorldVector<double> >& grdUhAtQP,
			  const mtl::dense_vector<WorldMatrix<double> >& D2UhAtQP,
			  mtl::dense_vector<double>& result,
			  double factor)
  {
    if (num_rows(grdUhAtQP) > 0) {
      for (int iq = 0; iq < nPoints; iq++) {
	WorldVector<double> b = (*vecFct)(vecAtQPs[iq], gradAtQPs[iq]);
	result[iq] += b * grdUhAtQP[iq] * factor;
      }
    }
  }


  // =========== FctGrad2_FOT ===========

  void FctGrad2_FOT::initElement(const ElInfo* elInfo,
				SubAssembler* subAssembler,
				Quadrature *quad)
  {
    getGradientsAtQPs(vec1, elInfo, subAssembler, quad, grad1AtQPs);
    getGradientsAtQPs(vec2, elInfo, subAssembler, quad, grad2AtQPs);
  }

  void FctGrad2_FOT::getLb(const ElInfo *elInfo,
			   vector<mtl::dense_vector<double> >& Lb) const
  {
    const DimVec<WorldVector<double> > &grdLambda = elInfo->getGrdLambda();
    const int nPoints = static_cast<int>(Lb.size());

    for (int iq = 0; iq < nPoints; iq++)
      lb(grdLambda, (*vecFct)(grad1AtQPs[iq], grad2AtQPs[iq]), Lb[iq], 1.0);
  }

  void FctGrad2_FOT::eval(int nPoints,
			  const mtl::dense_vector<double>& uhAtQP,
			  const mtl::dense_vector<WorldVector<double> >& grdUhAtQP,
			  const mtl::dense_vector<WorldMatrix<double> >& D2UhAtQP,
			  mtl::dense_vector<double>& result,
			  double factor)
  {
    if (num_rows(grdUhAtQP) > 0) {
      for (int iq = 0; iq < nPoints; iq++) {
	WorldVector<double> b = (*vecFct)(grad1AtQPs[iq], grad2AtQPs[iq]);
	result[iq] += b * grdUhAtQP[iq] * factor;
      }
    }
  }


  // ========== Vec2AndGradAtQP_FOT ==========

  Vec2AndGradAtQP_FOT::Vec2AndGradAtQP_FOT(DOFVectorBase<double> *dv1,
					   DOFVectorBase<double> *dv2,
					   TertiaryAbstractFunction<double, double, double, WorldVector<double> > *f_, WorldVector<double> *b_)
    : FirstOrderTerm(f_->getDegree()),
      vec1(dv1),
      vec2(dv2),
      f(f_),
      b(b_)
  {
    auxFeSpaces.insert(dv1->getFeSpace());
    auxFeSpaces.insert(dv2->getFeSpace());
  }

  void Vec2AndGradAtQP_FOT::initElement(const ElInfo* elInfo,
					SubAssembler* subAssembler,
					Quadrature *quad)
  {
    getVectorAtQPs(vec1, elInfo, subAssembler, quad, vec1AtQPs);
    getVectorAtQPs(vec2, elInfo, subAssembler, quad, vec2AtQPs);
    getGradientsAtQPs(vec1, elInfo, subAssembler, quad, gradAtQPs1);
  }

  void Vec2AndGradAtQP_FOT::getLb(const ElInfo *elInfo,
				  vector<mtl::dense_vector<double> >& Lb) const
  {
    const DimVec<WorldVector<double> > &grdLambda = elInfo->getGrdLambda();
    const int nPoints = static_cast<int>(Lb.size());

    for (int iq = 0; iq < nPoints; iq++) {
      if (b)
	lb(grdLambda, *b, Lb[iq], (*f)(vec1AtQPs[iq], vec2AtQPs[iq],gradAtQPs1[iq]));
      else
	l1(grdLambda, Lb[iq], (*f)(vec1AtQPs[iq], vec2AtQPs[iq], gradAtQPs1[iq]));
    }
  }

  void Vec2AndGradAtQP_FOT::eval( int nPoints,
				  const mtl::dense_vector<double>& uhAtQP,
				  const mtl::dense_vector<WorldVector<double> >& grdUhAtQP,
				  const mtl::dense_vector<WorldMatrix<double> >& D2UhAtQP,
				  mtl::dense_vector<double>& result,
				  double fac)
  {
    if (num_rows(grdUhAtQP) > 0)
      for (int iq = 0; iq < nPoints; iq++)
	result[iq] += fac *
	  (*f)(vec1AtQPs[iq], vec2AtQPs[iq], gradAtQPs1[iq]) *
	  ((*b) * grdUhAtQP[iq]);
  }


  // ========== FctVecAtQP_FOT ==========


  FctVecAtQP_FOT::FctVecAtQP_FOT(DOFVectorBase<double> *dv,
				 BinaryAbstractFunction<double, WorldVector<double>, double> *f_,
				 WorldVector<double> *b_)
    : FirstOrderTerm(f_->getDegree()), vec(dv), f(f_), b(b_)
  {
    auxFeSpaces.insert(dv->getFeSpace());
  }

  void FctVecAtQP_FOT::initElement(const ElInfo* elInfo,
				   SubAssembler* subAssembler,
				   Quadrature *quad)
  {
    getVectorAtQPs(vec, elInfo, subAssembler, quad, vecAtQPs);
    subAssembler->getCoordsAtQPs(elInfo, quad, coordsAtQPs);
  }

  void FctVecAtQP_FOT::getLb(const ElInfo *elInfo,
			     vector<mtl::dense_vector<double> >& Lb) const
  {
    const DimVec<WorldVector<double> > &grdLambda = elInfo->getGrdLambda();
    const int nPoints = static_cast<int>(Lb.size());

    for (int iq = 0; iq < nPoints; iq++) {
      if (b)
	lb(grdLambda, *b, Lb[iq], (*f)(coordsAtQPs[iq], vecAtQPs[iq]));
      else
	l1(grdLambda, Lb[iq], (*f)(coordsAtQPs[iq], vecAtQPs[iq]));
    }
  }

  void FctVecAtQP_FOT::eval(int nPoints,
			    const mtl::dense_vector<double>& uhAtQP,
			    const mtl::dense_vector<WorldVector<double> >& grdUhAtQP,
			    const mtl::dense_vector<WorldMatrix<double> >& D2UhAtQP,
			    mtl::dense_vector<double>& result,
			    double fac)
  {
    if (num_rows(grdUhAtQP) > 0)
      for (int iq = 0; iq < nPoints; iq++)
	result[iq] +=
	  fac * (*f)(coordsAtQPs[iq], vecAtQPs[iq]) * ((*b) * grdUhAtQP[iq]);
  }


  // ========== Vec2AtQP_FOT ==========

  Vec2AtQP_FOT::Vec2AtQP_FOT(DOFVectorBase<double> *dv1, DOFVectorBase<double> *dv2,
			     BinaryAbstractFunction<double, double, double> *af,
			     WorldVector<double> *b_)
    : FirstOrderTerm(af ?
		     af->getDegree() :
		     dv1->getFeSpace()->getBasisFcts()->getDegree()
		     + dv2->getFeSpace()->getBasisFcts()->getDegree()),
      vec1(dv1), vec2(dv2), f(af), b(b_)
  {
    TEST_EXIT(dv1)("No first vector!\n");
    TEST_EXIT(dv2)("No second vector!\n");

    auxFeSpaces.insert(dv1->getFeSpace());
    auxFeSpaces.insert(dv2->getFeSpace());
  }

  Vec2AtQP_FOT::Vec2AtQP_FOT(DOFVectorBase<double> *dv1, DOFVectorBase<double> *dv2,
			     BinaryAbstractFunction<double, double, double> *af,
			     int bIdx)
    : FirstOrderTerm(af ?
		     af->getDegree() :
		     dv1->getFeSpace()->getBasisFcts()->getDegree()
		     + dv2->getFeSpace()->getBasisFcts()->getDegree()),
      vec1(dv1), vec2(dv2), f(af)
  {
    TEST_EXIT(dv1)("No first vector!\n");
    TEST_EXIT(dv2)("No second vector!\n");

    bOne = bIdx;
    auxFeSpaces.insert(dv1->getFeSpace());
    auxFeSpaces.insert(dv2->getFeSpace());
  }

  void Vec2AtQP_FOT::initElement(const ElInfo* elInfo,
				 SubAssembler* subAssembler,
				 Quadrature *quad)
  {
    getVectorAtQPs(vec1, elInfo, subAssembler, quad, vec1AtQPs);
    getVectorAtQPs(vec2, elInfo, subAssembler, quad, vec2AtQPs);
  }

  void Vec2AtQP_FOT::getLb(const ElInfo *elInfo,
			   vector<mtl::dense_vector<double> >& Lb) const
  {
    const DimVec<WorldVector<double> > &grdLambda = elInfo->getGrdLambda();
    const int nPoints = static_cast<int>(Lb.size());

    if (bOne > -1) {
      if (f) {
	for (int iq = 0; iq < nPoints; iq++)
	  lb_one(grdLambda, Lb[iq], (*f)(vec1AtQPs[iq], vec2AtQPs[iq]));
      } else {
	for (int iq = 0; iq < nPoints; iq++)
	  lb_one(grdLambda, Lb[iq], vec1AtQPs[iq] * vec2AtQPs[iq]);
      }
    } else if (b) {
      if (f) {
	for (int iq = 0; iq < nPoints; iq++)
	  lb(grdLambda, *b, Lb[iq], (*f)(vec1AtQPs[iq], vec2AtQPs[iq]));
      } else {
	for (int iq = 0; iq < nPoints; iq++)
	  lb(grdLambda, *b, Lb[iq], vec1AtQPs[iq] * vec2AtQPs[iq]);
      }
    } else {
      if (f) {
	for (int iq = 0; iq < nPoints; iq++)
	  l1(grdLambda, Lb[iq], (*f)(vec1AtQPs[iq], vec2AtQPs[iq]));
      } else {
	for (int iq = 0; iq < nPoints; iq++)
	  l1(grdLambda, Lb[iq], vec1AtQPs[iq] * vec2AtQPs[iq]);
      }
    }
  }

  void Vec2AtQP_FOT::eval(int nPoints,
			  const mtl::dense_vector<double>& uhAtQP,
			  const mtl::dense_vector<WorldVector<double> >& grdUhAtQP,
			  const mtl::dense_vector<WorldMatrix<double> >& D2UhAtQP,
			  mtl::dense_vector<double>& result,
			  double fac)
  {
    if (num_rows(grdUhAtQP) > 0) {
      if (f) {
	for (int iq = 0; iq < nPoints; iq++)
	  result[iq] += fac * (*f)(vec1AtQPs[iq], vec2AtQPs[iq]) * ((*b) * grdUhAtQP[iq]);
      } else {
	for (int iq = 0; iq < nPoints; iq++)
	  result[iq] += fac * vec1AtQPs[iq] * vec2AtQPs[iq] * ((*b) * grdUhAtQP[iq]);
      }
    }
  }


  // ========== Vec3AtQP_FOT ==========

  Vec3AtQP_FOT::Vec3AtQP_FOT(DOFVectorBase<double> *dv1,
				   DOFVectorBase<double> *dv2,
				   DOFVectorBase<double> *dv3,
				   TertiaryAbstractFunction<double, double, double, double> *f_,
				   WorldVector<double> *bvec)
    : FirstOrderTerm(f_ ?
		     f_->getDegree() :
		     dv1->getFeSpace()->getBasisFcts()->getDegree()
		     + dv2->getFeSpace()->getBasisFcts()->getDegree()
		     + dv3->getFeSpace()->getBasisFcts()->getDegree()),
      vec1(dv1),
      vec2(dv2),
      vec3(dv3),
      f(f_),
      b(bvec)
  {
    auxFeSpaces.insert(dv1->getFeSpace());
    auxFeSpaces.insert(dv2->getFeSpace());
    auxFeSpaces.insert(dv3->getFeSpace());
  }

  Vec3AtQP_FOT::Vec3AtQP_FOT(DOFVectorBase<double> *dv1,
				   DOFVectorBase<double> *dv2,
				   DOFVectorBase<double> *dv3,
				   TertiaryAbstractFunction<double, double, double, double> *f_,
				   int b)
    : FirstOrderTerm(f_ ?
		     f_->getDegree() :
		     dv1->getFeSpace()->getBasisFcts()->getDegree()
		     + dv2->getFeSpace()->getBasisFcts()->getDegree()
		     + dv3->getFeSpace()->getBasisFcts()->getDegree()),
      vec1(dv1),
      vec2(dv2),
      vec3(dv3),
      f(f_)
  {
    bOne = b;
    auxFeSpaces.insert(dv1->getFeSpace());
    auxFeSpaces.insert(dv2->getFeSpace());
    auxFeSpaces.insert(dv3->getFeSpace());
  }

  void Vec3AtQP_FOT::initElement(const ElInfo* elInfo,
				    SubAssembler* subAssembler,
				    Quadrature *quad)
  {
    getVectorAtQPs(vec1, elInfo, subAssembler, quad, vec1AtQPs);
    getVectorAtQPs(vec2, elInfo, subAssembler, quad, vec2AtQPs);
    getVectorAtQPs(vec3, elInfo, subAssembler, quad, vec3AtQPs);
  }

  void Vec3AtQP_FOT::getLb(const ElInfo *elInfo,
			      vector<mtl::dense_vector<double> >& Lb) const
  {
    const DimVec<WorldVector<double> > &grdLambda = elInfo->getGrdLambda();
    const int nPoints = static_cast<int>(Lb.size());

    if (bOne > -1) {
      if (f) {
	for (int iq = 0; iq < nPoints; iq++)
	  lb_one(grdLambda, Lb[iq], (*f)(vec1AtQPs[iq], vec2AtQPs[iq], vec3AtQPs[iq]));
      } else {
	for (int iq = 0; iq < nPoints; iq++)
	  lb_one(grdLambda, Lb[iq], vec1AtQPs[iq] * vec2AtQPs[iq] * vec3AtQPs[iq]);
      }
    } else {
      if (f) {
	for (int iq = 0; iq < nPoints; iq++) {
	  if (b)
	    lb(grdLambda, *b, Lb[iq], (*f)(vec1AtQPs[iq], vec2AtQPs[iq], vec3AtQPs[iq]));
	  else
	    l1(grdLambda, Lb[iq], (*f)(vec1AtQPs[iq], vec2AtQPs[iq], vec3AtQPs[iq]));
	}
      } else {
	for (int iq = 0; iq < nPoints; iq++) {
	  if (b)
	    lb(grdLambda, *b, Lb[iq], vec1AtQPs[iq] * vec2AtQPs[iq] * vec3AtQPs[iq]);
	  else
	    l1(grdLambda, Lb[iq], vec1AtQPs[iq] * vec2AtQPs[iq] * vec3AtQPs[iq]);
	}
      }
    }
  }

  void Vec3AtQP_FOT::eval(int nPoints,
			     const mtl::dense_vector<double>& uhAtQP,
			     const mtl::dense_vector<WorldVector<double> >& grdUhAtQP,
			     const mtl::dense_vector<WorldMatrix<double> >& D2UhAtQP,
			     mtl::dense_vector<double>& result,
			     double fac)
  {
    if (num_rows(grdUhAtQP) == 0)
      return;

    if (bOne > -1) {
      ERROR_EXIT("Not yet implemented!\n");
    } else {
      if (f) {
	for (int iq = 0; iq < nPoints; iq++)
	  result[iq] += fac * (*f)(vec1AtQPs[iq], vec2AtQPs[iq] ,vec3AtQPs[iq]) *
	    ((*b) * grdUhAtQP[iq]);
      } else {
	for (int iq = 0; iq < nPoints; iq++)
	  result[iq] += fac * vec1AtQPs[iq] * vec2AtQPs[iq] * vec3AtQPs[iq] *
	    ((*b) * grdUhAtQP[iq]);
      }
    }
  }


  // =========== General_FOT ==========

  General_FOT::General_FOT(vector<DOFVectorBase<double>*> vecs,
			   vector<DOFVectorBase<double>*> grads,
			   TertiaryAbstractFunction<WorldVector<double>,
			   WorldVector<double>,
			   vector<double>,
			   vector<WorldVector<double> > > *af)
    : FirstOrderTerm(af->getDegree()), vecs_(vecs), grads_(grads), f_(af)
  {
    vecsAtQPs.resize(vecs_.size());
    gradsAtQPs_.resize(grads_.size());

    for (int i = 0; i < static_cast<int>(vecs.size()); i++) {
      TEST_EXIT(vecs[i])("One vector is NULL!\n");

      auxFeSpaces.insert(vecs[i]->getFeSpace());
    }

    for (int i = 0; i < static_cast<int>(grads.size()); i++) {
      TEST_EXIT(grads[i])("One gradient vector is NULL!\n");

      auxFeSpaces.insert(grads[i]->getFeSpace());
    }
  }


  void General_FOT::initElement(const ElInfo* elInfo,
				SubAssembler* subAssembler,
				Quadrature *quad)
  {
    int nVecs = static_cast<int>(vecs_.size());
    int nGrads = static_cast<int>(grads_.size());

    subAssembler->getCoordsAtQPs(elInfo, quad, coordsAtQPs);

    for (int i = 0; i < nVecs; i++)
      getVectorAtQPs(vecs_[i], elInfo, subAssembler, quad, vecsAtQPs[i]);
    for (int i = 0; i < nGrads; i++)
      getGradientsAtQPs(grads_[i], elInfo, subAssembler, quad, gradsAtQPs_[i]);
  }


  void General_FOT::getLb(const ElInfo *elInfo,
			  vector<mtl::dense_vector<double> >& Lb) const
  {
    int nVecs = static_cast<int>(vecs_.size());
    int nGrads = static_cast<int>(grads_.size());

    vector<double> vecsArg(nVecs);
    vector<WorldVector<double> > gradsArg(nGrads);

    const DimVec<WorldVector<double> > &grdLambda = elInfo->getGrdLambda();
    const int nPoints = static_cast<int>(Lb.size());

    for (int iq = 0; iq < nPoints; iq++) {
      for (int i = 0; i < nVecs; i++)
	vecsArg[i] = vecsAtQPs[i][iq];
      for (int i = 0; i < nGrads; i++)
	gradsArg[i] = gradsAtQPs_[i][iq];

      lb(grdLambda, (*f_)(coordsAtQPs[iq], vecsArg, gradsArg), Lb[iq], 1.0);
    }
  }


  void General_FOT::eval( int nPoints,
			  const mtl::dense_vector<double>& uhAtQP,
			  const mtl::dense_vector<WorldVector<double> >& grdUhAtQP,
			  const mtl::dense_vector<WorldMatrix<double> >& D2UhAtQP,
			  mtl::dense_vector<double>& result,
			  double factor)
  {
    int dow = Global::getGeo(WORLD);
    int nVecs = static_cast<int>(vecs_.size());
    int nGrads = static_cast<int>(grads_.size());

    vector<double> vecsArg(nVecs);
    vector<WorldVector<double> > gradsArg(nGrads);

    if (num_rows(grdUhAtQP) > 0) {
      for (int iq = 0; iq < nPoints; iq++) {
	double resultQP = 0.0;

	for (int i = 0; i < nVecs; i++)
	  vecsArg[i] = vecsAtQPs[i][iq];
	for (int i = 0; i < nGrads; i++)
	  gradsArg[i] = gradsAtQPs_[i][iq];

	const WorldVector<double> &b = (*f_)(coordsAtQPs[iq], vecsArg, gradsArg);

	for (int i = 0; i < dow; i++)
	  resultQP += grdUhAtQP[iq][i] * b[i];

	result[iq] += factor * resultQP;
      }
    }
  }


  // =========== GeneralParametric_FOT ==========

  GeneralParametric_FOT::GeneralParametric_FOT(vector<DOFVectorBase<double>*> vecs,
					       vector<DOFVectorBase<double>*> grads,
					       QuartAbstractFunction<WorldVector<double>,
					       WorldVector<double>,
					       WorldVector<double>,
					       vector<double>,
					       vector<WorldVector<double> > > *af)
    : FirstOrderTerm(af->getDegree()), vecs_(vecs), grads_(grads), f_(af)
  {
    vecsAtQPs.resize(vecs_.size());
    gradsAtQPs_.resize(grads_.size());

    for (int i = 0; i < static_cast<int>(vecs.size()); i++) {
      TEST_EXIT(vecs[i])("One vector is NULL!\n");

      auxFeSpaces.insert(vecs[i]->getFeSpace());
    }

    for (int i = 0; i < static_cast<int>(grads.size()); i++) {
      TEST_EXIT(grads[i])("One gradient vector is NULL!\n");

      auxFeSpaces.insert(grads[i]->getFeSpace());
    }
  }


  void GeneralParametric_FOT::initElement(const ElInfo* elInfo,
					  SubAssembler* subAssembler,
					  Quadrature *quad)
  {
    int nVecs = static_cast<int>(vecs_.size());
    int nGrads = static_cast<int>(grads_.size());

    elInfo->getElementNormal(elementNormal_);
    subAssembler->getCoordsAtQPs(elInfo, quad, coordsAtQPs);

    for (int i = 0; i < nVecs; i++)
      getVectorAtQPs(vecs_[i], elInfo, subAssembler, quad, vecsAtQPs[i]);
    for (int i = 0; i < nGrads; i++)
      getGradientsAtQPs(grads_[i], elInfo, subAssembler, quad, gradsAtQPs_[i]);
  }


  void GeneralParametric_FOT::getLb(const ElInfo *elInfo,
				    vector<mtl::dense_vector<double> >& Lb) const
  {
    int nVecs = static_cast<int>(vecs_.size());
    int nGrads = static_cast<int>(grads_.size());

    vector<double> vecsArg(nVecs);
    vector<WorldVector<double> > gradsArg(nGrads);

    const DimVec<WorldVector<double> > &grdLambda = elInfo->getGrdLambda();
    const int nPoints = static_cast<int>(Lb.size());

    for (int iq = 0; iq < nPoints; iq++) {
      for (int i = 0; i < nVecs; i++)
	vecsArg[i] = vecsAtQPs[i][iq];
      for (int i = 0; i < nGrads; i++)
	gradsArg[i] = gradsAtQPs_[i][iq];

      lb(grdLambda, (*f_)(coordsAtQPs[iq],  elementNormal_, vecsArg, gradsArg),
	 Lb[iq], 1.0);
    }
  }


  void GeneralParametric_FOT::eval(int nPoints,
				   const mtl::dense_vector<double>& uhAtQP,
				   const mtl::dense_vector<WorldVector<double> >& grdUhAtQP,
				   const mtl::dense_vector<WorldMatrix<double> >& D2UhAtQP,
				   mtl::dense_vector<double>& result,
				   double factor)
  {
    int dow = Global::getGeo(WORLD);
    int nVecs = static_cast<int>(vecs_.size());
    int nGrads = static_cast<int>(grads_.size());

    vector<double> vecsArg(nVecs);
    vector<WorldVector<double> > gradsArg(nGrads);

    if (num_rows(grdUhAtQP) > 0) {
      for (int iq = 0; iq < nPoints; iq++) {
	double resultQP = 0.0;

	for (int i = 0; i < nVecs; i++)
	  vecsArg[i] = vecsAtQPs[i][iq];
	for (int i = 0; i < nGrads; i++)
	  gradsArg[i] = gradsAtQPs_[i][iq];

	const WorldVector<double> &b =
	  (*f_)(coordsAtQPs[iq],  elementNormal_, vecsArg, gradsArg);

	for (int i = 0; i < dow; i++)
	  resultQP += grdUhAtQP[iq][i] * b[i];

	result[iq] += factor * resultQP;
      }
    }
  }



}