#include "ElInfo2d.h"
#include "BasisFunction.h"
#include "Element.h"
#include "Line.h"
#include "Triangle.h"
#include "Tetrahedron.h"
#include "FiniteElemSpace.h"
#include "Flag.h"
#include "MacroElement.h"
#include "Mesh.h"
#include "Global.h"
#include "FixVec.h"
#include "DOFVector.h"

namespace AMDiS {

  void ElInfo2d::fillMacroInfo(const MacroElement * mel)
  {
    FUNCNAME("ElInfo::fillMacroInfo");

    int      i, k;
    Element  *nb;
    MacroElement *mnb;
#if 1 // #if !NEIGH_IN_EL
    bool    fill_opp_coords;
#endif  

  
    macroElement_ = const_cast<MacroElement*>(mel);
    element_       = const_cast<Element*>(mel->getElement());
    parent_   = NULL;
    level_    = 0;
  
    int vertices = mesh_->getGeo(VERTEX);
    //int edges = mesh_->getGeo(EDGE);

    if (fillFlag_.isSet(Mesh::FILL_COORDS) || 
	fillFlag_.isSet(Mesh::FILL_DET)    ||
	fillFlag_.isSet(Mesh::FILL_GRD_LAMBDA)) {
      for (i=0; i<vertices; i++) {
	coord_[i] = mel->coord[i];
      }
    }

    //   if(fillFlag_.isSet(Mesh::FILL_DET) || fillFlag_.isSet(Mesh::FILL_GRD_LAMBDA)) {
    //     det = calcGrdLambda(*Lambda);
    //   }

    int neighbours = mesh_->getGeo(NEIGH);

    if(fillFlag_.isSet(Mesh::FILL_OPP_COORDS) || 
       fillFlag_.isSet(Mesh::FILL_NEIGH)) 
      {
	fill_opp_coords = (fillFlag_.isSet(Mesh::FILL_OPP_COORDS));
    
	for (i = 0; i < neighbours; i++) {
	  if ((mnb = mel->getNeighbour(i))) {
	    neighbour_[i] = mel->getNeighbour(i)->getElement();
	    nb = const_cast<Element*>(neighbour_[i]);
	    k  = oppVertex_[i] = mel->getOppVertex(i);
	    if (nb->getFirstChild() && (k != 2)) {   // make nb nearest el.
	      nb = neighbour_[i] = 
		(k==0) ? 
		nb->getSecondChild() : 
		nb->getFirstChild();
	      k  = oppVertex_[i] = 2;
	      if  (fill_opp_coords) {
		if (nb->getNewCoord(-1)) {
		  oppCoord_[i] = *(nb->getNewCoord());
		} else {
		  oppCoord_[i] = 
		    (mnb->coord[0] + mnb->coord[1]) * 0.5;
		}
	      }
	    } else {
	      if  (fill_opp_coords) {
		oppCoord_[i] = mnb->coord[k];
	      }
	    }
	  } else {
	    neighbour_[i] = NULL;
	  }
	}
      }

    if (fillFlag_.isSet(Mesh::FILL_BOUND)) {
    
      //     for (i = 0; i < vertices; i++)
      //       (*bound)[i] = mel->getBound(i);

      for (i = 0; i < element_->getGeo(BOUNDARY) ; i++) {
	boundary_[i] = mel->getBoundary(i);
      }

      for(i = 0; i < element_->getGeo(PROJECTION); i++) {
	projection_[i] = mel->getProjection(i);
      }
    }
  }


  /****************************************************************************/
  /*   fill ElInfo structure for one child of an element   		    */
  /****************************************************************************/

