/*
    C++ Mesh Generation Library
    Copyright (c) 2000echet <eric at bechet dot ca>

    This file is part of the C++ Mesh Generation Library.

    See the NOTICE & LICENSE files for conditions.
*/
//---------------------------------------------------------------------------
#ifndef MESH_LCM_TOOLS_H
#define MESH_LCM_TOOLS_H
//---------------------------------------------------------------------------

#include "mesh_const.h"

#include <list>
#include <set>
#include <map>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <ctime>
#include <cmath>
#include <cstdio>

#include "simplex.h"
#include "metric_field.h"
#include "metric_lcm_tools.h"
#include "geometry.h"
#include "mesh.h"
#include "mesh_globals.h"
#include "iterator_container.h"
#include "bipoint.h"
#include "topological_face.h"
#include "vector_unique_container.h"
#include "simple_mesh.h"

using namespace std;



/**
functions used in mesh modification related to LCM process
*/
template<int nbn,int dim,class E=Simplex<nbn,dim,Vertex<dim> > > class Mesh_LCM_Tools : public Simple_Mesh<nbn,dim,E>
{

public :

  typedef typename Simple_Mesh<nbn,dim,E>::Vertex_Type Vertex_Type;
  typedef typename Simple_Mesh<nbn,dim,E>::Element_Type Element_Type;
  typedef typename Simple_Mesh<nbn,dim,E>::Bipoint_Type Bipoint_Type;
  typedef typename Simple_Mesh<nbn,dim,E>::Topological_Face_Type Topological_Face_Type;
  typedef typename Simple_Mesh<nbn,dim,E>::Vertex_Iterator Vertex_Iterator;
  typedef typename Simple_Mesh<nbn,dim,E>::Element_Iterator Element_Iterator;
  typedef typename Simple_Mesh<nbn,dim,E>::Bipoint_Iterator Bipoint_Iterator;
  typedef typename Simple_Mesh<nbn,dim,E>::Topological_Face_Iterator Topological_Face_Iterator;

/// without metric
  Mesh_LCM_Tools() :Simple_Mesh<nbn,dim,E>()
  {
  }

/// constructor with a metric
  Mesh_LCM_Tools ( Metric_Field<typename Simple_Mesh<nbn,dim,E>::Vertex_Type> &M ) :Simple_Mesh<nbn,dim,E> ( M )
  {
  }

/// inserts an injection zone (2 nodes defining the position and angle)
  int Insert_Injection_Node ( Vertex_Type node_coord, Vertex_Type axis_coord,
                              double Injection_Radius, bool DoProject );
/// inserts an injection zone (1 node defining the position, angle normal to the surface)
  int Insert_Injection_Node ( Vertex_Type node_coord, double Injection_Radius, bool DoProject=true );
///
  int Insert_Trench ( Vertex_Type node_1, Vertex_Type node_2, double width );

/// remesh for a side effect.
  int Insert_Side_Effect ( double sizet,double sizen,double thk,Vertex_Type tabV[],int numpairs,int zone=0 );


};

//---------------------------- INSERT INJECTION HOLE METHOD ----------------------------------------
template<int nbn,int dim,class E>
int Mesh_LCM_Tools<nbn,dim,E>::Insert_Injection_Node ( Vertex_Type node_coord,double Injection_Radius, bool DoProject )
{
  return Insert_Injection_Node ( node_coord, node_coord,Injection_Radius,DoProject );
}


