// Vorosweep - Copyright (C) 2010-2014 T. Mouton
//
// See the LICENSE.txt file for license information. Please report all
// bugs and problems to <thibaud.mouton@gmail.com>.

#include <set>
#include <list>
#include <vector>
#include <stack>
#include <map>
#include "common.h"
#include "elementary.h"
#include "event.h"
#include <geom.h>

#ifndef _SWEEPOBJECT_INCLUDE
#define _SWEEPOBJECT_INCLUDE

namespace vorosweep
{
class Border;
class SweepObject;
class SweepEdge;
class SweepFacet;
class Front;
class FrontLine;
class Event;
class EventPtrComp;
class Convex_generator;

template<class T> class Container
{
  private:
    std::stack<T*> freeobject;
    std::stack<int> freeindex;
    int maxindex;
  public:
    Container()
    {
      maxindex = 0;
    }
    ~Container()
    {
      while ( !freeobject.empty() )
      {
        T *ob = freeobject.top();
        delete ob;
        freeobject.pop();
      }
    }
    inline T *get ()
    {
      T *ob = NULL;
      if ( freeobject.empty() )
        ob = new T;
      else
      {
        ob = freeobject.top();
        freeobject.pop();
      }
      return ob;
    }
    inline void del ( T *ob )
    {
      freeobject.push ( ob );
    }
    inline int get_index ()
    {
      int index = -1;
      if ( freeindex.empty() )
      {
        index = maxindex++;
//         printf ( "getting new index %d\n", index );
      }
      else
      {
        index = freeindex.top();
        freeindex.pop();
//         printf ( "reusing index %d\n", index );
      }
      return index;
    }
    inline void del_index ( int index )
    {
//       printf ( "deleting index %d\n", index );
      freeindex.push ( index );
    }
};

class basic_type
{
  public:
    static Container<Point> *pcontainer;
};

class SweepObject : public basic_type
{
  public:
    static const char *objectname[];
    static const char *statusname[];
  public:
    enum sweeptype {EDGE, BORDEREDGE, FACET};
    enum sweepstatus {STARTING, RUNNING, STOPPED};
  protected:
    Point *start; // start position
    Point *current; // current position
    double starttime;
    double currenttime;
    double xyspeed; // speed in the Z plane
//     double param;
    double dir3d[3]; // displacement vector
    double dir2d[3]; // displacement vector
//   Convex_generator *cv[2];
    sweeptype type;
    sweepstatus state;
    std::multiset<Event *, EventPtrComp> eventqueue;
  public:
    inline void init ( Point *from, double t0 )
    {
      dbgprintf ( 1, "new sweepobject %p --> start at %lf %lf %lf, time %lf\n", this, from->p[0], from->p[1], from->p[2], t0 );
      start = from;
      starttime = currenttime = t0;
      current = pcontainer->get();
      current->set ( start->p );
      dir2d[0] = dir2d[1] = 0.0;
      dir3d[0] = dir3d[1] = dir3d[2] = 0.0;
//     cv[0] = cv[1] = NULL;
      state = SweepObject::STARTING;
      xyspeed = 0.0;
    }
    inline void set_dir ( double *vec )
    {
      dir3d[0] = vec[0];
      dir3d[1] = vec[1];
      dir3d[2] = vec[2];
      dir2d[0] = vec[0];
      dir2d[1] = vec[1];
      dir2d[2] = 0.0;
      geom::normalize2d ( dir2d );
    }
    SweepObject ( Point *from, double t0 )
    {
      init ( from, t0 );
    }
    SweepObject ( Point *from, double *vec, double t0 )
    {
      init ( from, t0 );
      set_dir ( vec );
    }
//     inline void set_param ( double t )
//     {
//       param = t;
//     }
//     inline double get_param ( )
//     {
//       return param;
//     }
    inline void set_xyspeed ( double s )
    {
      dbgprintf ( 1, "--> setting xy speed %lf\n", s );
      xyspeed = s;
    }
    inline double get_xyspeed ( )
    {
      return xyspeed;
    }
    inline double get_squared_xyspeed ( )
    {
      return xyspeed*xyspeed;
    }
    inline Point *get_start()
    {
      return start;
    }
    inline Point *get_current()
    {
      return current;
    }
    inline void update_current ( double time )
    {
      if ( xyspeed != INFINITY )
      {
        current->p[0] = start->p[0] + ( time-starttime ) *xyspeed*dir2d[0];
        current->p[1] = start->p[1] + ( time-starttime ) *xyspeed*dir2d[1];
      }
      current->p[2] = time;
      currenttime = time;
    }
    inline void merge_current ( SweepObject *so, double time )
    {
      update_current ( time );
//       so->update_current ( time );
      so->currenttime = time;
      set_stopped();
      so->set_stopped();
      if ( current != so->current )
      {
        so->current->p[0] = 0.0;
        so->current->p[1] = 0.0;
        so->current->p[2] = 0.0;
        basic_type::pcontainer->del ( so->current );
        basic_type::pcontainer->del_index ( so->current->get_index() );
        so->current->set_index ( -1 );
        so->current = current;
      }
      dbgprintf ( 2, "--> merging complete : current %d = %p, current %d = %p\n", current->get_index(), current, so->current->get_index(), so->current );
    }
    inline double *get_dir3d()
    {
      return dir3d;
    }
    inline double *get_dir2d()
    {
      return dir2d;
    }
    inline sweeptype get_type()
    {
      return type;
    }
    virtual double time_to_position ( double *pt ) = 0;
    Event *get_earliest_event();
    void disable_futur_event ( SweepObject *s0, SweepObject *s1 );
    void remove_earliest_event ( Event *ev );
    inline void insert_event ( Event *ev )
    {      
      print_event_queue();
      dbgprintf ( 2, "%s %p --> adding event %p from %d\n", objectname[get_type() ], this, ev, ( int ) eventqueue.size() );
      eventqueue.insert ( ev );
    }
    inline void clear_events()
    {
      dbgprintf ( 3, "%s %p --> clearing events\n", objectname[get_type() ], this );
      eventqueue.clear();
    }
    inline void set_running()
    {
      state = SweepObject::RUNNING;
    }
    inline void set_stopped()
    {
      clear_events();
      dbgprintf ( 2, "%s --> stopping sweepobject %p start %lf %lf dir %lf %lf \n", objectname[get_type() ], this, get_start()->p[0], get_start()->p[1], get_dir2d() [0], get_dir2d() [1] );
      state = SweepObject::STOPPED;
    }
    inline int has_started()
    {
      return ( state == SweepObject::STARTING );
    }
    inline int is_stopped()
    {
      if ( state == SweepObject::STOPPED )
        dbgprintf ( 3, "%s --> sweepobject %p start %lf %lf dir %lf %lf is stopped\n", objectname[get_type() ], this, get_start()->p[0], get_start()->p[1], get_dir2d() [0], get_dir2d() [1] );
      return ( state == SweepObject::STOPPED );
    }
    virtual int self_check ()
    {
      return 0;
    }
    virtual void print();
    void print_event_queue();
};


class SweepFacet : public SweepObject
{
  public:
    enum status {INIT, NOINIT};
  private:
    SweepEdge *iedges[2]; // initial sweepedges
    FrontLine *mfl;
    double pl[4];
    Convex_generator *cv;
    int gpos; // position in generator
  public:
    SweepFacet ( Point *apex, double *plane, double *vec, double t0 ) : SweepObject ( apex, vec, t0 )
    {
      pl[0] = plane[0];
      pl[1] = plane[1];
      pl[2] = plane[2];
      pl[3] = -plane[0]*apex->p[0]-plane[1]*apex->p[1]-plane[2]*apex->p[2];
      type = SweepObject::FACET;
      iedges[0] = iedges[1] = NULL;
      gpos = -1;
    }
    inline void set_pos ( int p )
    {
      gpos = p;
    }
    inline int get_pos()
    {
      return gpos;
    }
    inline void set_frontline ( FrontLine *fl )
    {
      dbgprintf ( 2, "--> new frontline %p\n", fl );
      mfl = fl;
    }
    inline FrontLine *get_frontline ( )
    {
      return mfl;
    }
    inline void set_initial_sweepedge ( int i, SweepEdge * se )
    {
        dbgprintf ( 2, "--> setting initial sweepedge %d\n", i );
        iedges[i] = se;
    }
    inline SweepEdge *get_initial_sweepedge ( int i )
    {
      return iedges[i];
    }
    SweepEdge *get_original_sweepedge ( int i );
//     inline void set_bisect(double *dir)
//     {
//       bisect[0] = dir[0];
//       bisect[1] = dir[1];
//       bisect[2] = dir[2];
//     }
//     inline double *get_bisect()
//     {
//       return bisect;
//     }
    inline void set_generator ( Convex_generator *c )
    {
      cv = c;
    }
    inline Convex_generator *get_generator()
    {
      return cv;
    }
    inline double time_to_position ( double *pt )
    {
      double u[2];
      geom::vec2d ( start->p, pt, u );
      double d = geom::scalar_product2d ( get_dir2d(), u );
      return starttime + d / xyspeed;
    }
    inline void position_at_time ( double time, double *pt )
    {
      double d = ( time-starttime ) *xyspeed;
      pt[0] = d*dir2d[0];
      pt[1] = d*dir2d[1];
    }
    inline double *get_plane()
    {
      return pl;
    }
    inline void get_ortho_vector ( int i, double *vec2d )
    {
      if ( i == 1 )
      {
        vec2d[0] = -dir2d[1];
        vec2d[1] = dir2d[0];
      }
      else
      {
        vec2d[0] = dir2d[1];
        vec2d[1] = -dir2d[0];
      }
    }
    inline void get_ortho_vector ( double *vec2d )
    {
      vec2d[0] = dir2d[1];
      vec2d[1] = -dir2d[0];
    }
    inline void dir2d_to_dir3d ( double *dir, double *ndir )
    {
      ndir[0] = dir[0];
      ndir[1] = dir[1];
      ndir[2] = - ( pl[0]*ndir[0]+pl[1]*ndir[1] ) / pl[2];
      geom::normalize3d ( ndir );
    }
    inline double compute_squared_speeds ( double *dir )
    {
      double z = - ( get_plane() [0]*dir[0] + get_plane() [1]*dir[1] ) / get_plane() [2];
//       double vec[2] = {0.0, 0.0};
//       dbgprintf ( 2, "z --> %.20lf\n", z );
//       vec[0] = dir[0]/z;
//       vec[1] = dir[1]/z;
//       double xys = geom::sqrnorm2d ( vec );
      double xys = geom::sqrnorm2d ( dir ) / (z*z);
      return xys;
    }
//     void update_speed_range ();
    int self_check ();
    void print();
};

class SweepEdge : public SweepObject
{
  public:
    enum status {INIT, NOINIT};
  private:
    SweepFacet *ifaces[2]; // initial sweepfaces
    Front *fr[2];
    int lgid;
    double stime;
    status init;
  public:
    SweepEdge ( Point *from, double t0 ) : SweepObject ( from, t0 )
    {
      int id = basic_type::pcontainer->get_index();
      current->set_index ( id );
      type = SweepObject::EDGE;
      fr[0] = fr[1] = NULL;
      ifaces[0] = ifaces[1] = NULL;
      init = SweepEdge::NOINIT;
      lgid = -1;
      stime = 0.0;
    }
    SweepEdge ( Point *from, double *vec, double t0 ) : SweepObject ( from, vec, t0 )
    {
      int id = basic_type::pcontainer->get_index();
      current->set_index ( id );
      type = SweepObject::EDGE;
      fr[0] = fr[1] = NULL;
      ifaces[0] = ifaces[1] = NULL;
      init = SweepEdge::NOINIT;
      lgid = -1;
      stime = 0.0;
    }
//     // create a new instance which is a copy of an old one (except for the stating point and the time)
//     inline SweepEdge *copy ( Point *from, double t0 )
//     {
//       dbgprintf ( 2, "--> copying sweepedge %p\n", this );
//       SweepEdge *nse = new SweepEdge ( from, t0 );
//       nse->dir3d[0] = dir3d[0];
//       nse->dir3d[1] = dir3d[1];
//       nse->dir3d[2] = dir3d[2];
//       nse->dir2d[0] = dir2d[0];
//       nse->dir2d[1] = dir2d[1];
//       nse->dir2d[2] = dir2d[2];
//       nse->starttime = currenttime = t0;
//       nse->xyspeed = xyspeed;
//       nse->init = init;
//       nse->ifaces[0] = ifaces[0];
//       nse->ifaces[1] = ifaces[1];
//       return nse;
//     }
    inline void set_last_gid ( int gid )
    {
      lgid = gid;
    }
    inline int get_last_gid()
    {
      return lgid;
    }
    inline Front *get_front ( int i )
    {
      return fr[i];
    }
    inline void set_next_switch ( double switchtime )
    {
      stime = switchtime;
    }
    inline double get_next_switch()
    {
      return stime;
    }
//     inline void set_initial_sweepfacet ( int i, SweepFacet * sf )
//     {
//       if ( !ifaces[i] )
//       {
//         dbgprintf ( 2, "--> updating initial sweepfacet %d\n", i );
//         ifaces[i] = sf;
//       }
//     }
//     inline void set_initial_sweepfacets ( SweepFacet * sf0, SweepFacet * sf1 )
//     {
//       set_initial_sweepfacet ( 0, sf0 );
//       set_initial_sweepfacet ( 1, sf1 );
//     }
//     inline SweepFacet *get_initial_sweepfacet ( int i )
//     {
//       return ifaces[i];
//     }
    inline void set_init()
    {
      init = INIT;
    }
    inline int is_init()
    {
      return ( init == SweepEdge::INIT );
    }
    inline Convex_generator *get_generator()
    {
      if ( is_init() )
        return ifaces[0]->get_generator();
      else
        return NULL;
    }
    inline Convex_generator *get_generator ( int i )
    {
      if ( ifaces[i] )
        return ifaces[i]->get_generator();
      else
        return NULL;
    }
    inline double time_to_position ( double *pt )
    {
//         printf("time to current --> %lf\n", starttime + geom::distance2d ( start->p, current->p ) / xyspeed);
      double d = geom::distance2d ( start->p, pt );
      return starttime + d / xyspeed;
    }
    inline void position_at_time ( double time, double *pt )
    {
      double d = ( time-starttime ) *xyspeed;
      pt[0] = start->p[0] + d*dir2d[0];
      pt[1] = start->p[1] + d*dir2d[1];
    }
    int compatible_event ( SweepFacet *sf );
    SweepFacet *get_neighbor_sweepfacet ( int i, int level );
    void set_front ( int i, Front *f );
    SweepFacet *get_sweepfacet ( int i );
    void update_speeds ( SweepFacet *sf );
    int self_check ();
    void print();
};

class BorderSweepEdge : public SweepEdge
{
  private:
    Border *bo;
  public:
    BorderSweepEdge ( Point *from, double t0 ) : SweepEdge ( from, t0 )
    {
      type = SweepObject::BORDEREDGE;
    }
    BorderSweepEdge ( Point *from, double *vec, double t0 ) : SweepEdge ( from, vec, t0 )
    {
      type = SweepObject::BORDEREDGE;
    }
    inline void set_border ( Border *b )
    {
      bo = b;
    }
    inline Border *get_border()
    {
      return bo;
    }
};

};

#endif