/******************************************************************************
 *
 * Extension of 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 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.
 *
 *
 * See also license.opensource.txt in the distribution.
 * 
 ******************************************************************************/


#include "Helpers.h"

namespace AMDiS { namespace extensions {

namespace Helpers {

  void calcMaxOnXAxis(DOFVector<double> *rho, std::vector<std::pair<WorldVector<double>, double> > &maxima)
  {
    std::vector<WorldVector<double> > x;
    std::vector<double> y;

    double dimX=1.0,dimY=1.0;
    Parameters::get("mesh->dimension[x]",dimX);
    Parameters::get("mesh->dimension[y]",dimY);

    interpolOverLine(*rho,-dimX,0.0,dimX,0.0,1000,x,y);
    if (x.size() != y.size())
      throw(std::runtime_error("x.size /= y.size! Sollte nicht passieren!"));

    if(x.size()>0) {
      for(unsigned i=1; i<x.size()-1; ++i) {
	if(y[i-1]<y[i] && y[i+1]<y[i]) {
	  maxima.push_back(std::make_pair(x[i],y[i]));
	}
      }
    }
  }

  
  void calcMaxOnYAxis(DOFVector<double> *rho, std::vector<std::pair<WorldVector<double>, double> > &maxima)
  {

    std::vector<WorldVector<double> > x;
    std::vector<double> y;

    double dimX,dimY;
    Parameters::get("mesh->dimension[x]",dimX);
    Parameters::get("mesh->dimension[y]",dimY);

    interpolOverLine(*rho,0.0,-dimY,0.0,dimY,1000,x,y);

    #ifdef HAVE_PARALLEL_DOMAIN_AMDIS
    if (MPI::COMM_WORLD.Get_rank() == 0)
    #endif
    {
    for(unsigned i=1; i<x.size()-1; ++i) {
      if(y[i-1]<y[i] && y[i+1]<y[i]) {
	maxima.push_back(make_pair(x[i],y[i]));
      }
    }
    }
  }


  void transformMesh(Mesh *mesh, WorldVector<double> scale, WorldVector<double> shift, WorldVector<double> rotate)
  {

    int dow = Global::getGeo(WORLD);
    mtl::dense2D<double> Rx(dow,dow),Ry(dow,dow),Rz(dow,dow),T_(dow,dow),S(dow,dow);
    Rx = 1.0;
    Ry = 1.0;
    Rz = 1.0;
    S = 1.0;
    if (dow == 3) {
      Rx[1][1] = std::cos(rotate[0]);  Rx[1][2] = -std::sin(rotate[0]);
      Rx[2][1] = std::sin(rotate[0]);  Rx[2][2] = std::cos(rotate[0]);

      Ry[0][0] = std::cos(rotate[1]);  Ry[0][2] = std::sin(rotate[1]);
      Ry[2][0] = -std::sin(rotate[1]); Ry[2][2] = std::cos(rotate[1]);

      Rz[0][0] = std::cos(rotate[2]);  Rz[0][1] = -std::sin(rotate[2]);
      Rz[1][0] = std::sin(rotate[2]);  Rz[1][1] = std::cos(rotate[2]);

      S[0][0] = scale[0];  S[1][1] = scale[1];  S[2][2] = scale[2];
    } else if (dow == 2) {
      Rx[0][0] = std::cos(rotate[0]);  Rx[0][1] = -std::sin(rotate[0]);
      Rx[1][0] = std::sin(rotate[0]);  Rx[1][1] = std::cos(rotate[0]);

      Ry = 1.0;
      Rz = 1.0;

      S[0][0] = scale[0];  S[1][1] = scale[1];
    } else {
      S[0][0] = scale[0];
    }

    T_ = Rz*Ry*Rx*S;

    WorldMatrix<double> T;
    vector_operations::fillMatrix(T_,T);

    FixVec<WorldVector<double>, VERTEX> coords;
    deque<MacroElement*>::iterator macro;

    for(macro = mesh->firstMacroElement(); macro != mesh->endOfMacroElements(); macro++) {
      for (int i = 0; i < mesh->getDim()+1; ++i) {
	WorldVector<double> x = (*macro)->getCoord(i);
	WorldVector<double> x_temp = x;
	x.multMatrixVec(T,x_temp);
	x+= shift;
	(*macro)->setCoord(i, x);
      }
    }
  }

  
  void scaleMesh(Mesh *mesh, WorldVector<double> shift, WorldVector<double> scale)
  {
    deque<MacroElement*>::iterator macro;

    for(macro = mesh->firstMacroElement(); macro != mesh->endOfMacroElements(); macro++) {
      for (int i = 0; i < mesh->getDim()+1; ++i) {
	WorldVector<double> x = (*macro)->getCoord(i);
	x *= scale;
	x += shift;
	(*macro)->setCoord(i, x);
      }
    }
  }

  
  // scale by different values in all directions
  void scaleMesh(Mesh *mesh, WorldVector<double> scale)
  {
    WorldVector<double> shift; shift.set(0.0);
    scaleMesh(mesh,shift,scale);
  }
  