  void ElInfo2d::fillElInfo(int ichild, const ElInfo *elinfo_old)
  {
    FUNCNAME("ElInfo::fillElInfo");
    Element      *elem = elinfo_old->element_;
    int j;
    Element  *nb;

    Flag    fill_flag = elinfo_old->fillFlag_;
    bool   fill_opp_coords;

    TEST_EXIT(elem->getFirstChild())("no children?\n");
    TEST_EXIT(element_ = const_cast<Element*>((ichild==0) ? 
					      elem->getFirstChild() : 
					      elem->getSecondChild()))
      ("missing child %d?\n", ichild);

    macroElement_  = elinfo_old->macroElement_;
    fillFlag_ = fill_flag;
    parent_   = elem;
    level_    = elinfo_old->level_ + 1;

    int dow = Global::getGeo(WORLD);

    if (fillFlag_.isSet(Mesh::FILL_COORDS) || 
	fillFlag_.isSet(Mesh::FILL_DET)    ||
	fillFlag_.isSet(Mesh::FILL_GRD_LAMBDA))
      {
	if (elem->getNewCoord(-1)) {
	  coord_[2] = *(elem->getNewCoord());
	} else {
	  for (j = 0; j < dow; j++)
	    coord_[2][j] = 
	      0.5 * (elinfo_old->coord_[0][j] + elinfo_old->coord_[1][j]);
	  // projection !?!
	}

	if (ichild==0) {
	  for (j = 0; j < dow; j++) {
	    coord_[0][j] = elinfo_old->coord_[2][j];
	    coord_[1][j] = elinfo_old->coord_[0][j];
	  }
	} else {
	  for (j = 0; j < dow; j++) {
	    coord_[0][j] = elinfo_old->coord_[1][j];
	    coord_[1][j] = elinfo_old->coord_[2][j];
	  }
	}
      }

    //   if(fillFlag_.isSet(Mesh::FILL_DET) || fillFlag_.isSet(Mesh::FILL_GRD_LAMBDA)) {
    //     det = calcGrdLambda(*Lambda);
    //   }

    if (fill_flag.isSet(Mesh::FILL_NEIGH) 
	|| fillFlag_.isSet(Mesh::FILL_OPP_COORDS))
      {
	fill_opp_coords = (fill_flag.isSet(Mesh::FILL_OPP_COORDS));
	if (ichild==0) {
	  if ((neighbour_[2] = elinfo_old->neighbour_[1])) {
	    if (fill_opp_coords) {
	      for (j=0; j<dow; j++)
		oppCoord_[2][j] = elinfo_old->oppCoord_[1][j];
	    }
	  }
      
	  oppVertex_[2] = elinfo_old->oppVertex_[1];

	  if (elem->getFirstChild()  &&  
	      elem->getSecondChild()->getFirstChild()  &&  
	      elem->getSecondChild()->getFirstChild()) // ????
	    {
	      TEST_EXIT((neighbour_[1] = elem->getSecondChild()->getSecondChild()))
		("???");
	      oppVertex_[1] = 2;
	      if (fill_opp_coords) {
		if (elem->getSecondChild()->getNewCoord(-1))
		  oppCoord_[1] = *(elem->getSecondChild()->getNewCoord());
		else
		  for (j=0; j<dow; j++)
		    oppCoord_[1][j] = 
		      0.5 * (elinfo_old->coord_[1][j] + 
			     elinfo_old->coord_[2][j]);
		// projection !?!
	      }
	    } else {
	    TEST_EXIT((neighbour_[1] = elem->getSecondChild()))("???");
	    oppVertex_[1] = 0;
	    if (fill_opp_coords) {
	      for (j=0; j<dow; j++)
		oppCoord_[1][j] = elinfo_old->coord_[1][j]; 
	    }
	  }
	  if ((nb = elinfo_old->neighbour_[2])) {
	    TEST(elinfo_old->oppVertex_[2] == 2)("invalid neighbour\n"); 
	    TEST_EXIT(nb->getFirstChild())("missing children?\n");
	    TEST_EXIT((nb = nb->getSecondChild()))("missing getSecondChild()?\n");
	  
	    if (nb->getFirstChild()) {
	      oppVertex_[0] = 2;
	      if (fill_opp_coords) {
		if (nb->getNewCoord(-1)) {
		  oppCoord_[0] = *(nb->getNewCoord());
		} else {
		  for (j=0; j<dow; j++)
		    oppCoord_[0][j] =
		      0.5*(elinfo_old->oppCoord_[2][j] + 
			   elinfo_old->coord_[0][j]);
		}
	      }	    
	      nb = nb->getFirstChild();
	    } else {
	      oppVertex_[0] = 1;
	      if (fill_opp_coords) {
		for (j=0; j<dow; j++)
		  oppCoord_[0][j] = elinfo_old->oppCoord_[2][j];
	      }
	    }
	  }
      
	  neighbour_[0] = nb; 
	} else {   /* ichild==1 */
	  if ((neighbour_[2] = elinfo_old->neighbour_[0])) {
	    if (fill_opp_coords) {
	      for (j=0; j<dow; j++)
		oppCoord_[2][j] = elinfo_old->oppCoord_[0][j];
	    }
	  }
      
	  oppVertex_[2] = elinfo_old->oppVertex_[0];
	  if (elem->getFirstChild()->getFirstChild()) {
	    neighbour_[0] = elem->getFirstChild()->getFirstChild();
	    oppVertex_[0] = 2;
	    if (fill_opp_coords) {
	      if (elem->getFirstChild()->getNewCoord(-1)) {
		oppCoord_[0] = *(elem->getFirstChild()->getNewCoord());
	      } else {
		for (j=0; j<dow; j++)
		  oppCoord_[0][j] = 
		    0.5 * (elinfo_old->coord_[0][j] + 
			   elinfo_old->coord_[2][j]);
		// projection !?!
	      }
	    }
	  } else {
	    neighbour_[0] = elem->getFirstChild();
	    oppVertex_[0] = 1;
	    if (fill_opp_coords) {
	      for (j=0; j<dow; j++)
		oppCoord_[0][j] = elinfo_old->coord_[0][j]; 
	    }
	  }
	  if ((nb = elinfo_old->neighbour_[2])) {
	    TEST(elinfo_old->oppVertex_[2] == 2)("invalid neighbour\n"); 
	    TEST((nb = nb->getFirstChild()))("missing child?\n");
	    if (nb->getFirstChild()) {
	      oppVertex_[1] = 2;
	      if (fill_opp_coords) {
		if (nb->getNewCoord(-1))
		  oppCoord_[1] = *(nb->getNewCoord());
		else
		  for (j=0; j<dow; j++)
		    oppCoord_[1][j] =
		      0.5*(elinfo_old->oppCoord_[2][j] + 
			   elinfo_old->coord_[1][j]); 
		// projection !?!
	      }
	      nb = nb->getSecondChild();
	    } else {
	      oppVertex_[1] = 0;
	      if (fill_opp_coords) {
		for (j=0; j<dow; j++)
		  oppCoord_[1][j] = elinfo_old->oppCoord_[2][j];
	      }
	    }
	  }
	  neighbour_[1] = nb;
	}
      }
  
    if (fill_flag.isSet(Mesh::FILL_BOUND)) {
      if (elinfo_old->getBoundary(2))
	boundary_[5] = elinfo_old->getBoundary(2);
      else
	boundary_[5] = INTERIOR;
    

      if (ichild==0) {
	boundary_[3] = elinfo_old->getBoundary(5);
	boundary_[4] = elinfo_old->getBoundary(3);
	boundary_[0] = elinfo_old->getBoundary(2);
	boundary_[1] = INTERIOR;
	boundary_[2] = elinfo_old->getBoundary(1);
      } else {
	boundary_[3] = elinfo_old->getBoundary(4);
	boundary_[4] = elinfo_old->getBoundary(5);
	boundary_[0] = INTERIOR;
	boundary_[1] = elinfo_old->getBoundary(2);
	boundary_[2] = elinfo_old->getBoundary(0);
      }

      if(elinfo_old->getProjection(0) && 
	 elinfo_old->getProjection(0)->getType() == VOLUME_PROJECTION) 
	{
	  projection_[0] = elinfo_old->getProjection(0);
	} else { // boundary projection
	if (ichild==0) {
	  projection_[0] = elinfo_old->getProjection(2);
	  projection_[1] = NULL;
	  projection_[2] = elinfo_old->getProjection(1);
	} else {
	  projection_[0] = NULL;
	  projection_[1] = elinfo_old->getProjection(2);
	  projection_[2] = elinfo_old->getProjection(0);
	}
      }
    }
  }

