#include <fstream>

#include "FixVec.h"
#include "DOFVector.h"
#include "BasisFunction.h"
#include "Mesh.h"

namespace AMDiS {

  template<typename T>
  void GridWriter<T>::writeGrid(const WorldVector<double> *p,
				int               *numPoints, 
				double            *dist,
				DOFVector<T>      *vec,
				char*              filename)
  {
    FUNCNAME("GridWriter<T>::writeGrid()");
 
    std::ofstream *outFile = new std::ofstream(filename);
    TEST_EXIT(outFile)("can't open file %s\n", filename);

    const Mesh *mesh = vec->getFESpace()->getMesh();
    int dim = mesh->getDim();

    TEST_EXIT(dim == Global::getGeo(WORLD))
      ("not for DIM != DIM_OF_WORLD\n");


    WorldVector<double> *basis       = NEW WorldVector<double>[dim];
    double      *lengthBasis = GET_MEMORY(double, dim);
    WorldVector<double> *step        = NEW WorldVector<double>[3];

    for (int i = 0; i < dim; i++) {
      TEST_EXIT(numPoints[i] > 0)("numPoints < 1\n");
      TEST_EXIT(dist[i] >= 0)("dist < 0\n");
      lengthBasis[i] = 0;
    }

    WorldVector<double>          curCoord;
    DimVec<double>       bary(dim, NO_INIT);
    Element             *elp;
    const BasisFunction *basFcts = NULL;
    int                  inside;
    double               value;
    const double        *uhLoc;

    TEST_EXIT(vec)("no dof vector\n");
    TEST_EXIT(filename)("no filename\n");
    TEST_EXIT(vec->getFESpace()->getBasisFcts())("no basis fcts\n");

    mesh = vec->getFESpace()->getMesh();
    basFcts = vec->getFESpace()->getBasisFcts();

    /* get basis of grid */
    for (int i = 0; i < dim; i++) {
      for (int j = 0; j < dim; j++) {
	basis[i][j] = p[i+1][j] - p[0][j];
	lengthBasis[i] += basis[i][j] * basis[i][j];
      }
      lengthBasis[i] = sqrt(lengthBasis[i]);
    }

    /* norm basis, get steps */
    for (int i = 0; i < dim; i++) {
      for (int j = 0; j < dim; j++) {
	basis[i][j] /= lengthBasis[i];
	step[i][j] = basis[i][j] * dist[j];
      }
    }

    /* write grid points */
    int    localNumPoints[3] = {1, 1, 1};
    for (int i = 0; i < dim; i++) {
      localNumPoints[i] = numPoints[i];
    }

    // Warning "Coords not in mesh domain" should be printed at most one time.
    bool warning = false;

    for (int i = 0; i < localNumPoints[0]; i++) {
      for (int j = 0; j < localNumPoints[1]; j++) {   // pseudo-loop for dim < 2
	for (int k = 0; k < localNumPoints[2]; k++) { // pseudo-loop for dim < 3   
	  /* clac current coords */
	  for (int l = 0; l < dim; l++) {
	    curCoord[l] = p[0][l] 
	      + (i * step[0][l])
	      + (j * step[1][l])     // j = 0 for dim > 1
	      + (k * step[2][l]);    // k = 0 for dim > 2
	  }

	  inside = 
	    (const_cast<Mesh*>(mesh))->findElementAtPoint(curCoord, 
							  &elp, 
							  bary, 
							  NULL, NULL, NULL);

	  /* write coords */
	  for (int l = 0; l < dim; l++) {
	    (*outFile) << curCoord[l] << " ";
	  }

	  if (!inside) {
	    if (!warning) {
	      WARNING("Coords not in mesh domain\n");
	      warning = true;
	    }
	    /* write value */
	    (*outFile) << "0.0" << std::endl;
	  } else {
	    /* get value at coords */
	    uhLoc = vec->getLocalVector(elp, NULL);
	    value = basFcts->evalUh(bary, uhLoc);
	  
	    /* write value */
	    (*outFile) << value << std::endl;
	  }
	}
	if (localNumPoints[2] > 1) (*outFile) << std::endl;
      }
      if (localNumPoints[1] > 1) (*outFile) << std::endl;
    }
  
    DELETE [] basis;
    FREE_MEMORY(lengthBasis, double, dim);
    DELETE [] step;
    delete outFile;
  }

}