#include <vector>
#include "Assembler.h"
#include "ZeroOrderAssembler.h"
#include "Operator.h"
#include "QPsiPhi.h"
#include "FiniteElemSpace.h"
#include "Quadrature.h"
#include "DOFVector.h"
#include "ElementMatrix.h"
#include "OpenMP.h"
#include "SubElInfo.h"
#include "ScalableQuadrature.h"

namespace AMDiS {

  std::vector<SubAssembler*> ZeroOrderAssembler::optimizedSubAssemblers;
  std::vector<SubAssembler*> ZeroOrderAssembler::standardSubAssemblers;

  ZeroOrderAssembler::ZeroOrderAssembler(Operator *op,
					 Assembler *assembler,
					 Quadrature *quad,
					 bool optimized)
    : SubAssembler(op, assembler, quad, 0, optimized)
  {}

  ZeroOrderAssembler* ZeroOrderAssembler::getSubAssembler(Operator* op,
							  Assembler *assembler,
							  Quadrature *quad,
							  bool optimized)
  {
    // check if a assembler is needed at all
    if (op->zeroOrder.size() == 0) {
      return NULL;
    }

    ZeroOrderAssembler *newAssembler;

    std::vector<SubAssembler*> *subAssemblers =
      optimized ? &optimizedSubAssemblers : &standardSubAssemblers;

    int myRank = omp_get_thread_num();
    std::vector<OperatorTerm*> opTerms  = op->zeroOrder[myRank];

    sort(opTerms.begin(), opTerms.end());

    // check if a new assembler is needed
    if (quad) {
      for (int i = 0; i < static_cast<int>( subAssemblers->size()); i++) {
	std::vector<OperatorTerm*> assTerms = *((*subAssemblers)[i]->getTerms());

	sort(assTerms.begin(), assTerms.end());

	if ((opTerms == assTerms) && 
	    ((*subAssemblers)[i]->getQuadrature() == quad)) {
	
	  return dynamic_cast<ZeroOrderAssembler*>((*subAssemblers)[i]);
	}
      }
    }
 
    // check if all terms are pw_const
    bool pwConst = true;
    for (int i = 0; i < static_cast<int>( op->zeroOrder[myRank].size()); i++) {
      if (!op->zeroOrder[myRank][i]->isPWConst()) {
	pwConst = false;
	break;
      }
    }  

    // create new assembler
    if (!optimized) {
      newAssembler = NEW StandardZOA(op, assembler, quad);
    } else {
      if (pwConst) {
	newAssembler = NEW PrecalcZOA(op, assembler, quad);
      } else {
	newAssembler = NEW FastQuadZOA(op, assembler, quad);
      }
    }

    subAssemblers->push_back(newAssembler);
    return newAssembler;
  }

  StandardZOA::StandardZOA(Operator *op, Assembler *assembler, Quadrature *quad)
    : ZeroOrderAssembler(op, assembler, quad, false)
  {
  }