  double ElInfo2d::calcGrdLambda(DimVec<WorldVector<double> >& grd_lam) const
  {
    FUNCNAME("ElInfo2d::calcGrdLambda");
    //   TEST_EXIT(Global::getGeo(WORLD) == 2)
    //     ("dim != dim_of_world ! use parametric elements!\n");
  
    int         i, j;
    WorldVector<double> e1, e2;
    double      sdet, det1, adet = 0.0;
    const WorldVector<double>  v0= coord_[0];
    double        a11, a12, a21, a22;

    testFlag(Mesh::FILL_COORDS);
    int dow = Global::getGeo(WORLD);
    int dim = mesh_->getDim();

    for (i = 0; i < dow; i++) {
      e1[i] = coord_[1][i] - v0[i];
      e2[i] = coord_[2][i] - v0[i];
    }

    if(Global::getGeo(WORLD) == 2) {
      sdet = e1[0] * e2[1] - e1[1] * e2[0];
      adet = abs(sdet);

      if (adet < 1.0E-25) {
	MSG("abs(det) = %f\n", adet);
	for (i = 0; i <= dim; i++)
	  for (j = 0; j < dow; j++)
	    grd_lam[i][j] = 0.0;
      }
      else
	{
	  det1 = 1.0 / sdet;
	  a11 =  e2[1] * det1;       /* (a_ij) = A^{-T} */
	  a21 = -e2[0] * det1;
	  a12 = -e1[1] * det1;
	  a22 =  e1[0] * det1;

	  grd_lam[1][0] = a11;
	  grd_lam[1][1] = a21;
	  grd_lam[2][0] = a12;
	  grd_lam[2][1] = a22;
	  grd_lam[0][0] = - grd_lam[1][0] - grd_lam[2][0];
	  grd_lam[0][1] = - grd_lam[1][1] - grd_lam[2][1];
	}
    } else {
      WorldVector<double> normal;
    
      vectorProduct(e1, e2, normal);

      adet = norm(&normal);

      if (adet < 1.0E-15) {
	MSG("abs(det) = %lf\n", adet);
	for (i = 0; i <= dim; i++)
	  for (j = 0; j < dow; j++)
	    grd_lam[i][j] = 0.0;
      } else {
	vectorProduct(e2, normal, grd_lam[1]);
	vectorProduct(normal, e1, grd_lam[2]);
      
	double adet2 = 1.0/(adet*adet);

	for (i = 0; i < dow; i++) {
	  grd_lam[1][i] *= adet2;
	  grd_lam[2][i] *= adet2;
	}

	grd_lam[0][0] = - grd_lam[1][0] - grd_lam[2][0];
	grd_lam[0][1] = - grd_lam[1][1] - grd_lam[2][1];
	grd_lam[0][2] = - grd_lam[1][2] - grd_lam[2][2];
      }
    }

    return adet;
  }