  // scale by different values in all directions
  void scaleMesh(std::vector<Mesh*> meshes, WorldVector<double> scale)
  {
    WorldVector<double> shift; shift.set(0.0);
    for (size_t i = 0; i < meshes.size(); i++)
      scaleMesh(meshes[i],shift,scale);
  }
  

  // scale and shift by the same values in all directions
  void scaleMesh(Mesh *mesh, double shift, double scale)
  {
    WorldVector<double> wShift, wScale;
    wShift.set(shift);
    wScale.set(scale);
    scaleMesh(mesh,wShift,wScale);
  }
  

  void read_dof_vector(const std::string file, DOFVector<double> *dofvec, long size)
  { FUNCNAME("Helpers::read_dof_vector()");

    ifstream in;
    string buf;
    long i;

    in.open(file.c_str());
    TEST_EXIT(in)("Could not read value file.\n");

    DOFIterator<double> dofIter(dofvec, USED_DOFS);

    getline(in, buf);
    while(buf.substr(0,14) != "vertex values:") getline(in, buf);
    for (i = 0, dofIter.reset(); !dofIter.end(); ++i, ++dofIter){
      in >> buf;
      (*dofIter) = atof(buf.c_str());
    }
    in.close();

    TEST_EXIT(i == size)("#vertices != dofIter.size()\n");
  }

  
  WorldVector<double> getMeshDimension(Mesh *mesh)
  {
    WorldVector<double> dimension; dimension.set(-1.e10);

    TraverseStack stack;
    ElInfo *elInfo = stack.traverseFirst(mesh, 0, Mesh::CALL_EL_LEVEL | Mesh::FILL_COORDS);
    while (elInfo) {
      for (int i = 0; i <= mesh->getDim(); i++) {
	WorldVector<double> &coords = elInfo->getMacroElement()->getCoord(i);
	for(int j = 0; j < coords.getSize(); ++j) {
	  dimension[j] = std::max(dimension[j], coords[j]);
	}
      }
      elInfo = stack.traverseNext(elInfo);
    }
    return dimension;
  }

  
  void getMeshDimension(Mesh *mesh, WorldVector<double> &min_corner, WorldVector<double> &max_corner)
  {
    max_corner.set(-1.e10);
    min_corner.set(1.e10);

    TraverseStack stack;
    ElInfo *elInfo = stack.traverseFirst(mesh, 0, Mesh::CALL_EL_LEVEL | Mesh::FILL_COORDS);
    while (elInfo) {
      for (int i = 0; i <= mesh->getDim(); i++) {
	WorldVector<double> &coords = elInfo->getMacroElement()->getCoord(i);
	for(int j = 0; j < coords.getSize(); ++j) {
	  max_corner[j] = std::max(max_corner[j], coords[j]);
	  min_corner[j] = std::min(min_corner[j], coords[j]);
	}
      }
      elInfo = stack.traverseNext(elInfo);
    }
  }

  
  void getNormalsWeighted(FiniteElemSpace *feSpace, DOFVector<WorldVector<double> > *normals)
  {
    Mesh *mesh = feSpace->getMesh();
    const BasisFunction *basisFcts = feSpace->getBasisFcts();
    int nBasisFcts = basisFcts->getNumber();
    
    std::vector<DegreeOfFreedom> localIndices(nBasisFcts);
    
    DOFVector<double> numNormals(feSpace, "numNormals");
    numNormals.set(0.0);
      
    WorldVector<double> normal; normal.set(0.0);
    normals->set(normal);
    TraverseStack stack;
    ElInfo *elInfo = 
      stack.traverseFirst(mesh, -1, 
			  Mesh::CALL_LEAF_EL | Mesh::FILL_COORDS);
      
    while (elInfo) {
      elInfo->getElementNormal(normal);
      Element *el = elInfo->getElement();
      double det = elInfo->getDet();
      if (det <= DBL_TOL)
	det = elInfo->calcDet();
      
      basisFcts->getLocalIndices(el, feSpace->getAdmin(), localIndices);
      
      for (int i = 0; i < nBasisFcts; i++) {
	(*normals)[localIndices[i]] += normal*(1.0/det);
      }

      elInfo = stack.traverseNext(elInfo);
    }
      
    DOFIterator<WorldVector<double> > normalsIter(normals, USED_DOFS); normalsIter.reset();
    for (; !normalsIter.end(); normalsIter++)
    {
      *normalsIter *= 1.0/norm(*normalsIter);
    }
  }

  
  void getNormals(FiniteElemSpace *feSpace, DOFVector<WorldVector<double> > *normals, DOFVector<WorldMatrix<double> > *gradNormals)
  {
    using namespace mtl;
    using namespace itl;
    
    Mesh *mesh = feSpace->getMesh();
    const BasisFunction *basisFcts = feSpace->getBasisFcts();
    size_t nBasisFcts = basisFcts->getNumber();

    std::vector<DegreeOfFreedom> localIndices(nBasisFcts);
    DOFVector<std::set<DegreeOfFreedom> > neighbors(feSpace, "neighbors");

    TraverseStack stack;
    ElInfo *elInfo =
      stack.traverseFirst(mesh, -1,
			  Mesh::CALL_LEAF_EL | Mesh::FILL_COORDS);

    while (elInfo) {
      basisFcts->getLocalIndices(elInfo->getElement(), feSpace->getAdmin(), localIndices);

      for (size_t i = 0; i < nBasisFcts; i++) {
	for (size_t j = 0; j < nBasisFcts; j++) {
	  neighbors[localIndices[i]].insert(localIndices[j]);
	}
      }
      elInfo = stack.traverseNext(elInfo);
    }

    DOFVector<WorldVector<double> > coordsDOF(feSpace, "coords");
    mesh->getDofIndexCoords(coordsDOF);

    Helpers::getNormalsWeighted(feSpace, normals);
    AMDiS::io::VtkWriter::writeFile(normals, "normals_weighted.vtu");

  //   DOFVector<WorldMatrix<double> > *grdDOF = gradNormals;
  //   if (grdDOF == NULL)
  //     grdDOF = new DOFVector<WorldMatrix<double> >(feSpace, "grd");

    DOFIterator<WorldVector<double> > normalsIter(normals, USED_DOFS); normalsIter.reset();
    DOFIterator<WorldVector<double> > coordsIter(&coordsDOF, USED_DOFS); coordsIter.reset();
  /*  DOFIterator<WorldMatrix<double> > grdIter(grdDOF, USED_DOFS); grdIter.reset();*/
    DOFIterator<std::set<DegreeOfFreedom> > neighborsIter(&neighbors, USED_DOFS); neighborsIter.reset();
    for (; !neighborsIter.end(); neighborsIter++, normalsIter++, coordsIter++/*, grdIter++*/)
    {
      size_t numPoints = 10; // 6 oder 10
      std::set<DegreeOfFreedom> idx = *neighborsIter;
      std::set<DegreeOfFreedom>::iterator it, it2;

      for (size_t i = 1; i<3 && idx.size() < numPoints; i++) {
      for (it = idx.begin(); it != idx.end() && idx.size() < numPoints; it++) {
	for (it2 = (neighbors[*it]).begin(); it2 != (neighbors[*it]).end() && idx.size() < numPoints; it2++) {
	  idx.insert(*it2);
	}
      }
      }
      TEST_EXIT(idx.size()>=numPoints)("Nicht genügend Stützstellen gefunden!\n");
      size_t i = 0;
      mtl::dense2D<double> A(numPoints,6), B(6,6);
      mtl::dense_vector<double> b(numPoints, 1.0), c(6), y(6, 1.0);

      for (it = idx.begin(); it != idx.end() && i < numPoints; it++, i++) {
	WorldVector<double> x = coordsDOF[*it];
	A[i][0] = sqr(x[0]); A[i][1] = sqr(x[1]); A[i][2] = sqr(x[2]);
	A[i][3] = x[0]*x[1]; A[i][4] = x[0]*x[2]; A[i][5] = x[1]*x[2];
  //       if(numPoints>6)
  // 	A[i][6] = x[0]; A[i][7] = x[1]; A[i][8] = x[2]; A[i][9] = 1.0;
      }
      c = trans(A)*b;
      B = trans(A)*A;
  //     pc::ilu_0<mtl::dense2D<double> > P(B);
  //     basic_iteration<double>          iter(c, 100, 1.e-6);

  //     Solve Ax == b with left preconditioner P
  //     bicgstab(B, y, c, P, iter);

      y = mtl::matrix::lu_solve(B,c);

      WorldVector<double> oldNormal = (*normalsIter);

      (*normalsIter)[0] = 2.0*y[0]*(*coordsIter)[0] + y[3]*(*coordsIter)[1] + y[4]*(*coordsIter)[2]/* + (numPoints==10?y[6]:0.0)*/;
      (*normalsIter)[1] = 2.0*y[1]*(*coordsIter)[1] + y[3]*(*coordsIter)[0] + y[5]*(*coordsIter)[2]/* + (numPoints==10?y[7]:0.0)*/;
      (*normalsIter)[2] = 2.0*y[2]*(*coordsIter)[2] + y[4]*(*coordsIter)[0] + y[5]*(*coordsIter)[1]/* + (numPoints==10?y[8]:0.0)*/;

      double nrm = norm(*normalsIter);
      double diff = norm(oldNormal - *normalsIter);
      double signFactor = 1.0;
      if (diff > nrm)
	signFactor = -1.0;
      *normalsIter *= signFactor/nrm;

  //     if(gradNormals != NULL) {
  //       WorldMatrix<double> grad;
  //       grad[0][0] = 2.0*y[0]; grad[0][1] = y[3]; grad[0][2] = y[4];
  //       grad[1][0] = y[3]; grad[1][1] = 2.0*y[1]; grad[1][2] = y[5];
  //       grad[2][0] = y[4]; grad[2][1] = y[5]; grad[2][2] = 2.0*y[2];
  //       grad *= signFactor;
  //       
  //       double trace_ = 0.0;
  //       for (int i=0;i<3;i++)
  // 	trace_ += (grad[i][i]/nrm-sqr((*normalsIter)[i]));
  //       std::cout<<"nrm(n)="<<nrm<<std::endl;
  //       std::cout<<"trace(grad(n))="<<vector_operations::trace(grad)/nrm<<"="<<trace_<<std::endl;
  //       
  //       WorldMatrix<double> NN;
  //       NN.vecProduct(*normalsIter, *normalsIter);
  // 
  //       grad -= NN;
  //       grad *= 1.0/nrm;
  //       
  //       *grdIter = grad;
  // 
  //     }
    }

  //   if(gradNormals == NULL)
  //     delete grdDOF;
  }

  
  void getCurvature(DOFVector<WorldVector<double> >* normals,DOFVector<double>* curvature)
  {
    int dim = normals->getFeSpace()->getMesh()->getDim();
    int dow = Global::getGeo(WORLD);
    
    WorldVector<DOFVector<double>*> normalsVec;
    WorldVector<DOFVector<WorldVector<double> >*> gradNormals;
    for (int i = 0; i < dow; i++) {
      normalsVec[i] = new DOFVector<double>(normals->getFeSpace(), "n_i");
      gradNormals[i] = new DOFVector<WorldVector<double> >(normals->getFeSpace(), "grad_i");
    }
    transform(normals, &normalsVec);
    for (int i = 0; i < dow; i++)
      normalsVec[i]->getRecoveryGradient(gradNormals[i]);

    DOFIterator<WorldVector<double> > grd0Iter(gradNormals[0], USED_DOFS); grd0Iter.reset();
    DOFIterator<WorldVector<double> > grd1Iter(gradNormals[1], USED_DOFS); grd1Iter.reset();
    DOFIterator<WorldVector<double> > grd2Iter(gradNormals[2], USED_DOFS); grd2Iter.reset();
    DOFIterator<double> hIter(curvature, USED_DOFS); hIter.reset();
    
    for (; !grd0Iter.end(); grd0Iter++,grd1Iter++,grd2Iter++,hIter++)
    {
      *hIter = ((*grd0Iter)[0] + (*grd1Iter)[1] + (*grd2Iter)[2])/static_cast<double>(dim);
    }

    for (int i = 0; i < dow; i++) {
      delete normalsVec[i];
      delete gradNormals[i];
    }
  }

  
  // plots a vector with ascii-code
  void plot(std::vector<double> &values, int numRows, int numCols, std::string symbol)
  {
    unsigned num=values.size();
    unsigned cols=std::min(numCols,(int)num), rows=std::min(numRows,(int)num);
    unsigned step=(unsigned)std::floor(num/(double)cols);
    cols=(unsigned)std::ceil(num/(double)step);

    double maxVal=*max_element(values.begin(),values.end());
    double minVal=*min_element(values.begin(),values.end());
    std::stringstream ssMax; ssMax.setf(std::ios::fixed,std::ios::floatfield); ssMax.precision(2); ssMax<<maxVal;
    std::stringstream ssMin; ssMin.setf(std::ios::fixed,std::ios::floatfield); ssMin.precision(2); ssMin<<minVal;
    unsigned cols0 = std::max(ssMax.str().length(),ssMin.str().length());
    std::stringstream ssNum; ssNum<<num;

    std::string emptyLine(cols+cols0,' ');emptyLine.replace(cols0-1,1,"|");
    std::vector<std::string> lines(rows+1,emptyLine);

    double range=maxVal-minVal;
    
    for(unsigned i=0;i<cols; i+=1) {
      int v=(int)round((rows-1)*(values[i*step]-minVal)/range,0);
      int r=rows-v-1, c=i+cols0;
      
      lines[r].replace(c,1,symbol);
      lines[rows].replace(c,1,"-");
    }

    lines[0].replace(cols0-ssMax.str().length(),ssMax.str().length(),ssMax.str());
    lines[rows-1].replace(cols0-ssMin.str().length(),ssMin.str().length(),ssMin.str());
    lines[rows].replace(cols0-1,2," 0");
    lines[rows].erase(cols+cols0-1,1);
    lines[rows].append(ssNum.str());

    for(unsigned i=0;i<lines.size(); ++i)
      std::cout<<lines[i]<<std::endl;
  }

} // end namespace Helpers

} }