/*
    C++ Mesh Generation Library
    Copyright (c) 2013 Eric Bechet <bechet at vosges dot org>

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

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

#include <set>
#include <map>
#include <vector>
#include <algorithm>
#include <stdio.h>

#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:
    typedef pair<double,I> FieldPair;

    FastMarching_c ( const SpeedFunctor<E> &speed )
    {
      speedf=&speed;
    }
    void Clear ( void )
    {
      locked.clear();
      bnd.clear();
      class_val_nodes.clear();
      class_val_times.clear;
      lstime.clear();
    }
    void Init ( map<N,FieldPair> &initvals,set<E> &bndinit );
    void Propagate ( int factor=1 );
    virtual void Export ( char *name );
    virtual void Export_Debug ( set<E> &tab,char *name );
    virtual void SetExportFunctor ( ExportFunctor<N,FieldPair>& efp )
    {
      ef=&efp;
    }
    FieldPair operator() ( const N& node ) const
    {
      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 )
      {
        if ( n1.time==n2.time )
        {
          return ( n1.node<n2.node );
        }
        else
        {
          return ( fabs ( n1.time ) <fabs ( n2.time ) );
        }
      }
    };

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


    set<E> locked;
    set<E> bnd;
    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;
  }

  template<class N,class E,class I> void FastMarching_c<N,E,I>::Export ( char *name )
  {
    for ( 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,char *name )
  {
    for ( set<E>::iterator it=tab.begin(); it!=tab.end(); it++ )
    {
      E ele=*it;
      vector<N> tabv;
      ele.GetNodes ( tabv );

      for ( 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 );
    }
  }

  template<class N,class E,class I> void 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<map<N,FieldPair>::iterator,bool> itn;
    itn=class_val_nodes.insert ( pns );

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

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

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

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


      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 );
                UpdateTimeNode ( notcommons[nc],ele2,t,factor,res,trans );
              }
            }

            if ( allset )
            {
              locked.insert ( ele2 );
              bnd.insert ( ele2 );
            }

#if 1
            else
            {
              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 );
                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 ( 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
          }
        }
      }

      //     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;
        map<node_struct,info_struct,LessTimeNode>::iterator it;
        it=class_val_times.begin();
        ns=it->first;
        is=it->second;
        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 ( 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" );
        }

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

        for ( 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;
      }
    }
  }
} // namespace FM




#endif // FASTMARCHING_H