#include <iostream>
#include <fstream>
#include <boost/lexical_cast.hpp>
#include "Debug.h"
#include "DOFVector.h"
#include "MacroElement.h"
#include "VtkWriter.h"
#include "ElementFileWriter.h"
#include "ElementDofIterator.h"

namespace AMDiS {

  namespace debug {

#ifdef HAVE_PARALLEL_DOMAIN_AMDIS
    void writeLocalElementDofs(int rank, int elIdx, FiniteElemSpace *feSpace)
    {
      using boost::lexical_cast;
      
      if (MPI::COMM_WORLD.Get_rank() == rank) {
	DOFVector<double> tmp(feSpace, "tmp");
	colorDofVectorByLocalElementDofs(tmp, feSpace->getMesh(), elIdx);
	VtkWriter::writeFile(tmp, "tmp" + lexical_cast<std::string>(elIdx) + ".vtu");
      }
    }

    
    void writeDofMesh(int rank, DegreeOfFreedom dof, FiniteElemSpace *feSpace)
    {
      using boost::lexical_cast;

      if (MPI::COMM_WORLD.Get_rank() == rank) {
	DOFVector<double> tmp(feSpace, "tmp");
	tmp.set(0.0);
	tmp[dof] = 1.0;
	VtkWriter::writeFile(tmp, "dofmesh" + lexical_cast<std::string>(rank) + ".vtu");
      }    
    }

    
    void writeMesh(FiniteElemSpace *feSpace, int rank, std::string filename)
    {
      using boost::lexical_cast;
      
      int myRank = MPI::COMM_WORLD.Get_rank();
      if (rank == -1 || myRank == rank) {
	DOFVector<double> tmp(feSpace, "tmp");
	VtkWriter::writeFile(tmp, filename + lexical_cast<std::string>(myRank) + ".vtu", 
			     false);
      }
    }
#endif

    
    void writeDofIndexMesh(FiniteElemSpace *feSpace)
    {
      DOFVector<double> tmp(feSpace, "tmp");
      DOFIterator<double> it(&tmp, USED_DOFS);
      for (it.reset(); !it.end(); ++it)
	*it = it.getDOFIndex();
      VtkWriter::writeFile(tmp, "dofindex.vtu");
    }


    void writeElementIndexMesh(Mesh *mesh, std::string filename)
    {
      std::map<int, double> vec;    
      TraverseStack stack;
      ElInfo *elInfo = stack.traverseFirst(mesh, -1, Mesh::CALL_LEAF_EL);
      
      while (elInfo) {		  
	int index = elInfo->getElement()->getIndex();
	vec[index] = index;
	elInfo = stack.traverseNext(elInfo);
      }

      ElementFileWriter::writeFile(vec, mesh, filename);
    }


    void highlightElementIndexMesh(Mesh *mesh, int idx, std::string filename)
    {
      std::map<int, double> vec;    
      TraverseStack stack;
      ElInfo *elInfo = stack.traverseFirst(mesh, -1, Mesh::CALL_EVERY_EL_PREORDER);
      bool markChildren = false;
      int markLevel = -1;

      while (elInfo) {		  
	if (markChildren && elInfo->getLevel() <= markLevel)
	  markChildren = false;

	int index = elInfo->getElement()->getIndex();
	if (index == idx) {
	  markChildren = true;
	  markLevel = elInfo->getLevel();
	}
  
	if (elInfo->getElement()->isLeaf())
	  vec[index] = (markChildren ? 1.0 : 0.0);

	elInfo = stack.traverseNext(elInfo);
      }

      ElementFileWriter::writeFile(vec, mesh, filename);
    }