//---------------------------- INSERT INJECTION HOLE METHOD ----------------------------------------
template<int nbn,int dim,class E>
int Mesh_LCM_Tools<nbn,dim,E>::Insert_Injection_Node ( Vertex_Type node_coord, Vertex_Type axis_coord,
    double Injection_Radius, bool DoProject )
{
  cout << "YOU REQUESTED TO INSERT AN INJECTION HOLE AT POSITION: ";
  cout << "X= " << node_coord[0] << " Y= " << node_coord[1] << " Z= " << node_coord[2] << endl;



  //Local Declarations
  Element_Iterator ele,elemin;
  Vertex_Iterator ver[3];
  Vertex_Iterator bver[2];

  Iterator_Container<Element_Iterator> triangle_to_delete;
  typedef typename Iterator_Container<Element_Iterator>::iterator triangle_to_delete_it;
  triangle_to_delete_it ttd_it;

  Iterator_Container<Element_Iterator> triangle_deleted;

  list<Element_Iterator> triangle_init;
  typedef typename list<Element_Iterator>::iterator triangle_init_it;
  triangle_init_it tt;

  double param[3];
  double triangle_vertices_coord[3][3];
  const double *coord;
  double distance;
  double eps=1.e-16; //A standard REMESH accuracy is probably defined elsewhere

  int i,j;
  int nbnodes,nbnodesperelement;

  Cylindrical_Metric<Vertex_Type> Metric;

  this->Renum ( GLOBAL_NUMBERING );


  //------------------------------------------------------------------------------------
  //STEP -1:  Seek the normal if necessary
  //------------------------------------------------------------------------------------
  cout << "STEP -1: Seek for a normal if necessary" << endl;;
  double dist=0;
  double distmean=0;
  double distmin=-1;

  for ( i=0; i<3; i++ )
  {
    dist+= ( node_coord[i]-axis_coord[i] ) * ( node_coord[i]-axis_coord[i] );
  }

  dist=sqrt ( dist );

  if ( dist<1e-5 ) // 2 common nodes -> need to find the normal and set new nodes
  {
    Vector normal ( 3 );

    for ( ele=this->Get_Elements_Start(); ele!=this->Get_Elements_End(); ++ele )
    {
      distmean=0;

      for ( i=0; i<3; i++ )
      {
        dist=0;

        for ( j=0; j<3; j++ )
        {
          dist+= ( ( * ( ( *ele ) [i] ) ) [j]-node_coord[j] ) * ( ( * ( ( *ele ) [i] ) ) [j]-node_coord[j] );
        }

        dist=sqrt ( dist );
        distmean+=dist/3;
      }

      if ( ( distmin>distmean ) || ( distmin<0 ) )
      {
        distmin=distmean;
        elemin=ele;
      }
    }

    // ok we found the closest triangle
    // now get the normal
    ( *elemin ).Normal ( normal );

    // and set new nodes for the injection and axis
    for ( i=0; i<3; i++ )
    {
      axis_coord[i]=node_coord[i]-normal[i]*30*Injection_Radius;
      //cout << normal[i] << " " << axis_coord[i] <<  "  " ;

      node_coord[i]+=normal[i]*30*Injection_Radius;
      //cout << node_coord[i] << endl;
    }

    // voila.


  }



  //------------------------------------------------------------------------------------
  //STEP 0:  Remesh locally to achieve better regurity
  //------------------------------------------------------------------------------------
  cout << "STEP 0: Remeshing locally" << endl;
  Metric.Set_Params ( Injection_Radius,Injection_Radius/1.5,Injection_Radius/3,node_coord,axis_coord );
  Metric_Field<Vertex_Type> *ptr;
  ptr=this->Get_Metric_Field();
  this->Set_Metric_Field ( Metric );
  this->Do_Mesh();
  this->Set_Metric_Field ( *ptr );



  //------------------------------------------------------------------------------------
  //STEP 1:  Store all the bipoints that have no neighbours
  //------------------------------------------------------------------------------------

  nbnodesperelement=Element_Type::Nb_Nodes;
  nbnodes=this->Get_Nb_Vertices();
  cout << "STEP 1: Storing boundary bipoints " << endl;
  Iterator_Container<Bipoint_Iterator> bipoint_at_boundary;

  Bipoint_Iterator bip;

  for ( bip=this->Get_Bipoints_Start() ; bip!=this->Get_Bipoints_End(); ++bip )
  {
    if ( ( *bip ).Get_Number_Elements() == 1 )
    {
      bipoint_at_boundary.insert ( bip );
    }
  }

  //END STEP 1
  //-------------------------------------------------------------------------------------

  //-------------------------------------------------------------------------------------
  //STEP2:Scan all triangles and find the one that contains the center of the injection
  //-------------------------------------------------------------------------------------
  cout << "STEP 2: Cleaning injection area " << endl;
  bool ItIsInside;
  double x1,y1,z1;
  double x2,y2,z2;
  double x3,y3,z3;
  double xp1,yp1,zp1;
  double xp2,yp2,zp2;
  double node_temp[3];
  double xt,yt,zt;
  double A,B,C,D;
  double den,lambda;
  double distance_old=1.e10;
  int kk;
  // Define points P1 and P2 coordinate

  xp1=axis_coord[0] ;
  yp1=axis_coord[1] ;
  zp1=axis_coord[2] ;
  xp2=node_coord[0] ;
  yp2=node_coord[1] ;
  zp2=node_coord[2] ;

  kk=1;

  for ( ele=this->Get_Elements_Start(); ele!=this->Get_Elements_End(); ++ele )
  {
    ( *ele ).Get_Vertices ( ver );

    for ( i=0; i<nbnodesperelement; ++i )
    {
      coord= ( *ver[i] ).GetPos();

      for ( int j=0; j < 3; ++j )
      {
        triangle_vertices_coord[i][j]=coord[j];
      }
    }

    //If the user screws the injection node coordinates (which will be the case
    // for 3D surfaces, we might want to get the intersection of the line
    //defined by the axis of injection and any of the triangle in the mesh
    //That we have the coordinates of the triangle vertices
    x1=triangle_vertices_coord[0][0] ;
    y1=triangle_vertices_coord[0][1] ;
    z1=triangle_vertices_coord[0][2] ;
    x2=triangle_vertices_coord[1][0] ;
    y2=triangle_vertices_coord[1][1] ;
    z2=triangle_vertices_coord[1][2] ;
    x3=triangle_vertices_coord[2][0] ;
    y3=triangle_vertices_coord[2][1] ;
    z3=triangle_vertices_coord[2][2] ;

    A =  y1* ( z2 - z3 ) + y2* ( z3 - z1 ) + y3* ( z1 - z2 );
    B =  z1* ( x2 - x3 ) + z2* ( x3 - x1 ) + z3* ( x1 - x2 );
    C =  x1* ( y2 - y3 ) + x2* ( y3 - y1 ) + x3* ( y1 - y2 );
    D = - ( x1* ( y2*z3 - y3*z2 ) + x2* ( y3*z1 - y1*z3 ) + x3* ( y1*z2 - y2*z1 ) );

    // Check for Intersection
    den= ( xp1-xp2 ) *A+ ( yp1-yp2 ) *B+ ( zp1-zp2 ) *C;

    if ( den == 0. )
    {
      continue;
    }
    else
    {
      lambda= ( xp1*A+yp1*B+zp1*C+D ) /den;
    }

    node_temp[0]=xp1+lambda* ( xp2-xp1 ) ;
    node_temp[1]=yp1+lambda* ( yp2-yp1 ) ;
    node_temp[2]=zp1+lambda* ( zp2-zp1 ) ;

    xt=node_temp[0] ;
    yt=node_temp[1] ;
    zt=node_temp[2] ;

    distance=sqrt ( pow ( ( xp2-xt ),2 ) + pow ( ( yp2-yt ),2 ) + pow ( ( zp2-zt ),2 ) ) ;

    //process only if distance is less or equal than previous distance processed
    // Check if intersection is inside triangle
    ItIsInside=false;
    ItIsInside=Is_Inside_Triangle3D ( node_temp,triangle_vertices_coord,param );

    if ( ItIsInside )
    {
      if ( distance > distance_old && kk > 1 )
      {
        continue;
      }

      distance_old=distance;
      triangle_init.push_back ( ele );
      node_coord[0]=xt ;
      node_coord[1]=yt ;
      node_coord[2]=zt ;
      ++kk;
    }
  }

  //If injection axis does not fall onto geometry quit procedure
  if ( triangle_init.size() == 0 )
  {
    cout << "Injection axis does not intersect current geometry " << endl;
    cout << "Please check your Injection input coordinates " << endl;
    return 0;
  }

  for ( tt=triangle_init.begin(); tt!=triangle_init.end(); ++tt )
  {
    this->Detach ( *tt );
  }


  //END STEP 2
  //-------------------------------------------------------------------------------------

  //-------------------------------------------------------------------------------------
  //STEP 3:   Begin the loop for detaching triangles till cleaning the injection hole and
  //          some more. Any bipoint that cannot be inserted into bipoint_at_boundary
  //          (see STEP 1) container will be consequently inserted in hole container
  //-------------------------------------------------------------------------------------
  cout << "STEP 3: Creating sequence of nodes for the injection hole " << endl;

  //related to cleaning algorithm (Always cleans smallest dist from
  //                                      Injection center to sequence node)
  multimap<double, Vertex_Iterator> Sequence_map;
  typedef typename multimap<double, Vertex_Iterator>::value_type ValuePair;
  typename multimap<double, Vertex_Iterator>::iterator iter;

  list<Vertex_Iterator> Sequence;
  typedef typename list<Vertex_Iterator>::iterator Sequence_it;
  Sequence_it Sit;


  bool Injection_clean=true;
  double cos_angle,Injection_Radius_corrected;
  double Vx1, Vy1, Vz1;
  double Vx2, Vy2, Vz2;
  double V1, V2;
  int count_bdry_bipoints ;

  //Hole related
  typedef typename Iterator_Container<Bipoint_Iterator>::iterator bipiterator;
  list<Bipoint_Iterator> hole;
  pair<bipiterator,bool> Pb;

  //Sequence related
  typedef typename list<Bipoint_Iterator>::iterator listit;
  listit Lit, Lit2;
  Vertex_Iterator VI_start;
  Vertex_Iterator VI;
  Vertex_Iterator VI_temp;
  Vertex_Iterator VI_next;
  Vertex_Iterator I;

  //To check for starting node
  Iterator_Container<Bipoint_Iterator> Bips;
  typename Iterator_Container<Bipoint_Iterator>::iterator Bips_it, Bips_it2;
  bool found_node, done;
  bool closed_loop;
  done=false;

  do
  {
    //fill the hole container with Node IDs of the Bipoint that cannot be
    //inserted in STEP 1 container
    for ( bip=this->Get_Bipoints_Start() ; bip!=this->Get_Bipoints_End(); ++bip )
    {
      if ( ( *bip ).Get_Number_Elements() == 1 )
      {
        Pb=bipoint_at_boundary.insert ( bip );

        if ( Pb.second )
        {
          hole.push_front ( bip );
          bipoint_at_boundary.erase ( bip );
        }
      }
    }

    //IMPORTANT STEP:
    //If the injection hole touches the boundary of the geometry then the sequence construction
    //process might loose some nodes if the ones we start with, do not correspond to the beginning.
    //Therefore, before processing, check if a node of the hole's bipoint is connected to
    //one boundary bipoint and only one. If it is confirmed then chose this node as a starting node to construct
    //the sequence
    Lit=hole.begin();
    ( * ( *Lit ) ).Get_Vertices ( bver );
    VI=bver[0];
    VI_start=VI;
    VI_next=bver[1];

    found_node=false;

    for ( Lit=hole.begin() ; Lit!=hole.end() ; ++Lit )
    {
      ( * ( *Lit ) ).Get_Vertices ( bver );

      for ( int i=0; i<2; ++i )
      {
        VI_temp=bver[i];
        Bips.clear();
        this->Get_Bipoints ( VI_temp, Bips );

        if ( Bips.size() <= 1 )
        {
          cout << "Topological error: Check algorithm " << endl;
          return 0;
        }

        //Check if there is more than one boundary bipoint (in such case just skip)
        count_bdry_bipoints=0;

        for ( Bips_it=Bips.begin() ; Bips_it!=Bips.end() ; ++Bips_it )
        {
          if ( ( * ( *Bips_it ) ).Get_Number_Elements() == 1 )
          {
            count_bdry_bipoints++;
          }
        }

        if ( count_bdry_bipoints > 2 )
        {
          continue;
        }

        for ( Bips_it=Bips.begin() ; Bips_it!=Bips.end() ; ++Bips_it )
        {
          if ( ( * ( *Bips_it ) ).Get_Number_Elements() == 1 )
          {
            Pb=bipoint_at_boundary.insert ( *Bips_it );

            if ( Pb.second ) //Node not connected to boundary bipoint
            {
              bipoint_at_boundary.erase ( *Bips_it );
            }
            else
            {
              VI=bver[i];
              VI_start=VI;
              VI_next=bver[ ( i+1 ) %2];
              found_node=true;
              break;
            }
          }
        }

        if ( found_node == true )
        {
          break;
        }
      }

      if ( found_node == true )
      {
        break;
      }
    }

    //Initialize the process
    Sequence.push_back ( VI );
    Sequence.push_back ( VI_next );

    //Construct Sequence
    closed_loop=false;

    for ( Lit=hole.begin(); Lit!=hole.end() ; ++Lit )
    {
      for ( Lit2=hole.begin(); Lit2!=hole.end() ; ++Lit2 )
      {
        ( * ( *Lit2 ) ).Get_Vertices ( bver );

        for ( int j=0; j< 2; j++ )
        {
          if ( bver[j] == VI_next && bver[ ( j+1 ) %2] != VI )
          {
            VI=VI_next;
            VI_next=bver[ ( j+1 ) %2];

            if ( VI_next == VI_start )
            {
              closed_loop=true;
              break;
            }

            Sequence.push_back ( bver[ ( j+1 ) %2] );
          }
        }
      }

      if ( closed_loop ==  true )
      {
        break;
      }
    }

    //Now get distance from injection point to each of the sequence nodes
    //and compare to injection hole radius and remove triangle consequently
    Injection_clean=true;
    int SequenceSize;
    SequenceSize=Sequence.size();
    int nn=1;
    Sit=Sequence.begin();
    //create a multiset whose data is the distance from sequence node to injection center


    while ( nn <= SequenceSize )
    {
      I=*Sit;
      coord= ( * ( *Sit ) ).GetPos();
      distance=pow ( ( node_coord[0]-coord[0] ),2. ) +
               pow ( ( node_coord[1]-coord[1] ),2. ) +
               pow ( ( node_coord[2]-coord[2] ),2. );
      distance=sqrt ( distance );
      //Correct distance
      Vx1=axis_coord[0]-node_coord[0] ;
      Vy1=axis_coord[1]-node_coord[1] ;
      Vz1=axis_coord[2]-node_coord[2] ;
      Vx2=coord[0]-node_coord[0] ;
      Vy2=coord[1]-node_coord[1] ;
      Vz2=coord[2]-node_coord[2] ;
      V1=sqrt ( Vx1*Vx1+Vy1*Vy1+Vz1*Vz1 );
      V2=distance;
      cos_angle= ( Vx1*Vx2+Vy1*Vy2+Vz1*Vz2 ) /V1/V2;
      Injection_Radius_corrected=Injection_Radius/fabs ( sin ( acos ( fabs ( cos_angle ) ) ) );

      if ( distance < Injection_Radius_corrected )
      {
        Sequence_map.insert ( ValuePair ( distance, I ) );
      }

      ++nn;
      ++Sit;
    }

    if ( Sequence_map.size() != 0 )
    {
      Injection_clean = false;
      iter=Sequence_map.begin();

      I= ( *iter ).second;
      triangle_to_delete.clear();
      this->Get_Elements ( I,triangle_to_delete );

      if ( triangle_to_delete.size() != 0 )
      {
        for ( ttd_it = triangle_to_delete.begin(); ttd_it != triangle_to_delete.end(); ++ttd_it )
        {
          triangle_init.push_back ( *ttd_it ); //Continue storing altered elements
         this-> Detach ( *ttd_it );
        }
      }
    }

    Sequence_map.clear();

    if ( Injection_clean == false ) //If there is more to clean Loop again
    {
      hole.clear();
      Sequence.clear();
    }
    else
    {
      if ( done == false )
      {
        int nn=1;
        Sit=Sequence.begin();
        SequenceSize=Sequence.size();

        while ( nn <= SequenceSize )
        {
          I=*Sit;
          triangle_to_delete.clear();
         this-> Get_Elements ( I,triangle_to_delete );

          if ( triangle_to_delete.size() != 0 )
          {
            for ( ttd_it = triangle_to_delete.begin(); ttd_it != triangle_to_delete.end(); ++ttd_it )
            {
              triangle_init.push_back ( *ttd_it ); //Continue storing altered elements
              this->Detach ( *ttd_it );
            }
          }

          ++Sit;
          ++nn;
        }

        hole.clear();
        Sequence.clear();
        done=true;
        Injection_clean=false;
      }
    }

    //Clean bipoint_at_boundary

    Bips_it=bipoint_at_boundary.begin();

    while ( Bips_it != bipoint_at_boundary.end() )
    {
      Bips_it2=Bips_it;
      Bips_it++;
      ( * ( *Bips_it2 ) ).Get_Vertices ( bver );
      Bipoint_Type Fac ( bver[0],bver[1] );
      bip = this->Find_Bipoint ( Fac );

      if ( bip == this->Get_Bipoints_End() )
      {
        bipoint_at_boundary.erase ( Bips_it2 );
      }
    }


  }
  while ( Injection_clean == false ); //This loop is very important otherwise the algorithm

  //will leave triangles inside the injection area

  //----------------------------------------------------------------------------------------------
  //STEP 4:   Create an image of the Sequence of nodes at a distance equal to the corrected radius
  //----------------------------------------------------------------------------------------------
  cout << "STEP 4: Creating Injection hole vertices " << endl;

  Vertex_Type Vl;
  Element_Type El;
  Vertex_Iterator Image;
  list<Vertex_Iterator> Sequence_image;
  Sequence_it Sit_im;

  double theta, phi, Rad;
  double x, y, z;

  int jj=1;
  Sit=Sequence.begin();

  while ( jj <= ( int ) Sequence.size() )
  {
    coord= ( * ( *Sit ) ).GetPos();
    x=coord[0]-node_coord[0];
    y=coord[1]-node_coord[1];
    z=coord[2]-node_coord[2];

    //from cartesian to spherical (Need a new method for this)
    phi=0. ;
    theta=0. ;
    Rad=sqrt ( x*x+y*y+z*z );

    if ( Rad < eps )
    {
      cout << "Radius too small - Cannot process " << endl;
      return 0;
    }

    theta=atan2 ( sqrt ( x*x+y*y ),z );

    if ( fabs ( y ) > eps || fabs ( x ) > eps )
    {
      phi=atan2 ( y,x );
    }

    Vx1=axis_coord[0]-node_coord[0] ;
    Vy1=axis_coord[1]-node_coord[1] ;
    Vz1=axis_coord[2]-node_coord[2] ;
    Vx2=x ;
    Vy2=y ;
    Vz2=z ;
    V1=sqrt ( Vx1*Vx1+Vy1*Vy1+Vz1*Vz1 );
    V2=Rad;
    cos_angle= ( Vx1*Vx2+Vy1*Vy2+Vz1*Vz2 ) /V1/V2;

    Injection_Radius_corrected=Injection_Radius/fabs ( sin ( acos ( fabs ( cos_angle ) ) ) );

    //back to cartesian for new node (Note: point translation to global frame)
    Vl[0]=Injection_Radius_corrected*sin ( theta ) *cos ( phi ) +node_coord[0];
    Vl[1]=Injection_Radius_corrected*sin ( theta ) *sin ( phi ) +node_coord[1];
    Vl[2]=Injection_Radius_corrected*cos ( theta ) +node_coord[2];

    Image=this->Add_Vertex ( Vl,false ); //Insert new vertex

    Sequence_image.push_back ( Image ); //Insert new vertex in Image Sequence

    ++Sit;
    ++jj;
  }

  //
  //END STEP 4:
  //-------------------------------------------------------------------------------------

  //-------------------------------------------------------------------------------------
  //STEP 5:   Trying to project back to original geometry
  //-------------------------------------------------------------------------------------

  cout << "STEP 5: Reprojecting on initial geometry and finalizing mesh" << endl;

  list<Vertex_Iterator> Sequence_projected;

  double Xint[3];

  if ( DoProject == true )
  {
    // xp1,yp1,zp1  point on Sequence image
    // xp2,yp2,zp2  translated axis_coord point
    double xi1,yi1,zi1; //Injection node coordinates
    double xi2,yi2,zi2; //Second node used to define the injection axis
    //Intersection node

    xi1=node_coord[0] ;
    yi1=node_coord[1] ;
    zi1=node_coord[2];
    xi2=axis_coord[0] ;
    yi2=axis_coord[1] ;
    zi2=axis_coord[2];
    //Need to use the Sequence_image container
    jj=1;
    Sit_im=Sequence_image.begin();

    while ( jj <= ( int ) Sequence_image.size() )
    {
      //1 Get Sequence_image node coordinates
      coord= ( * ( *Sit_im ) ).GetPos();
      xp1=coord[0] ;
      yp1=coord[1] ;
      zp1=coord[2] ;
      //2 Get a node defining the translated axis_node following
      //  direction node_coord ---> Sequence_image node
      xp2=+xi2- ( xi1-xp1 );
      yp2=+yi2- ( yi1-yp1 );
      zp2=+zi2- ( zi1-zp1 );

      for ( tt=triangle_init.begin(); tt!=triangle_init.end() ; ++tt )
      {
        // Get triangle vertices coordinates
        ( * ( *tt ) ).Get_Vertices ( ver );

        for ( i=0; i<nbnodesperelement; ++i )
        {
          coord= ( *ver[i] ).GetPos();

          for ( int j=0; j < 3; ++j )
          {
            triangle_vertices_coord[i][j]=coord[j];
          }
        }

        // Get the coefficients of the triangle plane
        x1=triangle_vertices_coord[0][0] ;
        y1=triangle_vertices_coord[0][1] ;
        z1=triangle_vertices_coord[0][2] ;
        x2=triangle_vertices_coord[1][0] ;
        y2=triangle_vertices_coord[1][1] ;
        z2=triangle_vertices_coord[1][2] ;
        x3=triangle_vertices_coord[2][0] ;
        y3=triangle_vertices_coord[2][1] ;
        z3=triangle_vertices_coord[2][2] ;

        A =  y1* ( z2 - z3 ) + y2* ( z3 - z1 ) + y3* ( z1 - z2 );
        B =  z1* ( x2 - x3 ) + z2* ( x3 - x1 ) + z3* ( x1 - x2 );
        C =  x1* ( y2 - y3 ) + x2* ( y3 - y1 ) + x3* ( y1 - y2 );
        D = - ( x1* ( y2*z3 - y3*z2 ) + x2* ( y3*z1 - y1*z3 ) + x3* ( y1*z2 - y2*z1 ) );

        // Check for Intersection
        den= ( xp1-xp2 ) *A+ ( yp1-yp2 ) *B+ ( zp1-zp2 ) *C;

        if ( fabs ( den ) < eps ) //line parallel to triangle plane
        {
          continue;
        }
        else
        {
          lambda= ( xp1*A+yp1*B+zp1*C+D ) /den;
        }

        Xint[0]=xp1+lambda* ( xp2-xp1 ) ;
        Xint[1]=yp1+lambda* ( yp2-yp1 ) ;
        Xint[2]=zp1+lambda* ( zp2-zp1 ) ;
        // Check if intersection is inside triangle
        ItIsInside=false;
        ItIsInside=Is_Inside_Triangle3D ( Xint,triangle_vertices_coord,param );

        //If there is intersection -> Set Sequence_image node coordinates
        if ( ItIsInside )
        {
          Vx1=xi1-xi2 ;
          Vy1=yi1-yi2 ;
          Vz1=zi1-zi2 ; //vecteur 1
          Vx2=Xint[0]-xi2 ;
          Vy2=Xint[1]-yi2 ;
          Vz2=Xint[2]-zi2 ; //vecteur 2
          V1=sqrt ( Vx1*Vx1+Vy1*Vy1+Vz1*Vz1 );
          V2=sqrt ( Vx2*Vx2+Vy2*Vy2+Vz2*Vz2 );
          cos_angle= ( Vx1*Vx2+Vy1*Vy2+Vz1*Vz2 ) /V1/V2;

          if ( cos_angle < 0. )
          {
            continue;
          }
          else
          {
            Vl.SetPos ( Xint );
            Image=this->Add_Vertex ( Vl,false );
            Sequence_projected.push_back ( Image );
            break;
          }
        }
      }

      if ( ItIsInside == false )
      {
        Sequence_projected.push_back ( *Sit_im );
      }

      ++jj;
      ++Sit_im;
    }
  }
  else
  {
    for ( Sit_im=Sequence_image.begin(); Sit_im!=Sequence_image.end(); ++Sit_im )
    {
      Sequence_projected.push_back ( *Sit_im );
    }
  }


  //Attempting a direct triangle construction
  //Sequence_projected should be renamed into translated and here is the real one
  //Get closest distance from each node of Sequence to Sequence_projected

  Vertex_Iterator Vi[3];
  Element_Iterator Ei;

  int ll=1;
  int Sequencesize;

  double cos_angle1, cos_angle2;
  double dist1, dist2;
  double Xi[3][3];
  double d1,d2,d3;
  double VP1x,VP1y,VP1z;
  double VP2x,VP2y,VP2z;
  double VP3x,VP3y,VP3z;

  bool altered_sequence, defined_cos;

  Sequencesize=Sequence.size(); //Set container size

  if ( closed_loop == true )
  {
    Sequence_projected.push_back ( *Sequence_projected.begin() );
    Sequence.push_back ( *Sequence.begin() ); //push first element to the back
    Sequencesize++;
    Sit=Sequence.begin();
    ++Sit;
    Sequence.push_back ( *Sit );
    ++Sit;
    Sequence.push_back ( *Sit );
  }
  else
  {
    Sequencesize=Sequencesize-1;
  }

  Sit=Sequence.begin(); //Set iterator at top of container

  while ( ll < Sequencesize )
  {
    for ( int nc=0 ; nc<3 ; ++nc )
    {
      coord= ( * ( *Sit ) ).GetPos();
      x3=coord[0] ;
      y3=coord[1] ;
      z3=coord[2] ;

      distance_old=1.e10;

      Sit_im=Sequence_projected.begin();
      jj=1;

      while ( jj < ( int ) Sequence_projected.size() )
      {
        coord= ( * ( *Sit_im ) ).GetPos(); //Get P1 coordinates
        x1=coord[0] ;
        y1=coord[1] ;
        z1=coord[2] ;
        ++Sit_im;  //Get P2 coordinates
        coord= ( * ( *Sit_im ) ).GetPos();
        x2=coord[0] ;
        y2=coord[1] ;
        z2=coord[2] ;

        //Now get shortest distance from P3 to each P1-P2 segments
        lambda= ( ( x3-x1 ) * ( x2-x1 ) + ( y3-y1 ) * ( y2-y1 ) + ( z3-z1 ) * ( z2-z1 ) ) /
                ( pow ( ( x2-x1 ),2 ) + pow ( ( y2-y1 ),2 ) + pow ( ( z2-z1 ),2 ) ) ;

        //Now check for intersection
        if ( lambda > eps && lambda < 1-eps ) //Then P3 projects on P2-P1
        {
          x=x1+lambda* ( x2-x1 ); //Intersection coordinates at point P
          y=y1+lambda* ( y2-y1 );
          z=z1+lambda* ( z2-z1 );
          //compute distance from P3 to P
          distance=sqrt ( pow ( ( x3-x ),2 ) + pow ( ( y3-y ),2 ) + pow ( ( z3-z ),2 ) ) ;

          if ( distance < distance_old )
          {
            //Store distance and coordinates
            distance_old=distance;
            Xi[nc][0]=x ;
            Xi[nc][1]=y ;
            Xi[nc][2]=z ;
          }
        }
        else
        {
          //Get distance from P3 to P1 and compare to distance from P3 to P2 (could use lambda either)
          dist1=sqrt ( pow ( ( x3-x1 ),2 ) + pow ( ( y3-y1 ),2 ) + pow ( ( z3-z1 ),2 ) ) ;
          dist2=sqrt ( pow ( ( x3-x2 ),2 ) + pow ( ( y3-y2 ),2 ) + pow ( ( z3-z2 ),2 ) ) ;

          if ( dist1 < dist2 )
          {
            x=x1 ;
            y=y1 ;
            z=z1 ;
            distance=dist1;
          }
          else
          {
            x=x2 ;
            y=y2 ;
            z=z2 ;
            distance=dist2;
          }

          if ( distance < distance_old )
          {
            distance_old=distance;
            Xi[nc][0]=x ;
            Xi[nc][1]=y ;
            Xi[nc][2]=z ;
          }
        }

        ++jj;
      } //loop over sequence image done

      ++Sit;
    }//loop over nc done -> we have the three intersection

    --Sit;
    --Sit;
    --Sit;//back to first node

    //Get distance between intersection nodes and sequence nodes
    //VPi is the vector pointing from sequence node to intersection node
    coord= ( * ( *Sit ) ).GetPos();
    VP1x=coord[0]-Xi[0][0] ;
    VP1y=coord[1]-Xi[0][1] ;
    VP1z=coord[2]-Xi[0][2] ;
    d1=sqrt ( VP1x*VP1x + VP1y*VP1y + VP1z*VP1z );
    ++Sit;
    coord= ( * ( *Sit ) ).GetPos();
    VP2x=coord[0]-Xi[1][0] ;
    VP2y=coord[1]-Xi[1][1] ;
    VP2z=coord[2]-Xi[1][2] ;
    d2=sqrt ( VP2x*VP2x + VP2y*VP2y + VP2z*VP2z );
    ++Sit;
    coord= ( * ( *Sit ) ).GetPos();
    VP3x=coord[0]-Xi[2][0] ;
    VP3y=coord[1]-Xi[2][1] ;
    VP3z=coord[2]-Xi[2][2] ;
    d3=sqrt ( VP3x*VP3x + VP3y*VP3y + VP3z*VP3z );

    --Sit;
    --Sit; //back to first node

    //Construct vectors between intersection nodes
    Vx1=Xi[1][0]-Xi[0][0] ;
    Vy1=Xi[1][1]-Xi[0][1] ;
    Vz1=Xi[1][2]-Xi[0][2] ;
    Vx2=Xi[2][0]-Xi[0][0] ;
    Vy2=Xi[2][1]-Xi[0][1] ;
    Vz2=Xi[2][2]-Xi[0][2] ;
    V1=sqrt ( Vx1*Vx1+Vy1*Vy1+Vz1*Vz1 );
    V2=sqrt ( Vx2*Vx2+Vy2*Vy2+Vz2*Vz2 );

    defined_cos=false;

    if ( V1 < eps && V2 < eps )
    {
      //construct one or two triangles (depending on cosines - jump to third node and go to top of loop)
      cos_angle1= ( VP1x*VP2x + VP1y*VP2y + VP1z*VP2z ) /d1/d2;
      cos_angle2= ( VP3x*VP2x + VP3y*VP2y + VP3z*VP2z ) /d3/d2;

      //If none of the vectors are colinear
      if ( cos_angle1 < 1-eps && cos_angle2 < 1-eps )
      {
        Vl.SetPos ( Xi[0] );
        Vi[1]=this->Add_Vertex ( Vl,false );
        Vi[0]= ( *Sit );
        ++Sit;
        Vi[2]= ( *Sit );

        El[0]=Vi[0];
        El[1]=Vi[1];
        El[2]=Vi[2];
        Ei=this->Add_Element ( El );
        this->Attach ( Ei );

        ++Sit;
        Vi[0]= ( *Sit );

        El[0]=Vi[0];
        El[1]=Vi[1];
        El[2]=Vi[2];
        Ei=this->Add_Element ( El );
        this->Attach ( Ei );

        //reposition to first node
        Sit=Sequence.begin();

        for ( i=1 ; i<ll+2 ; ++i )
        {
          ++Sit;
        }

        ++ll;
        ++ll;
        continue;
      }

      if ( cos_angle1 > 1-eps ) //VP1 and VP2 are colinear then compare d1 and d2 to decide
      {
        if ( d1 < d2 ) //construct triangle P1 P2 P3
        {
          Vi[0]= ( *Sit );
          ++Sit;
          Vi[1]= ( *Sit );
          ++Sit;
          Vi[2]= ( *Sit );

          El[0]=Vi[0];
          El[1]=Vi[1];
          El[2]=Vi[2];
          Ei=this->Add_Element ( El );
          this->Attach ( Ei );

          //Set intersection coordinates
          for ( i=0 ; i<3 ; ++i )
          {
            Xi[1][i]=Xi[2][i];
          }

          //we are currently at third node
          --Sit;
          Sequence.erase ( Sit );
          --Sequencesize;
          //reposition to first node
          Sit=Sequence.begin();

          for ( i=1 ; i<ll ; ++i )
          {
            ++Sit;
          }
        }
        else //ignore node 1 and construct next triangle
        {
          ++Sit;
          Vi[0]= ( *Sit );
          Vl.SetPos ( Xi[1] );
          Vi[1]=this->Add_Vertex ( Vl,false );
          ++Sit;
          Vi[2]= ( *Sit );

          El[0]=Vi[0];
          El[1]=Vi[1];
          El[2]=Vi[2];
          Ei=this->Add_Element ( El );
          this->Attach ( Ei );
          //reposition to first node
          Sit=Sequence.begin();

          for ( i=1 ; i<ll+2 ; ++i )
          {
            ++Sit;
          }
        }
      }

      if ( cos_angle2 > 1-eps ) //VP2 and VP3 are colinear then compare d3 and d2 to decide
      {
        if ( d3 < d2 ) //construct triangle P1 P2 P3 and remove P2 from sequence
        {
          Vi[0]= ( *Sit );
          ++Sit;
          Vi[1]= ( *Sit );
          ++Sit;
          Vi[2]= ( *Sit );

          El[0]=Vi[0];
          El[1]=Vi[1];
          El[2]=Vi[2];
          Ei=this->Add_Element ( El );
          this->Attach ( Ei );

          //Set intersection coordinates
          for ( i=0 ; i<3 ; ++i )
          {
            Xi[1][i]=Xi[2][i];
          }

          //we are currently at third node
          --Sit;
          Sequence.erase ( Sit );
          --Sequencesize;
          //reposition to first node
          Sit=Sequence.begin();

          for ( i=1 ; i<ll ; ++i )
          {
            ++Sit;
          }
        }

        //else do not do anything as P2 P3 singularity will be caught in next iteration
      }
    }

    if ( V1 < eps )
    {
      if ( d1 > eps && d2 > eps )
      {
        cos_angle= ( VP1x*VP2x + VP1y*VP2y + VP1z*VP2z ) /d1/d2;

        if ( cos_angle < 1-eps ) //construct one triangle
        {
          Vi[0]= ( *Sit );
          ++Sit;
          Vi[1]= ( *Sit );
          Vl.SetPos ( Xi[0] );
          Vi[2]=this->Add_Vertex ( Vl,false );

          El[0]=Vi[0];
          El[1]=Vi[1];
          El[2]=Vi[2];
          Ei=this->Add_Element ( El );
          this->Attach ( Ei );
          //jump to next node and continue
          Sit=Sequence.begin();

          for ( i=1 ; i<ll+1 ; ++i )
          {
            ++Sit;
          }

          ++ll;
          continue;
        }
      }

      //Here vectors from intersection points to sequence points are colinear
      if ( d1 < d2 ) // construct triangle P1 P2 P3 - remove P2 from sequence
      {
        Vi[0]= ( *Sit );
        ++Sit;
        Vi[1]= ( *Sit );
        ++Sit;
        Vi[2]= ( *Sit );

        El[0]=Vi[0];
        El[1]=Vi[1];
        El[2]=Vi[2];
        Ei=this->Add_Element ( El );
        this->Attach ( Ei );

        //Set intersection coordinates
        for ( i=0 ; i<3 ; ++i )
        {
          Xi[1][i]=Xi[2][i];
        }

        //we are currently at third node
        --Sit;
        Sequence.erase ( Sit );
        --Sequencesize;
        //reposition to first node
        Sit=Sequence.begin();

        for ( i=1 ; i<ll ; ++i )
        {
          ++Sit;
        }
      }
      else //ignore P1 and P2
      {
        //Set intersection coordinates
        for ( i=0 ; i<3 ; ++i )
        {
          Xi[0][i]=Xi[1][i];
          Xi[1][i]=Xi[2][i];
        }

        Sit=Sequence.begin();

        for ( i=1 ; i<ll+1 ; ++i )
        {
          ++Sit;
        }
      }
    }

    if ( V2 < eps )
    {
      if ( d3 < d2 ) // construct triangle P1 P2 P3 - remove P2 from sequence
      {
        Vi[0]= ( *Sit );
        ++Sit;
        Vi[1]= ( *Sit );
        ++Sit;
        Vi[2]= ( *Sit );

        El[0]=Vi[0];
        El[1]=Vi[1];
        El[2]=Vi[2];
        Ei=this->Add_Element ( El );
        this->Attach ( Ei );

        //Set intersection coordinates
        for ( i=0 ; i<3 ; ++i )
        {
          Xi[1][i]=Xi[2][i];
        }

        //we are currently at third node
        --Sit;
        Sequence.erase ( Sit );
        --Sequencesize;
        //reposition to first node
        Sit=Sequence.begin();

        for ( i=1 ; i<ll ; ++i )
        {
          ++Sit;
        }
      }
      else // construct triangle P2 P3 P4 - Remove P3 from sequence
      {
        ++Sit;
        Vi[0]= ( *Sit );
        ++Sit;
        Vi[1]= ( *Sit );
        ++Sit;
        Vi[2]= ( *Sit );

        El[0]=Vi[0];
        El[1]=Vi[1];
        El[2]=Vi[2];
        Ei=this->Add_Element ( El );
        this->Attach ( Ei );

        //we are currently at fourth node
        --Sit; //go to P3
        Sequence.erase ( Sit );
        --Sequencesize;
        //reposition to first node
        Sit=Sequence.begin();

        for ( i=1 ; i<ll ; ++i )
        {
          ++Sit;
        }
      }
    }

    if ( V1 > eps && V2 > eps )
    {
      defined_cos = true;

      cos_angle1= ( Vx1*Vx2+Vy1*Vy2+Vz1*Vz2 ) /V1/V2;

      Vx1=Xi[0][0]-Xi[2][0] ;
      Vy1=Xi[0][1]-Xi[2][1] ;
      Vz1=Xi[0][2]-Xi[2][2] ;
      Vx2=Xi[1][0]-Xi[2][0] ;
      Vy2=Xi[1][1]-Xi[2][1] ;
      Vz2=Xi[1][2]-Xi[2][2] ;
      V1=sqrt ( Vx1*Vx1+Vy1*Vy1+Vz1*Vz1 );
      V2=sqrt ( Vx2*Vx2+Vy2*Vy2+Vz2*Vz2 );

      cos_angle2= ( Vx1*Vx2+Vy1*Vy2+Vz1*Vz2 ) /V1/V2;
    }

    altered_sequence = false;

    if ( defined_cos == true )
    {
      if ( cos_angle1 < 0. )
      {
        altered_sequence=true;

        if ( d1 < d2 ) //construct new triangle
        {
          Vi[0]= ( *Sit );
          ++Sit;
          Vi[1]= ( *Sit );
          ++Sit;
          Vi[2]= ( *Sit );

          El[0]=Vi[0];
          El[1]=Vi[1];
          El[2]=Vi[2];
          Ei=this->Add_Element ( El );
          this->Attach ( Ei );

          //Set intersection coordinates
          for ( i=0 ; i<3 ; ++i )
          {
            Xi[1][i]=Xi[2][i];
          }

          //we are currently at third node
          --Sit;
          Sequence.erase ( Sit );
          --Sequencesize;
          //reposition to first node
          Sit=Sequence.begin();

          for ( i=1 ; i<ll ; ++i )
          {
            ++Sit;
          }

          altered_sequence=false;
        }
      }

      if ( cos_angle2 < 0. )
      {
        altered_sequence=true;

        if ( d3 < d2 ) //construct new triangle
        {
          Vi[0]= ( *Sit );
          ++Sit;
          Vi[1]= ( *Sit );
          ++Sit;
          Vi[2]= ( *Sit );

          El[0]=Vi[0];
          El[1]=Vi[1];
          El[2]=Vi[2];
          Ei=this->Add_Element ( El );
          this->Attach ( Ei );

          //Set intersection coordinates
          for ( i=0 ; i<3 ; ++i )
          {
            Xi[1][i]=Xi[2][i];
          }

          //we are currently at third node
          --Sit;
          Sequence.erase ( Sit );
          --Sequencesize;
          //reposition to first node
          Sit=Sequence.begin();

          for ( i=1 ; i<ll ; ++i )
          {
            ++Sit;
          }

          altered_sequence=false;
        }
      }

      if ( altered_sequence == true )
      {
        Sit=Sequence.begin();

        for ( i=1 ; i<ll ; ++i )
        {
          ++Sit;
        }

        //Node 2 is connected to more than a node
        //Scan over triangle_to_delete vertices
        ++Sit;
        Vi[0]= ( *Sit );
        ++Sit;
        Vi[1]= ( *Sit );
        ++Sit;
        Vi[2]= ( *Sit );

        El[0]=Vi[0];
        El[1]=Vi[1];
        El[2]=Vi[2];
        Ei=this->Add_Element ( El );
        this->Attach ( Ei );
        --Sit;
        Sequence.erase ( Sit );
        --Sequencesize;
        //reposition to first node
        Sit=Sequence.begin();

        for ( i=1 ; i<ll ; ++i )
        {
          ++Sit;
        }

        altered_sequence=false;
      }
    }

    //Now construct obvious triangles

    Vi[0]= ( *Sit );
    Vl.SetPos ( Xi[0] );
    Vi[1]=this->Add_Vertex ( Vl,false );
    ++Sit;      //-----------------------------------P2
    Vi[2]= ( *Sit );
    //construct triangle only if distance from Vi1 to Vi0 is > eps
    coord= ( *Vi[0] ).GetPos();
    x1=coord[0] ;
    y1=coord[1] ;
    z1=coord[2] ;
    coord= ( *Vi[1] ).GetPos();
    x2=coord[0] ;
    y2=coord[1] ;
    z2=coord[2] ;
    distance=sqrt ( pow ( ( x2-x1 ),2 ) + pow ( ( y2-y1 ),2 ) + pow ( ( z2-z1 ),2 ) );

    if ( distance > eps )
    {
      El[0]=Vi[0];
      El[1]=Vi[1];
      El[2]=Vi[2];
      Ei=this->Add_Element ( El );
      this->Attach ( Ei );
    }

    Vi[0]= ( *Sit );
    Vl.SetPos ( Xi[1] );
    Vi[1]=this->Add_Vertex ( Vl,false );
    Vl.SetPos ( Xi[0] );
    Vi[2]=this->Add_Vertex ( Vl,false );

    coord= ( *Vi[0] ).GetPos();
    x1=coord[0] ;
    y1=coord[1] ;
    z1=coord[2] ;
    coord= ( *Vi[1] ).GetPos();
    x2=coord[0] ;
    y2=coord[1] ;
    z2=coord[2] ;
    distance=sqrt ( pow ( ( x2-x1 ),2 ) + pow ( ( y2-y1 ),2 ) + pow ( ( z2-z1 ),2 ) );

    if ( distance > eps )
    {
      El[0]=Vi[0];
      El[1]=Vi[1];
      El[2]=Vi[2];
      Ei=this->Add_Element ( El );
      this->Attach ( Ei );
    }

    ++ll;
  }

  if ( closed_loop == true )
  {
    Sequence.pop_back();
    Sequence.pop_back();
  }
  else
  {
    //construct last triangle
    Vertex_Iterator Vit[2][3];  // 2 by 3 nodes sequence
    double X3[2][3];
    Sit=Sequence.end();
    --Sit;
    Vi[0]= ( *Sit );
    Vl.SetPos ( Xi[1] );
    Vi[1]=this->Add_Vertex ( Vl,false );
    --Sit;
    Vi[2]= ( *Sit );

    coord= ( *Vi[0] ).GetPos();
    x1=coord[0] ;
    y1=coord[1] ;
    z1=coord[2] ;
    coord= ( *Vi[1] ).GetPos();
    x2=coord[0] ;
    y2=coord[1] ;
    z2=coord[2] ;
    distance=sqrt ( pow ( ( x2-x1 ),2 ) + pow ( ( y2-y1 ),2 ) + pow ( ( z2-z1 ),2 ) );

    if ( distance > eps )
    {
      El[0]=Vi[0];
      El[1]=Vi[1];
      El[2]=Vi[2];
      Ei=this->Add_Element ( El );
      this->Attach ( Ei );
    }

    //Now get a last triangle to avoid the overcut

    Sit=Sequence.begin();
    Vit[0][1]= ( *Sit );
    Sit=Sequence.end();
    --Sit;
    Vit[1][1]= ( *Sit );

    for ( i=0; i<2 ; ++i )
    {
      Bips.clear();
      this->Get_Bipoints ( Vit[i][1] , Bips );

      for ( Bips_it=Bips.begin() ; Bips_it!=Bips.end() ; ++Bips_it )
      {
        if ( ( * ( *Bips_it ) ).Get_Number_Elements() == 1 )
        {

          Pb=bipoint_at_boundary.insert ( *Bips_it );
          ( * ( *Bips_it ) ).Get_Vertices ( bver );

          if ( Pb.second )
          {
            bipoint_at_boundary.erase ( *Bips_it );

            if ( bver[0] != Vit[i][1] )
            {
              coord= ( *bver[0] ).GetPos();
              Vit[i][2]=bver[0];
            }
            else
            {
              coord= ( *bver[1] ).GetPos();
              Vit[i][2]=bver[1];
            }

            X3[i][0]=coord[0] ;
            X3[i][1]=coord[1] ;
            X3[i][2]=coord[2] ;
          }
          else
          {
            if ( bver[0] != Vit[i][1] )
            {
              Vit[i][0]=bver[0];
            }
            else
            {
              Vit[i][0]=bver[1];
            }
          }
        }
      }
    }

    for ( i=0; i<2 ; ++i )
    {
      x3=X3[i][0] ;
      y3=X3[i][1] ;
      z3=X3[i][2] ;

      coord= ( *Vit[i][0] ).GetPos();
      x1=coord[0];
      y1=coord[1];
      z1=coord[2];
      coord= ( *Vit[i][1] ).GetPos();
      x2=coord[0];
      y2=coord[1];
      z2=coord[2];

      den= ( x2-x1 ) * ( x2-x1 ) + ( y2-y1 ) * ( y2-y1 ) + ( z2-z1 ) * ( z2-z1 );

      if ( den < eps )
      {
        continue ;
      }

      den= ( ( x3-x1 ) * ( x2-x1 ) + ( y3-y1 ) * ( y2-y1 ) + ( z3-z1 ) * ( z2-z1 ) ) / den ;

      if ( den < 0. || den > 1 )
      {
        x=x1+den* ( x2-x1 ) ;
        y=y1+den* ( y2-y1 ) ;
        z=z1+den* ( z2-z1 ) ;
        Vl[0]=x ;
        Vl[1]=y ;
        Vl[2]=z ;

        dist=sqrt ( pow ( ( x3-Vl[0] ),2 ) + pow ( ( y3-Vl[1] ),2 ) + pow ( ( z3-Vl[2] ),2 ) ) ;

        if ( dist < eps )
        {
          continue;
        }

        Vi[2]=this->Add_Vertex ( Vl,false );

        El[0]=Vit[i][1];
        El[1]=Vit[i][2];
        El[2]=Vi[2];

        Ei=this->Add_Element ( El );
        this->Attach ( Ei );
      }
    }

  }

  //Erase all altered triangles
  for ( tt=triangle_init.begin(); tt!=triangle_init.end() ; ++tt )
  {
    this->Delete_Element ( *tt );
  }

  cout << "INJECTION HOLE SUCCESSFULLY INSERTED " << endl;
  return 1;
}




