/*
    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 FASTMARCHING_H
#define FASTMARCHING_H

#include <cassert>
#include <set>
#include <map>
#include <vector>
#include <algorithm>
#include <stdio.h>
#include <math.h>
#include <iostream>
#include <fstream>
#include <cassert>
#include "SpeedFunctor.h"
#include "ExportFunctor.h"
#include "FM3D.h"


using namespace std;

namespace FM
{
  template<class N,class E,class I> class FastMarching_c
  {
  public:
    /// FieldPair type : 1st double distance, 2nd other element
    typedef pair<double,I> FieldPair;

    FastMarching_c ( const SpeedFunctor<E> &speed )
    {
      speedf=&speed;
    }
    void Clear ( void )
    {
      locked.clear();
      lockedn.clear();
      bnd.clear();
      bndn.clear();
      class_val_nodes.clear();
      class_val_times.clear();
      lstime.clear();
    }
    void Init ( map<N,FieldPair> &initvals,set<E> &bndinit );
    void Init ( FastMarching_c & other)
    {
      locked=other.locked;
      bnd=other.bnd;
      lockedn=other.lockedn;
      bndn=other.bndn;
      class_val_nodes=other.class_val_nodes;
      lstime=other.lstime;
      speedf=other.speedf;
      ef=other.ef;
    }
    void Propagate ( int factor=1 );
    virtual void Export ( const char *name );
    virtual void Export ( std::map<N, FieldPair> &dest);
    virtual void Export_Debug ( set<E> &tab,const char *name );
    virtual void SetExportFunctor ( ExportFunctor<N,FieldPair>& efp )
    {
      ef=&efp;
    }
    FieldPair operator() ( const N& node ) const
    {
      typename map<N,FieldPair>::const_iterator p=lstime.find ( node );

      if ( p!=lstime.end() )
      {
        return p->second;
      }
      else
      {
        return  FieldPair ( 0.,I() );
      }
    }
  protected:
    virtual double ComputeTime ( N &node,E &element,vector<N> &base,int &res,I &trans );

    struct node_struct
    {
      N node;
      double time;
      node_struct ( void ) {}
      node_struct ( N& nodep,double timep ) :node ( nodep ),time ( timep ) {}
    };
    struct info_struct
    {
      set<E> elements;
      int res;
      I trans;
      info_struct ( void ) {}
      info_struct ( E& elementp,int resp, I transp ) :res ( resp ),trans ( transp )
      {
        elements.insert ( elementp );
      }
    };

    struct LessTimeNode
    {
      bool operator () ( const node_struct& n1,const node_struct& n2 ) const
      {
        if ( n1.time==n2.time )
        {
          return ( n1.node<n2.node );
        }
        else
        {
          return ( fabs ( n1.time ) <fabs ( n2.time ) );
        }
      }
    };

  private:
    bool UpdateTimeNode ( N &node,E &element, double time,int factor,int res,I trans );


    set<E> locked;
    set<E> bnd;
    set<N> lockedn;
    set<N> bndn;

    map<N,FieldPair> class_val_nodes;
    map<node_struct,info_struct,LessTimeNode> class_val_times;
    map<N,FieldPair> lstime;
    const SpeedFunctor<E>* speedf;
    ExportFunctor<N,FieldPair>* ef;
  };

 
  template<class N,class E,class I> void FastMarching_c<N,E,I>::Init ( map<N,FieldPair> &initvals,set<E> &bndinit )
  {
    lstime=initvals;
    bnd=bndinit;
    locked=bndinit;
    typename set<E>::iterator itbnd;
    set<N> newnodes;

    for ( itbnd=bnd.begin(); itbnd!=bnd.end(); ++itbnd )
    {
      E ele=*itbnd;
      vector<N> ele_nodes;
      ele.GetNodes ( ele_nodes );
      typename vector<N>::iterator itn;

      for ( itn=ele_nodes.begin(); itn!=ele_nodes.end(); ++itn )
      {
        N node=*itn;
        lockedn.insert ( node );
        bndn.insert ( node );
      }
    }

//    cout << bnd.size() << " unique elements" << endl;
//    cout << bndn.size() << " unique nodes" << endl;
//    cout << lockedn.size() << " locked nodes" << endl;
  }

  /**
   * Export private map lstim into map dest
   * 
   * @param[in] dest map<N,FieldPair>
  */
  template<class N,class E,class I> void FastMarching_c<N,E,I>::Export ( std::map<N, FieldPair> &dest)
  {
    dest=lstime;
  }
  
  template<class N,class E,class I> void FastMarching_c<N,E,I>::Export ( const char *name )
  {
    for ( typename map<N,FieldPair>::iterator it=lstime.begin(); it!=lstime.end(); it++ )
    {
      ( *ef ) ( it->first ) =it->second;
    }

    ( *ef ).Write ( name );
    ( *ef ).Clear();
  }

  template<class N,class E,class I> void FastMarching_c<N,E,I>::Export_Debug ( set<E> &tab,const char *name )
  {
    for ( typename set<E>::iterator it=tab.begin(); it!=tab.end(); it++ )
    {
      E ele=*it;
      vector<N> tabv;
      ele.GetNodes ( tabv );

      for ( typename vector<N>::iterator it2=tabv.begin(); it2!=tabv.end(); it2++ )
      {
        ( *ef ) ( *it2 ) = ( *this ) ( *it2 );
      }
    }

    ( *ef ).Write ( name );
    //   (*ef).Clear();
  }


  template<class N,class E,class I> double FastMarching_c<N,E,I>::ComputeTime ( N &node,E &element,vector<N> &base,int &res,I& trans )
  {
    double x,y,z;
    int s=base.size();
    int ss=0;
    bool pos=false;
    bool neg=false;
    int flag=1;
    double P[5][3],t[5], v;
    I tr[5];
    double alpha,beta;

    node.GetPos ( x,y,z );
    P[4][0]=x;
    P[4][1]=y;
    P[4][2]=z;

    for ( int c=0; c<s; ++c )
    {
      base[c].GetPos ( x,y,z );
      P[c][0]=x;
      P[c][1]=y;
      P[c][2]=z;

      if ( lstime.find ( base[c] ) !=lstime.end() )
      {
        FieldPair ti=lstime[base[c]];
        t[c]=ti.first;
        tr[c]=ti.second;

        if ( t[c]>0 )
        {
          pos=true;
        }

        if ( t[c]<0 )
        {
          neg=true;
        }
      }
      else if ( class_val_nodes.find ( base[c] ) !=class_val_nodes.end() )
      {
        FieldPair ti=class_val_nodes[base[c]];

        if ( flag>0 )
        {
          flag=-1;
        }

        t[c]=ti.first;
        tr[c]=ti.second;

        if ( t[c]>0 )
        {
          pos=true;
        }

        if ( t[c]<0 )
        {
          neg=true;
        }
      }
      else
      {
        t[c]=0.0;
        tr[c]=I();
        flag=0;
      }
    }

    if ( pos )
    {
      v= ( *speedf ) ( element );
    }
    else
    {
      v=- ( *speedf ) ( element );
    }

    if ( flag )
    {
      assert ( pos!=neg );

      if ( s==2 )
      {
        res=flag*val2d ( P[0][0],P[0][1],P[0][2],P[1][0],P[1][1],P[1][2],P[4][0],P[4][1],P[4][2],t[0],t[1],v,t[4],alpha );
        trans=tr[0]* ( 1-alpha ) +tr[1]*alpha;
        return ( t[4] );
      }
      else if ( s==3 )
      {
        res=flag*val3d ( P[0][0],P[0][1],P[0][2],P[1][0],P[1][1],P[1][2],P[2][0],P[2][1],P[2][2],P[4][0],P[4][1],P[4][2],t[0],t[1],t[2],v,t[4],alpha,beta );
        trans=tr[0]* ( 1-alpha-beta ) +tr[1]*alpha+tr[2]*beta;
        return ( t[4] );
      }
      else
      {
        assert ( 1==0 );
      }
    }
    else
    {
      res=32; // erreur
      t[4]=0;
      trans=I();
      assert ( 1==0 );
    }
    return 0;
  }

  template<class N,class E,class I> bool FastMarching_c<N,E,I>::UpdateTimeNode ( N &node,E &element,double time,int factor,int res,I trans )
  {
    node_struct nns ( node,time );
    info_struct ins ( element,res,trans );

    pair<N,FieldPair> pns ( node,FieldPair ( time,trans ) );
    pair<typename map<N,FieldPair>::iterator,bool> itn;
    itn=class_val_nodes.insert ( pns );

    if ( itn.second )
    {
      class_val_times[nns]=ins;
      return true;
    }
    else
    {
      double timeprev= ( * ( itn.first ) ).second.first;
      node_struct nns2 ( node,timeprev );
      typename map<node_struct,info_struct>::iterator its=class_val_times.find ( nns2 );
      its->second.elements.insert ( element );

//      ins.elements.insert(element);
      if ( fabs ( timeprev*factor ) >fabs ( time*factor ) )
      {
        ( * ( itn.first ) ).second.first=time;
        ( * ( itn.first ) ).second.second=trans;
        ins.elements=its->second.elements;
        class_val_times.erase ( its );
//  ins.elements.insert(element);
        class_val_times[nns]=ins;
      }

      return false;
    }
  }