    void colorDofVectorByLocalElementDofs(DOFVector<double>& vec, Element *el)
    {
      // === Get local indices of the given element. ===
      
      const BasisFunction *basisFcts = vec.getFeSpace()->getBasisFcts();
      int nBasisFcts = basisFcts->getNumber();
      std::vector<DegreeOfFreedom> localDofs(nBasisFcts);
      basisFcts->getLocalIndices(el, vec.getFeSpace()->getAdmin(), localDofs);
      
      // === Set the values of the dof vector. ===
      
      vec.set(0.0);
      for (int i = 0; i < nBasisFcts; i++)
	vec[localDofs[i]] = static_cast<double>(i);
    }

    
    bool colorDofVectorByLocalElementDofs(DOFVector<double>& vec, Mesh *mesh, 
					  int elIndex)
    {
      FUNCNAME("debug::colorDofVectorByLocalElementDofs()");
      
      TraverseStack stack;
      ElInfo *elInfo = stack.traverseFirst(mesh, -1, Mesh::CALL_LEAF_EL);
      while (elInfo) {
	if (elInfo->getElement()->getIndex() == elIndex) {
	  colorDofVectorByLocalElementDofs(vec, elInfo->getElement());
	  return true;
	}
	elInfo = stack.traverseNext(elInfo);
      }
      
      return false;
    }

    
    Element* getDofIndexElement(FiniteElemSpace *feSpace, DegreeOfFreedom dof)
    {
      const BasisFunction* basFcts = feSpace->getBasisFcts();
      int nBasFcts = basFcts->getNumber();
      std::vector<DegreeOfFreedom> dofVec(nBasFcts);
      
      TraverseStack stack;
      ElInfo *elInfo = stack.traverseFirst(feSpace->getMesh(), -1, Mesh::CALL_LEAF_EL);
      while (elInfo) {
	basFcts->getLocalIndices(elInfo->getElement(), feSpace->getAdmin(), dofVec);
	for (int i = 0; i < nBasFcts; i++) 
	  if (dofVec[i] == dof)
	    return elInfo->getElement();	  
	
	elInfo = stack.traverseNext(elInfo);
      }
      
      return NULL;
    }

    
    Element* getLevel0ParentElement(Mesh *mesh, Element *el)
    {    
      TraverseStack stack;
      ElInfo *elInfo = stack.traverseFirst(mesh, -1, Mesh::CALL_EVERY_EL_PREORDER);
      while (elInfo) {
	if (elInfo->getElement() == el)
	  return elInfo->getMacroElement()->getElement();      
	
	elInfo = stack.traverseNext(elInfo);
      }
      
      return NULL;
    }


    Element* getParentElement(Mesh *mesh, Element *el)
    {
      TraverseStack stack;
      ElInfo *elInfo = stack.traverseFirst(mesh, -1, Mesh::CALL_EVERY_EL_PREORDER);
      while (elInfo) {
	if (elInfo->getElement()->getChild(0) == el || 
	    elInfo->getElement()->getChild(1) == el)
	  return elInfo->getElement();      
	
	elInfo = stack.traverseNext(elInfo);
      }
      
      return NULL;

    }


    Element* getParentElement(Mesh *mesh, int elIndex)
    {
      TraverseStack stack;
      ElInfo *elInfo = stack.traverseFirst(mesh, -1, Mesh::CALL_EVERY_EL_PREORDER);
      while (elInfo) {
	if (elInfo->getElement()->isLeaf() == false) 
	  if (elInfo->getElement()->getChild(0)->getIndex() == elIndex || 
	      elInfo->getElement()->getChild(1)->getIndex() == elIndex)
	    return elInfo->getElement();
	
	elInfo = stack.traverseNext(elInfo);
      }
      
      return NULL;

    }


    void printElementInfo(Element *el)
    {
      FUNCNAME("printElementInfo()");
      
      if (el->isLeaf()) {
	MSG("el %d is leaf!\n", el->getIndex());
      } else {
	MSG("el child0 %d    child1 %d\n", 
	    el->getChild(0)->getIndex(),
	    el->getChild(1)->getIndex());
	printElementInfo(el->getChild(0));
	printElementInfo(el->getChild(1));
      }
    }

    
    void printInfoByDof(FiniteElemSpace *feSpace, DegreeOfFreedom dof)
    {
      FUNCNAME("debug::printInfoByDof()");

      Element *el = getDofIndexElement(feSpace, dof);
      Element *parEl = getLevel0ParentElement(feSpace->getMesh(), el);
      
      MSG("[DBG] DOF-INFO:  dof = %d  elidx = %d  pelidx = %d\n", 
	  dof, el->getIndex(), parEl->getIndex());
      
      TraverseStack stack;
      ElInfo *elInfo = stack.traverseFirst(feSpace->getMesh(), -1, 
					   Mesh::CALL_EVERY_EL_PREORDER);
      while (elInfo) {
	if (elInfo->getElement()->getIndex() == parEl->getIndex()) {
	  MSG("[DBG] EL INFO TO %d: type = %d\n", parEl->getIndex(), elInfo->getType());
	}

	elInfo = stack.traverseNext(elInfo);
      }	  
    }