template<int nbn,int dim,class E>
int Mesh_LCM_Tools<nbn,dim,E>::Insert_Trench ( Vertex_Type node_1, Vertex_Type node_2, double width )
{
  Element_Iterator ele;
  Vertex_Iterator ver[3];

  //list<Element_Iterator> triangle_init;
  //typedef typename list<Element_Iterator>::iterator triangle_init_it;
  //triangle_init_it tt;

  Iterator_Container<Element_Iterator> triangle_init;
  typedef typename Iterator_Container<Element_Iterator>::iterator triangle_init_it;
  triangle_init_it tt;

  double triangle_vertices_coord[3][3];
  const double * coord;
  double lambda,den;
  double A,B,C,D;
  double An,Bn,Cn,Dn;
  double x1,y1,z1;
  double x2,y2,z2;
  double x3,y3,z3;
  double xp1,yp1,zp1;
  double xp2,yp2,zp2;
  double param[3];
  bool ItIsInside;
  double node_temp[3];
  double ref_plane[4];
  double P[4][3]; //trench node coordinates (P[node #][coordinate])
  double PL[4][4]; //trench plane coefficients (PL[plane #][coefficients]
  double pos[3][4]; // triangle nodes position with respect to each plane
  int nbnodesperelement;
  nbnodesperelement=Element_Type::Nb_Nodes;


//----------------------------------------------------------------------------------------------------------
//  STEP 1: LOCATE INPUT NODES ON CURRENT GEOMETRY
//----------------------------------------------------------------------------------------------------------
  for ( ele=this->Get_Elements_Start(); ele!=this->Get_Elements_End(); ++ele )
  {
    ( *ele ).Get_Vertices ( ver );

    for ( int i=0; i<nbnodesperelement; ++i )
    {
      coord= ( *ver[i] ).GetPos();

      for ( int j=0; j < 3; ++j )
      {
        triangle_vertices_coord[i][j]=coord[j];
      }
    }

    //rename coordinates
    x1=triangle_vertices_coord[0][0] ;
    y1=triangle_vertices_coord[0][1] ;
    z1=triangle_vertices_coord[0][2] ;
    x2=triangle_vertices_coord[1][0] ;
    y2=triangle_vertices_coord[1][1] ;
    z2=triangle_vertices_coord[1][2] ;
    x3=triangle_vertices_coord[2][0] ;
    y3=triangle_vertices_coord[2][1] ;
    z3=triangle_vertices_coord[2][2] ;
    //Get plane coefficients
    A =  y1* ( z2 - z3 ) + y2* ( z3 - z1 ) + y3* ( z1 - z2 );
    B =  z1* ( x2 - x3 ) + z2* ( x3 - x1 ) + z3* ( x1 - x2 );
    C =  x1* ( y2 - y3 ) + x2* ( y3 - y1 ) + x3* ( y1 - y2 );
    D = - ( x1* ( y2*z3 - y3*z2 ) + x2* ( y3*z1 - y1*z3 ) + x3* ( y1*z2 - y2*z1 ) );

    //FIRST NODE
    //get a translated node from node_1 along plane normal
    node_temp[0]=node_1[0]+A;
    node_temp[1]=node_1[1]+B;
    node_temp[2]=node_1[2]+C;


    xp1=node_temp[0] ;
    yp1=node_temp[1] ;
    zp1=node_temp[2] ;
    xp2=node_1[0] ;
    yp2=node_1[1] ;
    zp2=node_1[2] ;
    // Check for Intersection
    den= ( xp1-xp2 ) *A+ ( yp1-yp2 ) *B+ ( zp1-zp2 ) *C;

    if ( den == 0. )
    {
      continue;
    }
    else
    {
      lambda= ( xp1*A+yp1*B+zp1*C+D ) /den;
    }

    node_temp[0]=xp1+lambda* ( xp2-xp1 ) ;
    node_temp[1]=yp1+lambda* ( yp2-yp1 ) ;
    node_temp[2]=zp1+lambda* ( zp2-zp1 ) ;

    //process only if distance is less or equal than previous distance processed
    // Check if intersection is inside triangle
    ItIsInside=false;
    ItIsInside=Is_Inside_Triangle3D ( node_temp,triangle_vertices_coord,param );

    if ( ItIsInside )
    {
      //correct node_1
      node_1[0]=node_temp[0] ;
      node_1[1]=node_temp[1] ;
      node_1[2]=node_temp[2] ;
      //store ref_plane coefficients
      ref_plane[0]=A ;
      ref_plane[1]=B ;
      ref_plane[2]=C ;
      ref_plane[3]=D ;
    }

    //SECOND NODE
    node_temp[0]=node_2[0]+A;
    node_temp[1]=node_2[1]+B;
    node_temp[2]=node_2[2]+C;

    xp1=node_temp[0] ;
    yp1=node_temp[1] ;
    zp1=node_temp[2] ;
    xp2=node_2[0] ;
    yp2=node_2[1] ;
    zp2=node_2[2] ;
    // Check for Intersection
    den= ( xp1-xp2 ) *A+ ( yp1-yp2 ) *B+ ( zp1-zp2 ) *C;

    if ( den == 0. )
    {
      continue;
    }
    else
    {
      lambda= ( xp1*A+yp1*B+zp1*C+D ) /den;
    }

    node_temp[0]=xp1+lambda* ( xp2-xp1 ) ;
    node_temp[1]=yp1+lambda* ( yp2-yp1 ) ;
    node_temp[2]=zp1+lambda* ( zp2-zp1 ) ;

    //process only if distance is less or equal than previous distance processed
    // Check if intersection is inside triangle
    ItIsInside=false;
    ItIsInside=Is_Inside_Triangle3D ( node_temp,triangle_vertices_coord,param );

    if ( ItIsInside )
    {
      //correct node_2
      node_2[0]=node_temp[0] ;
      node_2[1]=node_temp[1] ;
      node_2[2]=node_temp[2] ;
      cout << "Found second node position on triangle " << endl;
    }
  }

  //TO DO : check if nodes are not on gemetry
  // if they are not start subdividing the segments and create new positions
  //till a triangle is hit and select its plane as a reference
//  for(tt=triangle_init.begin(); tt!=triangle_init.end(); ++tt)
//  {
//      Detach(*tt);
//      Elements.erase(*tt);
//  }
//  return 0;
//
// END STEP 1
//-----------------------------------------------------------------------------------------------

//-----------------------------------------------------------------------------------------------
//  STEP 2: TRANSLATE NEW NODES ALONG REF. TRI. PLANE, BY WIDTH
//-----------------------------------------------------------------------------------------------

  double wd;
  //Recreate translated node along ref_plane
  node_temp[0]=node_1[0]+ref_plane[0];
  node_temp[1]=node_1[1]+ref_plane[1];
  node_temp[2]=node_1[2]+ref_plane[2];

  cout << "Node temp at 1 " <<
       setprecision ( 6 ) << node_temp[0] << " " << node_temp[1] << " " << node_temp[2] << " " << endl;

  //Get plane coeff. perpendicular to ref_plane
  x1=node_1[0] ;
  y1=node_1[1] ;
  z1=node_1[2] ;
  x2=node_2[0] ;
  y2=node_2[1] ;
  z2=node_2[2] ;
  x3=node_temp[0] ;
  y3=node_temp[1] ;
  z3=node_temp[2] ;

  //Get normal plane coefficients
  An =  y1* ( z2 - z3 ) + y2* ( z3 - z1 ) + y3* ( z1 - z2 );
  Bn =  z1* ( x2 - x3 ) + z2* ( x3 - x1 ) + z3* ( x1 - x2 );
  Cn =  x1* ( y2 - y3 ) + x2* ( y3 - y1 ) + x3* ( y1 - y2 );
  Dn = - ( x1* ( y2*z3 - y3*z2 ) + x2* ( y3*z1 - y1*z3 ) + x3* ( y1*z2 - y2*z1 ) );

  //translate node_1 and node_2 by width/2 along the above plane normal
  //Create the 4 points of the trench
  wd=width/sqrt ( An*An+Bn*Bn+Cn*Cn ) /2;

  P[0][0]=node_1[0]+wd*An ;
  P[0][1]=node_1[1]+wd*Bn ;
  P[0][2]=node_1[2]+wd*Cn ;
  P[1][0]=node_2[0]+wd*An ;
  P[1][1]=node_2[1]+wd*Bn ;
  P[1][2]=node_2[2]+wd*Cn ;
  P[2][0]=node_2[0]-wd*An ;
  P[2][1]=node_2[1]-wd*Bn ;
  P[2][2]=node_2[2]-wd*Cn ;
  P[3][0]=node_1[0]-wd*An ;
  P[3][1]=node_1[1]-wd*Bn ;
  P[3][2]=node_1[2]-wd*Cn ;

  //Get trench planes coefficients
  cout << "Width is " << width << " " << An << " " << Bn << " " << Cn << endl;

  for ( int j=0; j<4; ++j )
  {
    cout << " Translated " << setprecision ( 6 ) << P[j][0] << " " << P[j][1] << " " << P[j][2] << " " << endl;

    for ( int i=0; i<3; ++i )
    {
      node_temp[i]=P[j][i]+ref_plane[i]; //translate P node along ref_plane normal
    }

    //Construct coefficients
    x1=P[j][0] ;
    y1=P[j][1] ;
    z1=P[j][2] ;
    x2=P[ ( j+1 ) %4][0] ;
    y2=P[ ( j+1 ) %4][1] ;
    z2=P[ ( j+1 ) %4][2] ;
    x3=node_temp[0] ;
    y3=node_temp[1] ;
    z3=node_temp[2] ;
    //Get plane coefficients
    PL[j][0] =  y1* ( z2 - z3 ) + y2* ( z3 - z1 ) + y3* ( z1 - z2 );
    PL[j][1] =  z1* ( x2 - x3 ) + z2* ( x3 - x1 ) + z3* ( x1 - x2 );
    PL[j][2] =  x1* ( y2 - y3 ) + x2* ( y3 - y1 ) + x3* ( y1 - y2 );
    PL[j][3] = - ( x1* ( y2*z3 - y3*z2 ) + x2* ( y3*z1 - y1*z3 ) + x3* ( y1*z2 - y2*z1 ) );
  }

//
// END STEP 2
//-------------------------------------------------------------------------------------------

//-------------------------------------------------------------------------------------------
//  STEP 3: Locate mesh triangle nodes with respect to each plane
//-------------------------------------------------------------------------------------------

  int ncount;
  Vertex_Type Vl;
  Element_Type El;
  Element_Iterator EI;
  Vertex_Iterator VI[3];
  int ip,i,j;

  //Load triangle init with all current elements
  for ( ele=this->Get_Elements_Start() ; ele != this->Get_Elements_End(); ++ele )
  {
    triangle_init.insert ( ele );
  }

  for ( ele=this->Get_Elements_Start() ; ele != this->Get_Elements_End() ; ++ele ) //Loop over all triangles
  {
    ( *ele ).Get_Vertices ( ver );

    for ( ip=0; ip<4 ; ++ip ) //with respect to each plane
    {
      for ( i=0; i<nbnodesperelement; ++i ) //check all triangle nodes
      {
        coord= ( *ver[i] ).GetPos(); //get node coordinates
        pos[i][ip]=0.;

        for ( j=0; j < 3; ++j ) //Load coordinates
        {
          pos[i][ip]=pos[i][ip]+coord[j]*PL[ip][j];
        }

        pos[i][ip]=pos[i][ip]+PL[ip][3];
      }
    }

    for ( ip=0; ip<4; ++ip )
    {
      if ( pos[0][ip] > 0 && pos[1][ip] > 0 && pos[2][ip] > 0 )
      {
        triangle_init.erase ( ele );
        break;
      }
    }

    ncount=0;

    for ( ip=0; ip<4; ++ip )
    {
      if ( pos[0][ip] < 0 && pos[1][ip] < 0 && pos[2][ip] < 0 )
      {
        ncount=ncount+1;
      }

      if ( ncount == 4 )
      {
        triangle_init.erase ( ele );
      }

    }

    //Now triangle_init contains only the elements that intersect the 4 planes
  }


//Now work within trangle_init to change mesh
  bool attached;
  int somme[3] , all_somme, nadd;
  double N[3][4][3], side[3][4];
  double mu[3][4];
  double eps=1.e-12;
  int jj,nn;
  // N[side#][plane#][coordinates] ; side[side#][plane#]

  for ( tt=triangle_init.begin() ; tt!=triangle_init.end() ; ++tt ) //Loop over all triangles
  {
    ( * ( *tt ) ).Get_Vertices ( ver );

    for ( j=0; j<3 ; ++j )
    {
      for ( jj=0; jj<4; ++jj )
      {
        side[j][jj]=0;
      }
    }//Initialize side intersec. indices

    for ( ip=0; ip<4 ; ++ip ) //check intersection with respect to each plane
    {
      //cout << "Getting intersection with planes " << ip << endl;
      for ( i=0; i<3; ++i ) //check all triangle sides
      {
        //cout << "checking for triangle side " << i << endl;

        coord= ( *ver[i] ).GetPos(); //get node coordinates
        xp1=coord[0] ;
        yp1=coord[1] ;
        zp1=coord[2] ;

        coord= ( *ver[ ( i+1 ) %3] ).GetPos();
        xp2=coord[0] ;
        yp2=coord[1] ;
        zp2=coord[2] ;

        //Get intersection of the above node with plane ip
        A=PL[ip][0] ;
        B=PL[ip][1] ;
        C=PL[ip][2];
        D=PL[ip][3];
        den= ( xp1-xp2 ) *A+ ( yp1-yp2 ) *B+ ( zp1-zp2 ) *C;

        if ( den == 0. ) //normal to plane parallel to triangle bipoint
        {
          continue;
        }
        else
        {
          mu[i][ip]= ( xp1*A+yp1*B+zp1*C+D ) /den;
          //cout << "Intersection detected " << mu[i][ip] <<  endl;
        }

        //Get cases where intersection is at side end and act dependently
        if ( mu[i][ip] >= 0. && mu[i][ip] < 1. ) //Intersection on line segment
        {
          side[i][ip]=1; //load side number
          N[i][ip][0]=xp1+mu[i][ip]* ( xp2-xp1 ) ;
          N[i][ip][1]=yp1+mu[i][ip]* ( yp2-yp1 ) ;
          N[i][ip][2]=zp1+mu[i][ip]* ( zp2-zp1 ) ;
        }
      }
    }

    //initialize somme and all_somme
    all_somme=0;

    //load somme information
    for ( j=0; j<3 ; ++j )
    {
      somme[j]=0;

      for ( int jj=0; jj<4; ++jj )
      {
        somme[j]=somme[j]+side[j][jj];
      }

      all_somme=all_somme+somme[j];
    }

    if ( all_somme%2 == 1 || all_somme == 0 )
    {
      cout << "Facing a logic error in algorithm: Contact developer " << all_somme << endl;
      return -1;
    }

    //Go for the case by case
    switch ( all_somme )
    {
      case 2: //should be the easiest case
      {
        //cout << "Treating case 2 " << endl;
        //Check if two sides are involved or just one side
        for ( j=0; j<3 ; ++j )
        {
          if ( somme[j] == 2 )
          {
          }

          attached=false;

          if ( somme[j]==1 && somme[ ( j+1 ) %3]==1 )
          {
            //cout << "Different sides involved " << endl;
            //Get the plane again
            for ( jj=0; jj<4; ++jj )
            {
              nadd=0;

              if ( side[j][jj] == 1 && side[ ( j+1 ) %3][jj] == 1 )
              {
                for ( nn=0; nn<3; ++nn )
                {
                  Vl[nn]=N[j][jj][nn] ;
                }

                VI[0]=this->Add_Vertex ( Vl,false );

                for ( nn=0; nn<3; ++nn )
                {
                  Vl[nn]=N[ ( j+1 ) %3][jj][nn] ;
                }

                VI[1]=this->Add_Vertex ( Vl,false );
                nadd=1;
                break;
              }
            }

            if ( nadd != 1 ) //Just checking for logic errors
            {
              cout << "Incompatible result: Check algorithm " << endl;
              return -1;
            }

            //first triangle
            //Detach(*tt);
            //Elements.erase(*tt);
//TO DO : check in and out desired region with repect to eps
            if ( fabs ( mu[j][jj] ) < eps || fabs ( mu[ ( j+1 ) %3][jj] ) < eps )
            {
              El[0]=VI[j];
              El[1]=VI[1];
              El[2]=ver[ ( j+1 ) %3];
              EI=this->Add_Element ( El );
              this->Attach ( EI );
              El[0]=VI[j];
              El[1]=VI[1];
              El[2]=ver[ ( j+2 ) %3];
              EI=this->Add_Element ( El );
              this->Attach ( EI );
              attached=true;
              break;
            }

            El[0]=VI[0];
            El[1]=VI[1];
            El[2]=ver[ ( j+1 ) %3];
            EI=this->Add_Element ( El );
            this->Attach ( EI );

            El[0]=VI[0];
            El[1]=ver[ ( j+2 ) %3];
            El[2]=ver[j];
            EI=this->Add_Element ( El );
            this->Attach ( EI );

            El[0]=VI[1];
            El[1]=ver[ ( j+2 ) %3];
            El[2]=VI[0];
            EI=this->Add_Element ( El );
            this->Attach ( EI );

            attached=true;
            break;
          }

          if ( attached==true )
          {
            break;
          }
        }
      }
      break;

      case 4:
      {
      }
      break;

      case 6:
      {
      }
      break;
    }
  }

  //return 0;
  //At this point intersections and their coordinatea have been all loaded
  //All needed information is in N tensor and side matrix
  //1) check for errors: get total # of intersections and treat different cases with a switch




  //Detach and erase elements to check with tecplot
  for ( tt=triangle_init.begin(); tt!=triangle_init.end(); ++tt )
  {
    this->Detach ( *tt );
  }

  for ( tt=triangle_init.begin(); tt!=triangle_init.end(); ++tt )
  {
    this->Delete_Element ( *tt );
  }

  cout << "Exiting Insert trench method " << endl;


// END STEP 3
//-------------------------------------------------------------------------------------------

  return 0;
}