  void StandardZOA::calculateElementMatrix(const ElInfo *elInfo, ElementMatrix *mat)
  {
    const BasisFunction *psi = owner->getRowFESpace()->getBasisFcts();
    const BasisFunction *phi = owner->getColFESpace()->getBasisFcts();

    double *phival = GET_MEMORY(double, nCol);
    int nPoints = quadrature->getNumPoints();
    double *c = GET_MEMORY(double, nPoints);

    for (int iq = 0; iq < nPoints; iq++) {
      c[iq] = 0.0;
    }

    int myRank = omp_get_thread_num();
    std::vector<OperatorTerm*>::iterator termIt;
    for (termIt = terms[myRank].begin(); termIt != terms[myRank].end(); ++termIt) {
      (static_cast<ZeroOrderTerm*>((*termIt)))->getC(elInfo, nPoints, c);
    }
      
    if (symmetric) {
      for (int iq = 0; iq < nPoints; iq++) {
	c[iq] *= elInfo->getDet();

	// calculate phi at QPs only once!
	for (int i = 0; i < nCol; i++) {
	  phival[i] = (*(phi->getPhi(i)))(quadrature->getLambda(iq));
	}

	for (int i = 0; i < nRow; i++) {
	  double psival = (*(psi->getPhi(i)))(quadrature->getLambda(iq));
	  (*mat)[i][i] += quadrature->getWeight(iq) * c[iq] * psival * phival[i];
	  for (int j = i + 1; j < nCol; j++) {
	    double val = quadrature->getWeight(iq) * c[iq] * psival * phival[j];
	    (*mat)[i][j] += val;
	    (*mat)[j][i] += val;
	  }
	}
      }
    } else {      //  non symmetric assembling 
      for (int iq = 0; iq < nPoints; iq++) {
	c[iq] *= elInfo->getDet();

	// calculate phi at QPs only once!
	for (int i = 0; i < nCol; i++) {
	  phival[i] = (*(phi->getPhi(i)))(quadrature->getLambda(iq));
	}

	for (int i = 0; i < nRow; i++) {
	  double psival = (*(psi->getPhi(i)))(quadrature->getLambda(iq));
	  for (int j = 0; j < nCol; j++) {
	    (*mat)[i][j] += quadrature->getWeight(iq) * c[iq] * psival * phival[j];
	  }
	}
      }
    }

    FREE_MEMORY(phival, double, nCol);
    FREE_MEMORY(c, double, nPoints);
  }

  void StandardZOA::calculateElementMatrix(const ElInfo *rowElInfo,
					   const ElInfo *colElInfo,
					   ElementMatrix *mat) 
  {
    const BasisFunction *psi = owner->getRowFESpace()->getBasisFcts();
    const BasisFunction *phi = owner->getColFESpace()->getBasisFcts();

    double *phival = GET_MEMORY(double, nCol);
    int nPoints = quadrature->getNumPoints();
    double *c = GET_MEMORY(double, nPoints);

    int myRank = omp_get_thread_num();
    std::vector<OperatorTerm*>::iterator termIt;
    for (termIt = terms[myRank].begin(); termIt != terms[myRank].end(); ++termIt) {
      (static_cast<ZeroOrderTerm*>((*termIt)))->getC(rowElInfo, nPoints, c);
    }

    SubElInfo *subElInfo = NEW SubElInfo(colElInfo->getSubElemCoords(), rowElInfo);
    Quadrature psiQuadrature(*quadrature);
    ScalableQuadrature *scaledQuadrature = NEW ScalableQuadrature(&psiQuadrature);
    scaledQuadrature->scaleQuadrature(*subElInfo);

    for (int iq = 0; iq < nPoints; iq++) {
      c[iq] *= rowElInfo->getDet();

      // calculate phi at QPs only once!
      for (int i = 0; i < nCol; i++) {
	phival[i] = (*(phi->getPhi(i)))(quadrature->getLambda(iq));
      }

      for (int i = 0; i < nRow; i++) {
	double psival = (*(psi->getPhi(i)))(psiQuadrature.getLambda(iq));
	for (int j = 0; j < nCol; j++) {
	  (*mat)[i][j] += quadrature->getWeight(iq) * c[iq] * psival * phival[j];
	}
      }
    }

    DELETE scaledQuadrature;
    DELETE subElInfo;

    FREE_MEMORY(phival, double, nCol);
    FREE_MEMORY(c, double, nPoints);

    //    ERROR_EXIT("SO, HIER GEHTS WEITER\n");
  }