    void printMatValuesStatistics(Matrix<DOFMatrix*> *mat)
    {
      std::map<int, int> counter;
      int counter0 = 0;

      for (int i = 0; i < mat->getNumRows(); i++) {
	for (int j = 0; j < mat->getNumCols(); j++) {
	  DOFMatrix *dofMat = (*mat)[i][j];
	  if (dofMat) {

	    using mtl::tag::major; using mtl::tag::nz; using mtl::begin; using mtl::end;
	    namespace traits = mtl::traits;
	    typedef DOFMatrix::base_matrix_type base_matrix_type;
	    
	    traits::const_value<base_matrix_type>::type value(dofMat->getBaseMatrix());
	    typedef traits::range_generator<major, base_matrix_type>::type cursor_type;
	    typedef traits::range_generator<nz, cursor_type>::type icursor_type;
    
	    for (cursor_type cursor = begin<major>(dofMat->getBaseMatrix()), 
		   cend = end<major>(dofMat->getBaseMatrix()); cursor != cend; ++cursor)
	      for (icursor_type icursor = begin<nz>(cursor), 
		     icend = end<nz>(cursor); icursor != icend; ++icursor) {
		if (value(*icursor) == 0.0) {
		  counter0++;
		} else {
		  int a = static_cast<int>(floor(log10(fabs(value(*icursor)))));
		  counter[a]++;
		}
	      }
		
	  }
	}
      }

      std::cout << "value = 0: " << counter0 << std::endl;
      for (std::map<int, int>::iterator it = counter.begin(); it != counter.end(); ++it)
	std::cout << pow(10, it->first) << " <= values <= " 
		  << pow(10, it->first + 1) << ": " 
		  << it->second << std::endl;
    }


    void printAllDofCoords(FiniteElemSpace *feSpace)
    {
      FUNCNAME("printAllDofCoords()");

      TraverseStack stack;
      ElInfo *elInfo = stack.traverseFirst(feSpace->getMesh(), -1, 
					   Mesh::CALL_LEAF_EL | Mesh::FILL_COORDS);
      while (elInfo) {
	Element *el = elInfo->getElement();
	for (int i = 0; i <= feSpace->getMesh()->getDim(); i++) {
	  MSG("Coords for DOF %d = %f %f\n", 
	      *(el->getDOF(i)), (elInfo->getCoord(i))[0], (elInfo->getCoord(i))[1]);
	}
	elInfo = stack.traverseNext(elInfo);
      }      
    }


    void getAllDofs(FiniteElemSpace *feSpace, std::set<const DegreeOfFreedom*>& dofs)
    {
      FUNCNAME("getAllDofs()");

      ElementDofIterator elDofIter(feSpace);
      TraverseStack stack;
      ElInfo *elInfo = stack.traverseFirst(feSpace->getMesh(), -1, Mesh::CALL_LEAF_EL);
      while (elInfo) {	
	elDofIter.reset(elInfo->getElement());
	do {
	  dofs.insert(elDofIter.getDofPtr());
	} while (elDofIter.next());
	elInfo = stack.traverseNext(elInfo);
      }      
    }


    void writeMatlabMatrix(DOFMatrix &mat, std::string filename)
    {
      using mtl::tag::major; using mtl::tag::nz; using mtl::begin; using mtl::end;
      namespace traits = mtl::traits;
      typedef DOFMatrix::base_matrix_type Matrix;

      traits::row<Matrix>::type row(mat.getBaseMatrix());
      traits::col<Matrix>::type col(mat.getBaseMatrix());
      traits::const_value<Matrix>::type value(mat.getBaseMatrix());

      typedef traits::range_generator<major, Matrix>::type cursor_type;
      typedef traits::range_generator<nz, cursor_type>::type icursor_type;

      std::ofstream out;
      out.open(filename.c_str());
    
      for (cursor_type cursor = begin<major>(mat.getBaseMatrix()), 
	     cend = end<major>(mat.getBaseMatrix()); cursor != cend; ++cursor)
	for (icursor_type icursor = begin<nz>(cursor), icend = end<nz>(cursor); icursor != icend; ++icursor)
	  out << row(*icursor) + 1 << " " 
	      << col(*icursor) + 1 << " " 
	      << value(*icursor) << "\n";

      out.close();
    }