  const int ElInfo2d::worldToCoord(const WorldVector<double>& xy,
				   DimVec<double>* lambda) const
  {
    FUNCNAME("ElInfo::worldToCoord");
    DimVec<WorldVector<double> > edge(mesh_->getDim(), NO_INIT);
    WorldVector<double> x;
    double  x0, det, det0, det1;
    int   i, j;
  
    static DimVec<double> vec(mesh_->getDim(), NO_INIT);

    TEST_EXIT(lambda)("lambda must not be NULL\n");

    int dim = mesh_->getDim();
    int dimOfWorld = Global::getGeo(WORLD);

    for (j=0; j<dimOfWorld; j++) {
      x0 = coord_[dim][j];
      x[j] = xy[j] - x0;
      for (i=0; i < dim; i++)
	edge[i][j] = coord_[i][j] - x0;
    }
  
    det  = edge[0][0] * edge[1][1] - edge[0][1] * edge[1][0]; 
    det0 =       x[0] * edge[1][1] -       x[1] * edge[1][0]; 
    det1 = edge[0][0] * x[1]       - edge[0][1] * x[0]; 

    if (abs(det) < DBL_TOL) {
      ERROR("det = %le; abort\n", det);
      for (i=0; i<=dim; i++) (*lambda)[i] = 1.0/dim;
      return 0;
    }

    (*lambda)[0] = det0 / det;
    (*lambda)[1] = det1 / det;
    (*lambda)[2] = 1.0 - (*lambda)[0] - (*lambda)[1];

    //   if(dimOfWorld == 3) {
    //     if(abs(x[2] - edge[0][2] * lambda[0] - edge[1][2] * lambda[1]) > 1.e-5) {
    //       WARNING("system not solvable\n");
    //     };
    //   }

    int k = -1;
    double lmin = 0.0;
    j = 0;
    for (i = 0; i <= dim; i++) {
      if ((*lambda)[i] < -1.E-5) {
	if ((*lambda)[i] < lmin) {
	  k = i;
	  lmin = (*lambda)[i];
	}
	j++;
      }
    }

    return k;
  }


  double ElInfo2d::getNormal(int side, WorldVector<double> &normal) const
  {
    FUNCNAME("ElInfo::getNormal");
    double   det = 0.0;
    int    i0, i1;

    int dow = Global::getGeo(WORLD);

    i0 = (side+1) % 3;
    i1 = (side+2) % 3;

    if (dow == 2){
      normal[0] = coord_[i1][1] - coord_[i0][1];
      normal[1] = coord_[i0][0] - coord_[i1][0];
    } else { // dow == 3
      WorldVector<double> e0, e1,e2, elementNormal;

      e0 = coord_[i1]; e0 -= coord_[i0];
      e1 = coord_[i1]; e1 -= coord_[side];
      e2 = coord_[i0]; e2 -= coord_[side];

      vectorProduct(e1, e2, elementNormal);
      vectorProduct(elementNormal, e0, normal);
    }

    det = norm(&normal);

    TEST_EXIT(det > 1.e-30)("det = 0 on face %d\n", side);

    normal *= 1.0 / det;
    
    return(det);
  }


  /****************************************************************************/
  /*  calculate the normal of the element for dim of world = dim + 1          */
  /*  return the absulute value of the determinant from the                   */
  /*  transformation to the reference element                                 */
  /****************************************************************************/
  double ElInfo2d::getElementNormal( WorldVector<double> &elementNormal) const
  {
    FUNCNAME("ElInfo::getElementNormal");
    double   det = 0.0;
    int dow = Global::getGeo(WORLD);
    WorldVector<double> e0, e1;

    TEST_EXIT(dow =  3)(" element normal only well defined for  DIM_OF_WORLD = DIM + 1 !!");

    e0 = coord_[1] - coord_[0];
    e1 = coord_[2] - coord_[0];

    vectorProduct(e0, e1, elementNormal);

    det = norm(&elementNormal);

    TEST_EXIT(det > 1.e-30)("det = 0");

    elementNormal *= 1.0 / det;
    
    return(det);
  }









}