#if 1

  template<class N,class E,class I> void FastMarching_c<N,E,I>::Propagate ( int factor )
  {
    int layer =0;
//#define DEBUG_FM 1

    while ( true )
    {
      typename set<E>::iterator itbnd;
      set<E> bndold;

      set<N> newnodes;

//     cout << layer << endl;
      while ( bnd.size() >0 )
      {
        itbnd=bnd.begin();
        E ele=*itbnd;
        bnd.erase ( itbnd );
        bndold.insert ( ele );
        vector<E> tab_nei;
        ele.GetNeighbors ( tab_nei );

        for ( int e=0; e<tab_nei.size(); ++e )
        {
          E ele2=tab_nei[e];

          if ( locked.find ( ele2 ) ==locked.end() )
          {
            int d=ele2.GetDim();
            vector<N> commons,notcommons;
            ele2.CommonNodes ( ele,commons,notcommons );
            bool allset=true;

            for ( int nc=0; nc<notcommons.size(); ++nc )
            {
              if ( lstime.find ( notcommons[nc] ) ==lstime.end() )
              {
                allset=false;
                int res=0;
                I trans;
                double t=ComputeTime ( notcommons[nc],ele2,commons,res,trans );
                bool result=UpdateTimeNode ( notcommons[nc],ele2,t,factor,res,trans );

                if ( result )
                {
                  newnodes.insert ( notcommons[nc] );
                }
              }
            }

            if ( allset )
            {
              locked.insert ( ele2 );
              bnd.insert ( ele2 );
//        cout << "locked" << endl;
            }

// else plus bas .....
#if 0
            else
            {
              typename map<N,FieldPair>::iterator it,beg=class_val_nodes.begin(),end=class_val_nodes.end();

              //          cout << " nodes " <<  class_val_nodes.size() << " " << class_val_times.size() << endl;
              for ( it=beg; it!=end; it++ )
              {
                N node=it->first;
                double tt=it->second.first;
                node_struct ns ( node,tt );
                typename map<node_struct,info_struct,LessTimeNode>::iterator it2;
                it2=class_val_times.find ( ns );
                int oldres=1;

                if ( it2!=class_val_times.end() )
                {
                  oldres=it2->second.res;
                }

                if ( oldres!=0 )
                {
                  int res=0;
                  I trans;
                  double t=ComputeTime ( node,ele2,commons,res,trans );
                  double x,y,z;
                  node.GetPos ( x,y,z );

                  if ( ( fabs ( x+0.214928854871447 ) +fabs ( y-0.02 ) +fabs ( z-0.01381080934510988 ) ) <1e-5 )
                  {
                    cout << "node not yet updated" << endl;
                    cout << "t=" << t <<endl;
                    cout << "res=" << res << endl;

                    for ( typename vector<N>::iterator it=commons.begin(); it!=commons.end(); ++it )
                    {
                      it->Print();
                    }
                  }

                  if ( res==0 )
                  {
                    E ele3=* ( it2->second.elements.begin() );
                    cout << "*";
                    UpdateTimeNode ( node,ele3,t,factor,1,trans );
                  }
                }
              }
            }

#endif







          }
        }
      }

#if 0
#ifdef DEBUG_FM
      bool doit=true;
      char n1[100];
      sprintf ( n1,"debug%d.pos",layer );
      ofstream f ( n1 );
      f << "View \"debugfaces\" {" << endl;
#endif
      cout << class_val_nodes.size() << endl;
      typename map<N,FieldPair>::iterator it,beg=class_val_nodes.begin(),end=class_val_nodes.end();

      for ( it=beg; it!=end; it++ )
      {
        N node =it->first;
        double tt=it->second.first;
        node_struct ns ( node,tt );
        typename map<node_struct,info_struct,LessTimeNode>::iterator itr;
        itr=class_val_times.find ( ns );
        E ele3=* ( itr->second.elements.begin() );
        int  res=itr->second.res;

        if ( res!=0 )
        {
          typename map<N,FieldPair>::iterator it2,beg2=class_val_nodes.begin(),end2=class_val_nodes.end();

          for ( it2=beg2; it2!=end2; it2++ )
          {
            N node2=it2->first;

            if ( ! ( node2==node ) )
            {
              double tt2=it2->second.first;
              node_struct ns2 ( node2,tt2 );
              typename map<node_struct,info_struct,LessTimeNode>::iterator itr2;
              itr2=class_val_times.find ( ns2 );
              set<E> &els=itr2->second.elements;

              for ( typename set<E>::iterator it3=els.begin(); it3!=els.end(); it3++ )
              {
                E eletest=*it3;
                vector<N> nodes;
                eletest.GetNodes ( nodes );
                vector<N> face;

                for ( typename vector<N>::iterator it=nodes.begin(); it!=nodes.end(); it++ )
                {
                  if ( ! ( *it==node2 ) )
                  {
                    face.push_back ( *it );
                  }
                }

#ifdef DEBUG_FM
                double x,y,z;

                if ( doit )
                {
                  f << "ST(";

                  for ( typename vector<N>::iterator it=face.begin(); it!=face.end(); ++it )
                  {
                    N node=*it;
                    node.GetPos ( x,y,z );

                    if ( it==face.begin() )
                    {
                      f << x << ", " << y << ", " << z ;
                    }
                    else
                    {
                      f << ", " <<  x << ", " << y << ", " << z ;
                    }
                  }

                  f << "){0.,0.,0.};" << endl;

                }

                /*View "ls2_t" {
                SS(-3.96018e-01, -4.02899e-02,  0.00000e+00,-4.02172e-01, -3.03799e-02,  6.25000e-03,-4.07321e-01, -4.95748e-02,  0.00000e+00,-3.98193e-01, -3.64469e-02,  1.51449e-02){ 3.49791e-01,  3.54296e-01,  3.62598e-01,  3.50896e-01};
                */

#endif
                int res3=0;
                I trans;
                double t=ComputeTime ( node,eletest,face,res3,trans );
#ifdef DEBUG_FM
                node.GetPos ( x,y,z );

                if ( ( fabs ( x+0.214928854871447 ) +fabs ( y-0.02 ) +fabs ( z-0.01381080934510988 ) ) <1e-5 )
                {
                  cout << "node not yet updated" << endl;
                  cout << "t=" << t <<endl;
                  cout << "res=" << res3 << endl;

                  for ( typename vector<N>::iterator it=face.begin(); it!=face.end(); ++it )
                  {
                    it->Print();
                  }

                  if ( res3==0 )
                  {
                    cout << "node updated" << endl;
                  }
                }

#endif

                if ( res3==0 )
                {
                  UpdateTimeNode ( node,ele3,t,factor,100,trans );
                  //cout << "*" << endl;
                }
              }

            }
          }

//    doit=false;
        }

#ifdef DEBUG_FM
        f << "};" << endl;
        f.close();
#endif
      }


#endif

















      //     cout << " bnd " <<  bndold.size() << endl;
      /*      char num[5];
            sprintf(num,"%d",layer);
            char str[100];
            strcpy(str,"debug");
            strcat(str,num);
            Export_Debug(bndold,str);*/
      layer++;

      if ( class_val_times.size() !=0 )
      {
        node_struct ns;
        info_struct is;
        typename map<node_struct,info_struct,LessTimeNode>::iterator it;
        it=class_val_times.begin();
        ns=it->first;
        is=it->second;
#ifdef DEBUG
        double x,y,z;
        ns.node.GetPos ( x,y,z );

        if ( ( fabs ( x+0.214928854871447 ) +fabs ( y-0.02 ) +fabs ( z-0.01381080934510988 ) ) <1e-5 )
        {
          ns.node.Print();

          for ( typename set<E>::iterator it=is.elements.begin(); it!=is.elements.end(); ++it )
          {
            E ele = *it;
            ele.Print();
          }

          cout << "time=" << ns.time << endl;
          cout << "r=" << is.res << endl;
          Export_Debug ( bndold,"debug" );
        }

#endif
        class_val_times.erase ( it );
        class_val_nodes.erase ( ns.node );

        for ( typename set<E>::iterator it=is.elements.begin(); it!=is.elements.end(); ++it )
        {
          locked.insert ( *it );
          bnd.insert ( *it );
        }

        lstime[ns.node]=FieldPair ( ns.time,is.trans );
      }
      else
      {
//  cout << locked.size();
        return;
      }
    }
  }