template<int nbn,int dim,class E>
int Mesh_LCM_Tools<nbn,dim,E>::Insert_Side_Effect ( double sizet,double sizen,double thk,Vertex_Type tabV[],int numpairs,int zone )
{
  Multiline_Metric<Mesh_LCM_Tools<3,3>::Vertex_Type> MLM ( sizet,sizen,thk,tabV,numpairs );

  Metric_Field<Vertex_Type> *ptr;
  ptr=this->Get_Metric_Field();
  this->Set_Metric_Field ( MLM );
  this->Do_Mesh();
  Set_Metric_Field ( *ptr );

  Element_Iterator ite;
  Vertex_Iterator itv;
  Vertex_Type V1,V2;

  int k;
  bool inside=true;
  double d;

  if ( zone<0 )
  {
    for ( ite=this->Get_Elements_Start(); ite!=this->Get_Elements_End(); ++ite )
    {
      int z;
      z= ( *ite ).Get_Zone();

      if ( zone<z )
      {
        zone=z;
      }
    }

    zone++;
  }

  for ( ite=this->Get_Elements_Start(); ite!=this->Get_Elements_End(); ++ite )
  {
    inside=true;

    for ( k=0; k<3; k++ )
    {
      itv= ( *ite ) [k];
      d=MLM.FindClosest ( *itv,V1,V2 );

      if ( d>thk )
      {
        inside=false;
        break;
      }
    }

    if ( inside )
    {
      ( *ite ).Set_Zone ( zone );
    }
  }

  return 1;
}


#endif