  void StandardZOA::calculateElementVector(const ElInfo *elInfo, ElementVector *vec)
  {
    int nPoints = quadrature->getNumPoints();

    double *c = GET_MEMORY(double, nPoints);

    for (int iq = 0; iq < nPoints; iq++) {
      c[iq] = 0.0;
    }

    int myRank = omp_get_thread_num();
    std::vector<OperatorTerm*>::iterator termIt;
    for (termIt = terms[myRank].begin(); termIt != terms[myRank].end(); ++termIt) {
      (static_cast<ZeroOrderTerm*>((*termIt)))->getC(elInfo, nPoints, c);
    }

    for (int iq = 0; iq < nPoints; iq++) {
      c[iq] *= elInfo->getDet();

      for (int i = 0; i < nRow; i++) {
	double psi = (*(owner->getRowFESpace()->getBasisFcts()->getPhi(i)))
	  (quadrature->getLambda(iq));
	(*vec)[i] += quadrature->getWeight(iq) * c[iq] * psi;
      }
    }
    
    FREE_MEMORY(c, double, nPoints);
  }

  FastQuadZOA::FastQuadZOA(Operator *op, Assembler *assembler, Quadrature *quad)
    : ZeroOrderAssembler(op, assembler, quad, true)
  {
    cPtrs.resize(omp_get_max_threads());
  }

  FastQuadZOA::~FastQuadZOA()
  {
    for (int i = 0; i < omp_get_max_threads(); i++) {
      FREE_MEMORY(cPtrs[i], double, quadrature->getNumPoints());
    }
  }

  void FastQuadZOA::calculateElementMatrix(const ElInfo *elInfo, ElementMatrix *mat)
  {
    int nPoints = quadrature->getNumPoints();
    int myRank = omp_get_thread_num();

    if (firstCall) {
#ifdef _OPENMP
#pragma omp critical
#endif 
      {
	cPtrs[myRank] = GET_MEMORY(double, nPoints);
	const BasisFunction *basFcts = owner->getRowFESpace()->getBasisFcts();
	psiFast = updateFastQuadrature(psiFast, basFcts, INIT_PHI);
	basFcts = owner->getColFESpace()->getBasisFcts();
	phiFast = updateFastQuadrature(phiFast, basFcts, INIT_PHI);
	firstCall = false;
      }
    }

    double *c = cPtrs[myRank];
    for (int iq = 0; iq < nPoints; iq++) {
      c[iq] = 0.0;
    }

    std::vector<OperatorTerm*>::iterator termIt;
    for (termIt = terms[myRank].begin(); termIt != terms[myRank].end(); ++termIt) {
      (static_cast<ZeroOrderTerm*>((*termIt)))->getC(elInfo, nPoints, c);
    }

    if (symmetric) {
      for (int iq = 0; iq < nPoints; iq++) {
	c[iq] *= elInfo->getDet();

	const double *psi = psiFast->getPhi(iq);
	const double *phi = phiFast->getPhi(iq);
	for (int i = 0; i < nRow; i++) {
	  (*mat)[i][i] += quadrature->getWeight(iq) * c[iq] * psi[i] * phi[i];
	  for (int j = i + 1; j < nCol; j++) {
	    double val = quadrature->getWeight(iq) * c[iq] * psi[i] * phi[j];
	    (*mat)[i][j] += val;
	    (*mat)[j][i] += val;
	  }
	}
      }
    } else {      /*  non symmetric assembling   */
      for (int iq = 0; iq < nPoints; iq++) {
	c[iq] *= elInfo->getDet();

	const double *psi = psiFast->getPhi(iq);
	const double *phi = phiFast->getPhi(iq);
	for (int i = 0; i < nRow; i++) {
	  for (int j = 0; j < nCol; j++) {
	    (*mat)[i][j] += quadrature->getWeight(iq) * c[iq] * psi[i] * phi[j];
	  }
	}
      }
    }
  }

