#include <vector>
#include <boost/numeric/mtl/mtl.hpp>
#include "Assembler.h"
#include "ZeroOrderAssembler.h"
#include "Operator.h"
#include "QPsiPhi.h"
#include "FiniteElemSpace.h"
#include "Quadrature.h"
#include "DOFVector.h"
#include "OpenMP.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)
  {
    int myRank = omp_get_thread_num();

    // check if an assembler is needed at all
    if (op->zeroOrder[myRank].size() == 0)
      return NULL;   

    ZeroOrderAssembler *newAssembler;

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

    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;
      }
    }  

    newAssembler = new StandardZOA(op, assembler, quad);


    // 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)      
  {
    name = "standard zero order assembler";
  }


  void StandardZOA::calculateElementMatrix(const ElInfo *elInfo, ElementMatrix& mat)
  {
    const BasisFunction *psi = rowFeSpace->getBasisFcts();
    const BasisFunction *phi = colFeSpace->getBasisFcts();

    int nPoints = quadrature->getNumPoints();
    std::vector<double> c(nPoints, 0.0);
    std::vector<double> phival(nCol);

    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) {
      TEST_EXIT_DBG(nCol == nRow)("nCol != nRow, but symmetric assembling!\n");
            
      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];
	}
      }
    }
  }


  void StandardZOA::calculateElementVector(const ElInfo *elInfo, ElementVector& vec)
  {
    int nPoints = quadrature->getNumPoints();
    std::vector<double> c(nPoints, 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 = (*(rowFeSpace->getBasisFcts()->getPhi(i)))
	  (quadrature->getLambda(iq));
	vec[i] += quadrature->getWeight(iq) * c[iq] * psi;
      }
    }
  }


  FastQuadZOA::FastQuadZOA(Operator *op, Assembler *assembler, Quadrature *quad)
    : ZeroOrderAssembler(op, assembler, quad, true)
  {
    name = "fast quadrature zero order assembler";
    tmpC.resize(omp_get_overall_max_threads());
  }


  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 
      {
	const BasisFunction *basFcts = rowFeSpace->getBasisFcts();
	psiFast = updateFastQuadrature(psiFast, basFcts, INIT_PHI);
	basFcts = colFeSpace->getBasisFcts();
	phiFast = updateFastQuadrature(phiFast, basFcts, INIT_PHI);
	firstCall = false;
      }
    }

    std::vector<double> &c = tmpC[myRank];
    if (static_cast<int>(c.size()) < nPoints)
      c.resize(nPoints);
    for (int i = 0; i < nPoints; i++)
      c[i] = 0.0;

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

    if (symmetric) {
      TEST_EXIT_DBG(nCol == nRow)("nCol != nRow, but symmetric assembling!\n");

      for (int iq = 0; iq < nPoints; iq++) {
	c[iq] *= elInfo->getDet() * quadrature->getWeight(iq);
	const double *psi = psiFast->getPhi(iq);
	const double *phi = phiFast->getPhi(iq);

	for (int i = 0; i < nRow; i++) {
	  mat[i][i] += c[iq] * psi[i] * phi[i];
	  for (int j = i + 1; j < nCol; j++) {
	    double val = 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() * quadrature->getWeight(iq);

	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] += c[iq] * psi[i] * phi[j];
      }
    }
  }


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

    if (firstCall) {
#ifdef _OPENMP
#pragma omp critical
#endif 
      {
	const BasisFunction *basFcts = rowFeSpace->getBasisFcts();
	psiFast = updateFastQuadrature(psiFast, basFcts, INIT_PHI);
	basFcts = colFeSpace->getBasisFcts();
	phiFast = updateFastQuadrature(phiFast, basFcts, INIT_PHI);
	firstCall = false;
      }
    }

    std::vector<double> c(nPoints, 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);
    }

    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];
      }
    }
  }


  PrecalcZOA::PrecalcZOA(Operator *op, Assembler *assembler, Quadrature *quad) 
    : ZeroOrderAssembler(op, assembler, quad, true)
  {
    name = "precalculated zero order assembler";
  }


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

    std::vector<double> c(1, 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[0] *= elInfo->getDet();
    
    if (symmetric) {
      for (int i = 0; i < nRow; i++) {
	mat[i][i] += c[0] * q00->getValue(i,i);
	for (int j = i + 1; j < nCol; j++) {
	  double val = c[0] * 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[0] * q00->getValue(i, j);
	}
      }
    }
  }


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

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

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

    c[0] *= elInfo->getDet();

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

}