#endif


#if 0
  template<class N,class E,class I> void FastMarching_c<N,E,I>::Propagate ( int factor )
  {
    int layer =0;
#define DEBUG_FM 1

    set<N> newnodes;
    set<E> neweles;
    set<E> bndeles;
    set<E> lockedeles;

    typename set<N>::iterator itnbnd;
    int dim=bnd.begin()->GetDim();

    for ( itnbnd=bndn.begin(); itnbnd!=bndn.end(); ++itnbnd )
    {
      N node=*itnbnd;
      vector<E> node_eles;
      node.GetNeighbors ( dim,node_eles );
//      cout << node_ele.size() << endl;
      typename vector<E>::iterator ite;
      typename vector<N>::iterator itn;

      for ( ite=node_eles.begin(); ite!=node_eles.end(); ++ite )
      {
        E ele=*ite;
        vector<N> ele_nodes;

        if ( bnd.find ( ele ) ==bnd.end() )
        {
          neweles.insert ( ele );
        }

        ele.GetNodes ( ele_nodes );

        for ( itn=ele_nodes.begin(); itn!=ele_nodes.end(); ++itn )
        {
          N node2=*itn;

          if ( bndn.find ( node2 ) ==bndn.end() )
          {
            newnodes.insert ( node2 );
          }
        }
      }
    }

    typename set<E>::iterator itebnd;

    for ( itebnd=bnd.begin(); itebnd!=bnd.end(); ++itebnd )
    {
      E ele=*itebnd;
      vector<E> eles;
      ele.GetNeighbors ( eles );
      typename vector<E>::iterator itee;

      for ( itee=eles.begin(); itee!=eles.end(); ++itee )
      {
        E elee=*itee;

        if ( locked.find ( elee ) ==locked.end() )
        {
          bndeles.insert ( elee );
          vector<N> commons,notcommons;
          elee.CommonNodes ( ele,commons,notcommons );

          for ( int nc=0; nc<notcommons.size(); ++nc )
          {
            int res=0;
            I trans;
            double t=ComputeTime ( notcommons[nc],elee,commons,res,trans );
            cout << nc << " " << t << endl;
          }
        }
      }
    }





    lockedeles=bndeles;


    cout << bndeles.size() << " bnd in new elements" << endl;
    cout << neweles.size() << " new elements" << endl;
    cout << newnodes.size() << " new nodes" << endl;



    //for


  }