    void writeMatlabMatrix(Matrix<DOFMatrix*> &mat, std::string filename)
    {
      using mtl::tag::major; using mtl::tag::nz; using mtl::begin; using mtl::end;
      namespace traits = mtl::traits;
      typedef DOFMatrix::base_matrix_type Matrix;

      std::ofstream out;
      out.open(filename.c_str());

      for (int i = 0; i < mat.getNumRows(); i++) {
	for (int j = 0; j < mat.getNumCols(); j++) {
	  if (!mat[i][j])
	    continue;

	  traits::row<Matrix>::type row(mat[i][j]->getBaseMatrix());
	  traits::col<Matrix>::type col(mat[i][j]->getBaseMatrix());
	  traits::const_value<Matrix>::type value(mat[i][j]->getBaseMatrix());
	  
	  typedef traits::range_generator<major, Matrix>::type cursor_type;
	  typedef traits::range_generator<nz, cursor_type>::type icursor_type;	  

	  for (cursor_type cursor = begin<major>(mat[i][j]->getBaseMatrix()), 
		 cend = end<major>(mat[i][j]->getBaseMatrix()); cursor != cend; ++cursor)
	    for (icursor_type icursor = begin<nz>(cursor), icend = end<nz>(cursor); icursor != icend; ++icursor)
	      out << i * num_rows(mat[i][j]->getBaseMatrix()) + row(*icursor) + 1 << " " 
		  << j * num_cols(mat[i][j]->getBaseMatrix()) + col(*icursor) + 1 << " " 
		  << value(*icursor) << "\n";
	}
      }
    
      out.close();
    }


    void writeMatlabVector(DOFVector<double> &vec, std::string filename)
    {
      std::ofstream out;    
      out.open(filename.c_str());

      DOFIterator<double> it(&vec, USED_DOFS);
      for (it.reset(); !it.end(); ++it)
	out << *it << "\n";

      out.close();      
    }

    
    void writeMatlabVector(SystemVector &vec, std::string filename)
    {
      std::ofstream out;    
      out.open(filename.c_str());

      for (int i = 0; i < vec.getSize(); i++) {
	DOFIterator<double> it(vec.getDOFVector(i), USED_DOFS);
	for (it.reset(); !it.end(); ++it)
	  out << *it << "\n";
      }

      out.close();            
    }


    void writeCoordsFile(const FiniteElemSpace* feSpace, std::string filename)
    {
      FUNCNAME("debug::writeCoordsFile()");

      DOFVector<WorldVector<double> > coords(feSpace, "tmp");
      feSpace->getMesh()->getDofIndexCoords(feSpace, coords);

      std::ofstream file;
      file.open(filename.c_str());
      file << coords.getUsedSize() << "\n";
      DOFIterator<WorldVector<double> > it(&coords, USED_DOFS);
      for (it.reset(); !it.end(); ++it) {
	file << it.getDOFIndex();
	for (int i = 0; i < feSpace->getMesh()->getDim(); i++)
	  file << " " << (*it)[i];
	file << "\n";
      }
      file.close();
    }


    void printElementHierarchie(Mesh *mesh, int elIndex)
    {
      FUNCNAME("debug::printElementHierarchie()");

      bool printInfo = false;
      int elLevel = -1;

      TraverseStack stack;
      ElInfo *elInfo = stack.traverseFirst(mesh, -1, Mesh::CALL_EVERY_EL_PREORDER);
      while (elInfo) {
	if (printInfo && elInfo->getLevel() <= elLevel)
	  return;

	if (elInfo->getElement()->getIndex() == elIndex) {
	  printInfo = true;
	  elLevel = elInfo->getLevel();
	  MSG(" Hierarchie for elIdx = %d\n", elIndex);
	} else {
	  if (printInfo) {
	    std::stringstream oss;
	    for (int i = 0; i < (elInfo->getLevel() - elLevel); i++)
	      oss << "   ";
	    oss << "|--" << elInfo->getElement()->getIndex();
	    MSG("%s\n", oss.str().c_str());
	  }
	}

	elInfo = stack.traverseNext(elInfo);
      }
    }