  void FastQuadZOA::calculateElementVector(const ElInfo *elInfo, ElementVector *vec)
  {
    if (firstCall) {
#ifdef _OPENMP
#pragma omp critical
#endif 
      {
	const BasisFunction *basFcts = owner->getRowFESpace()->getBasisFcts();
	psiFast = updateFastQuadrature(psiFast, basFcts, INIT_PHI);
	basFcts = owner->getColFESpace()->getBasisFcts();
	phiFast = updateFastQuadrature(phiFast, basFcts, INIT_PHI);
	firstCall = false;
      }
    }

    int nPoints = quadrature->getNumPoints();
    double *c = GET_MEMORY(double, nPoints);

    for (int iq = 0; iq < nPoints; iq++) {
      c[iq] = 0.0;
    }

    int myRank = omp_get_thread_num();
    std::vector<OperatorTerm*>::iterator termIt;
    for (termIt = terms[myRank].begin(); termIt != terms[myRank].end(); ++termIt) {
      (static_cast<ZeroOrderTerm*>((*termIt)))->getC(elInfo, nPoints, c);
    }

    for (int iq = 0; iq < nPoints; iq++) {
      c[iq] *= elInfo->getDet();

      const double *psi = psiFast->getPhi(iq);
      for (int i = 0; i < nRow; i++) {
	(*vec)[i] += quadrature->getWeight(iq) * c[iq] * psi[i];
      }
    }
    FREE_MEMORY(c, double, nPoints);
  }

  PrecalcZOA::PrecalcZOA(Operator *op, Assembler *assembler, Quadrature *quad) 
    : ZeroOrderAssembler(op, assembler, quad, true)
  {
  }

  void PrecalcZOA::calculateElementMatrix(const ElInfo *elInfo, ElementMatrix *mat)
  {
    if (firstCall) {
#ifdef _OPENMP
#pragma omp critical
#endif 
      {
	q00 = Q00PsiPhi::provideQ00PsiPhi(owner->getRowFESpace()->getBasisFcts(), 
					  owner->getColFESpace()->getBasisFcts(), 
					  quadrature);
	q0 = Q0Psi::provideQ0Psi(owner->getRowFESpace()->getBasisFcts(),
				 quadrature);
	firstCall = false;
      }
    }

    double c = 0.0;
    int myRank = omp_get_thread_num();
    int size = static_cast<int>(terms[myRank].size());

    for (int i = 0; i < size; i++) {
      (static_cast<ZeroOrderTerm*>((terms[myRank][i])))->getC(elInfo, 1, &c);
    }

    c *= elInfo->getDet();

    if (symmetric) {
      for (int i = 0; i < nRow; i++) {
	(*mat)[i][i] += c * q00->getValue(i,i);
	for (int j = i + 1; j < nCol; j++) {
	  double val = c * q00->getValue(i, j);
	  (*mat)[i][j] += val;
	  (*mat)[j][i] += val;
	}
      }
    } else {
      for (int i = 0; i < nRow; i++)
	for (int j = 0; j < nCol; j++)
	  (*mat)[i][j] += c * q00->getValue(i, j);
    }
  }

  void PrecalcZOA::calculateElementVector(const ElInfo *elInfo, ElementVector *vec)
  {
    if (firstCall) {
#ifdef _OPENMP
#pragma omp critical
#endif 
      {
	q00 = Q00PsiPhi::provideQ00PsiPhi(owner->getRowFESpace()->getBasisFcts(), 
					  owner->getColFESpace()->getBasisFcts(), 
					  quadrature);
	q0 = Q0Psi::provideQ0Psi(owner->getRowFESpace()->getBasisFcts(),
				 quadrature);	
	firstCall = false;
      }
    }

    std::vector<OperatorTerm*>::iterator termIt;

    int myRank = omp_get_thread_num();
    double c = 0.0;
    for (termIt = terms[myRank].begin(); termIt != terms[myRank].end(); ++termIt) {
      (static_cast<ZeroOrderTerm*>( *termIt))->getC(elInfo, 1, &c);
    }

    c *= elInfo->getDet();

    for (int i = 0; i < nRow; i++)
      (*vec)[i] += c * q0->getValue(i);
  }

}