#endif



#if 0
  template<class N,class E,class I> void FastMarching_c<N,E,I>::Propagate ( int factor )
  {
    int layer =0;

    while ( true )
    {
      typename set<E>::iterator itbnd;
      set<E> bndold;
      set<N> newnodes;

//     cout << layer << endl;
      while ( bnd.size() >0 )
      {
        itbnd=bnd.begin();
        E ele=*itbnd;
        bnd.erase ( itbnd );
        bndold.insert ( ele );
        vector<E> tab_nei;
        ele.GetNeighbors ( tab_nei );

        for ( int e=0; e<tab_nei.size(); ++e )
        {
          E ele2=tab_nei[e];

          if ( locked.find ( ele2 ) ==locked.end() )
          {
            int d=ele2.GetDim();
            vector<N> commons,notcommons;
            ele2.CommonNodes ( ele,commons,notcommons );
            bool allset=true;

            for ( int nc=0; nc<notcommons.size(); ++nc )
            {
              if ( lstime.find ( notcommons[nc] ) ==lstime.end() )
              {
                allset=false;
                int res=0;
                I trans;
                double t=ComputeTime ( notcommons[nc],ele2,commons,res,trans );
                bool result=UpdateTimeNode ( notcommons[nc],ele2,t,factor,res,trans );

                if ( result )
                {
                  newnodes.insert ( notcommons[nc] );
                }
              }
            }

            if ( allset )
            {
              locked.insert ( ele2 );
              bnd.insert ( ele2 );
//        cout << "locked" << endl;
            }

          }
        }
      }

#ifdef DEBUG_FM
      cout << " bnd " <<  bndold.size() << endl;
      char num[5];
      sprintf ( num,"%d",layer );
      char str[100];
      strcpy ( str,"debug" );
      strcat ( str,num );
      Export_Debug ( bndold,str );
#endif
      layer++;

      if ( class_val_times.size() !=0 )
      {
        node_struct ns;
        info_struct is;
        typename map<node_struct,info_struct,LessTimeNode>::iterator it;
        it=class_val_times.begin();
        ns=it->first;
        is=it->second;
        class_val_times.erase ( it );
        class_val_nodes.erase ( ns.node );

        for ( typename set<E>::iterator it=is.elements.begin(); it!=is.elements.end(); ++it )
        {
          locked.insert ( *it );
          bnd.insert ( *it );
        }

        lstime[ns.node]=FieldPair ( ns.time,is.trans );
      }
      else
      {
//  cout << locked.size();
        return;
      }
    }
  }
#endif



} // namespace FM




#endif // FASTMARCHING_H