    int getLocalNeighbourIndex(Mesh *mesh, int elIndex, int neighIndex)
    {
      FUNCNAME("debug::getLocalNeighbourIndex()");

      TraverseStack stack;
      ElInfo *elInfo = 
	stack.traverseFirst(mesh, -1, Mesh::CALL_EVERY_EL_PREORDER | Mesh::FILL_NEIGH);
      while (elInfo) {
	if (elInfo->getElement()->getIndex() == elIndex) {
	  int returnValue = -1;
	  for (int i = 0; i <= mesh->getDim(); i++) {
	    if (elInfo->getNeighbour(i)) {
	      MSG("NEIGH %d of El-Idx %d: %d\n", i, elIndex, elInfo->getNeighbour(i)->getIndex());
	    } else {
	      MSG("NEIGH %d of El-Idx %d: %d\n", i, elIndex, -1);
	    }
	    
	    if (elInfo->getNeighbour(i) && 
		elInfo->getNeighbour(i)->getIndex() == neighIndex)
	      returnValue = i;
	  }

	  return returnValue;	  
	}
	elInfo = stack.traverseNext(elInfo);
      }      

      return -2;
    }


    void createSortedDofs(Mesh *mesh, ElementIdxToDofs &elMap)
    {
      FUNCNAME("debug::dbgCreateElementMap()");
      
      int dim = mesh->getDim();
      elMap.clear();
      
      TraverseStack stack;
      ElInfo *elInfo = stack.traverseFirst(mesh, -1, Mesh::CALL_LEAF_EL);
      while (elInfo) {
	Element *el = elInfo->getElement();
	switch (dim) {
	case 2:
	  sortDofs(el->getDOF(0), el->getDOF(1), el->getDOF(2), elMap[el->getIndex()]);
	  break;
	case 3:
	  sortDofs(el->getDOF(0), el->getDOF(1), el->getDOF(2), el->getDOF(3), elMap[el->getIndex()]);
	  break;
	default:
	  ERROR_EXIT("What is this?\n");
	}
	elInfo = stack.traverseNext(elInfo);
      }
    }


    void testSortedDofs(Mesh *mesh, ElementIdxToDofs &elMap)
    {
      FUNCNAME("debug::dbgTestElementMap()");
      
      int dim = mesh->getDim();
      int nVertex = Global::getGeo(VERTEX, dim);
      DofContainer vec(nVertex);
      
      TraverseStack stack;
      ElInfo *elInfo = stack.traverseFirst(mesh, -1, Mesh::CALL_LEAF_EL);
      while (elInfo) {
	Element *el = elInfo->getElement();
	
	switch (dim) {
	case 2:
	  sortDofs(el->getDOF(0), el->getDOF(1), el->getDOF(2), vec);
	  break;
	case 3:
	  sortDofs(el->getDOF(0), el->getDOF(1), el->getDOF(2), el->getDOF(3), vec);
	  break;
	default:
	  ERROR_EXIT("What is this?\n");
	}
	
	for (int i = 0; i < nVertex; i++) {
	  if (elMap[el->getIndex()][i] != vec[i]) {
	    MSG("[DBG] Wrong new dof numeration in element = %d\n!", el->getIndex());
	    std::cout << "[DBG]: Old numeration was: ";
	    for (int j = 0; j < nVertex; j++)
	      std::cout << elMap[el->getIndex()][j] << " = " 
			<< *(elMap[el->getIndex()][j]) << "  ";
	    std::cout << std::endl;
	    std::cout << "[DBG]: New numeration is:  ";
	    for (int j = 0; j < nVertex; j++)
	      std::cout << vec[j] << " = "  << *(vec[j]) << "  ";
	    std::cout << std::endl;
	    ERROR_EXIT("WRONG NEW DOF NUMERATION!\n");
	  }
	}
	elInfo = stack.traverseNext(elInfo);
      }
    }


    void sortDofs(const DegreeOfFreedom* dof0,
		  const DegreeOfFreedom* dof1,
		  const DegreeOfFreedom* dof2,
		  DofContainer &vec)
    {
      DofPtrSortFct dofPtrSort;
      vec.resize(3);
      vec[0] = dof0; 
      vec[1] = dof1; 
      vec[2] = dof2;
      sort(vec.begin(), vec.end(), dofPtrSort);
    }

    void sortDofs(const DegreeOfFreedom* dof0,
		  const DegreeOfFreedom* dof1,
		  const DegreeOfFreedom* dof2,
		  const DegreeOfFreedom* dof3,
		  DofContainer &vec)
    {
      DofPtrSortFct dofPtrSort;
      vec.resize(4);
      vec[0] = dof0; 
      vec[1] = dof1; 
      vec[2] = dof2;
      vec[3] = dof3;
      sort(vec.begin(), vec.end(), dofPtrSort);
    }


  } // namespace debug
  
} // namespace AMDiS