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

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <vector>
#include <list>
#include <set>
#include <queue>
#include <map>
#include <iterator>
#include <geom.h>

#ifndef _motorgraph_include
#define _motorgraph_include

namespace motorgraph
{
class Motorcycle;
class Event;
class EventPtrComp;

class EventPtrComp
{
public:
  bool operator() ( const Event* a, const Event* b );
};

class Motorcycle
{
  public:
    enum status {RUNNING, CRASHED, STOPPED};
  private:
    double start[2]; // start position
    double starttime;
    double dir[2]; // displacement vector
    double traj[3];
    double current[2]; // current position
    double currenttime;
    double speed; // speed
    int index;
    int team;
    status state;
    std::multiset<Event *, EventPtrComp> eventqueue;
  public:
    Motorcycle ( double p0[2], double vec[2] )
    {
      start[0] = current[0] = p0[0];
      start[1] = current[1] = p0[1];
      dir[0] = vec[0];
      dir[1] = vec[1];
      starttime = currenttime = 0.0;
      geom::line2d_dir_to_coeff ( start, dir, traj, 1e-10 );
      state = RUNNING;
      speed = 1.0;
    }
    inline double *get_start()
    {
      return start;
    }
    inline double *get_current()
    {
      return current;
    }
    inline void update_current ( double *p, double time )
    {
      current[0] = p[0];
      current[1] = p[1];
      currenttime = time;
    }
    inline double *get_dir()
    {
      return dir;
    }
    inline double *get_coeff()
    {
      return traj;
    }
    inline void set_index ( int i )
    {
      index = i;
    }
    inline void set_team ( int i ) // no crash between same team
    {
      team = i;
    }
    inline int get_index()
    {
      return index;
    }
    inline int get_team()
    {
      return team;
    }
    inline void set_crashed()
    {
      state = CRASHED;
    }
    inline void set_stopped()
    {
      state = STOPPED;
    }
    inline int is_crashed()
    {
      return ( state == CRASHED );
    }
    inline int is_stopped()
    {
      return ( state == STOPPED );
    }
    inline Event *get_earliest_event()
    {
      std::set<Event*>::iterator it = eventqueue.begin();
      return ( *it );
    }
    inline void insert_event ( Event *ev )
    {
      eventqueue.insert ( ev );
    }
    void remove_earliest_event ( );
    inline void clear_events()
    {
//       printf("motorcycle %d --> clearing events\n", index);
      eventqueue.clear();
    }
    inline double time_to_position ( double *pos )
    {
      return geom::distance2d ( start, pos ) / speed;
    }
    void print();
};

class Event
{
  friend class EventPtrComp;
  
  public:
    enum eventtype {CRASH, SWITCH};
  private:
    double p[2];
    double time; // time of event
    eventtype type;
    Motorcycle *mc;
    int gid;

  public:
    Event ( Motorcycle *motc, double *pos, double t, eventtype what, int index_in_grid );
    inline Motorcycle *get_mc()
    {
      return mc;
    }
    inline double *get_coord()
    {
      return p;
    }
    inline double get_time()
    {
      return time;
    }
    inline eventtype get_type()
    {
      return type;
    }
    inline int get_gid()
    {
      return gid;
    }
    inline void print()
    {
      printf ( "event %p type %d : motorcycle %d pos %lf %lf, time %lf, bucket %d\n", this, get_type(), mc->get_index(), p[0], p[1], time, gid );
    }
};

class BBox
{
  private:
    double min[2]; // min bbox
    double max[2]; // max bbox
    double lt[2];
    double enlarge;
  public:
    BBox ( double enlarge_factor )
    {
      min[0] = min[1] = INFINITY;
      max[0] = max[1] = -INFINITY;
      lt[0] = lt[1] = -INFINITY;
      enlarge = enlarge_factor;
    }
    inline void update ( double *p )
    {
      for ( int i = 0; i < 2; i++ )
      {
        if ( p[i] < min[i] )
          min[i] = p[i];
        else if ( p[i] > max[i] )
          max[i] = p[i];
      }
    }
    inline void lock()
    {
      for ( int i = 0; i < 2; i++ )
      {
        double dt = ( max[i] - min[i] ) * enlarge;
        min[i] -= dt;
        max[i] += dt;
        lt[i] = max[i] - min[i];
      }
    }
    inline double *get_min()
    {
      return min;
    }
    inline double *get_max()
    {
      return max;
    }
    inline double get_lenght ( int i )
    {
      return lt[i];
    }
};

class BucketTrajectory
{
  private:
    Motorcycle * mc;
    double bounds[2][2];
  public:
    BucketTrajectory()
    {}
    BucketTrajectory ( Motorcycle *motorcycle )
    {
      mc = motorcycle;
      bounds[0][0] = INFINITY;
      bounds[0][1] = INFINITY;
      bounds[1][0] = INFINITY;
      bounds[1][1] = INFINITY;
    }
    inline void set ( Motorcycle *motorcycle, double *start, double *end )
    {      
      mc = motorcycle;
      bounds[0][0] = start[0];
      bounds[0][1] = start[1];
      bounds[1][0] = end[0];
      bounds[1][1] = end[1];
//       printf("set trajectory %lf %lf -- %lf %lf\n", bounds[0][0], bounds[0][1], bounds[1][0], bounds[1][1]);
    }
    inline Motorcycle *get_mc()
    {
      return mc;
    }
    inline double *get_bounds ( int i )
    {
      return bounds[i];
    }
    inline void set_crash ( double *crashpos )
    {
      bounds[1][0] = crashpos[0];
      bounds[1][1] = crashpos[1];
    }
    int potential_crash ( BucketTrajectory *bt, double *crashpos );
};

class Bucket
{
  private:
    std::map<Motorcycle *, BucketTrajectory *> trajectorylist;

  public:
    inline void insert_motorcycle ( Motorcycle *mc, double *start, double *end )
    {
      BucketTrajectory *traj = new BucketTrajectory;
      traj->set ( mc, start, end );
      trajectorylist[mc] = traj;
    }
    inline void set_crash ( Motorcycle *mc, double *crashpos )
    {
      std::map<Motorcycle *, BucketTrajectory *>::iterator it = trajectorylist.find ( mc );
      BucketTrajectory *bt = it->second;
      if ( bt )
        bt->set_crash ( crashpos );
    }
    void generate_crash_events ( Motorcycle *mc, int gid, Event **eventlist, int &nbe );
};

class Grid
{
  private:
    BBox *bb;
    int nb[2]; // subdivision /x and /y
    double bl[2]; // size bucket /x and /y
    Bucket *motorgrid;
  public:
    Grid ( double enlarge )
    {
      bb = new BBox ( enlarge );
    }
    inline void init ( int nbx, int nby )
    {
      bb->lock();
      nb[0] = nbx;
      nb[1] = nby;
      motorgrid = new Bucket[nb[0]*nb[1]];
      bl[0] = bb->get_lenght ( 0 ) / ( double ) nb[0];
      bl[1] = bb->get_lenght ( 1 ) / ( double ) nb[1];
      printf ( "init grid %d * %d --> %lf %lf\n", nb[0], nb[1], bl[0], bl[1] );
    }
    inline void insert_motorcycle ( Motorcycle *mc, double *start, double *end, int index )
    {
      motorgrid[index].insert_motorcycle ( mc, start, end );
    }
    inline void get_indices ( int id, int &i, int &j )
    {
      j = ( int ) ( id/nb[0] );
      i = id%nb[0];
    }
    inline int get_index ( int i, int j )
    {
      if ( i < 0 || i >= nb[0] || j < 0 || j >= nb[1] )
        return -1;
      return i+nb[0]*j;
    }
    inline void get_indices ( double *p, int &i, int &j )
    {
      i = static_cast<int> ( ( p[0] - bb->get_min() [0] ) / bl[0] );
      j = static_cast<int> ( ( p[1] - bb->get_min() [1] ) / bl[1] );
    }
    inline int get_index ( double *p )
    {
      int i, j;
      get_indices ( p, i, j );
      return get_index ( i, j );
    }
    inline void get_cell ( int i, int j, double *minc, double *maxc )
    {
      minc[0] = bb->get_min() [0] + i*bl[0];
      minc[1] = bb->get_min() [1] + j*bl[1];
      maxc[0] = minc[0] + bl[0];
      maxc[1] = minc[1] + bl[1];
    }
    inline BBox *get_bbox()
    {
      return bb;
    }
    inline Bucket *get_bucket ( int i )
    {
      return &motorgrid[i];
    }
    void compute_exit ( int gid, double* orig, double* dir, int &ngid, double* exit );
    void export_vtk ( char *name );
};


class Graph
{
  private:
    Grid *motorgrid;
    std::set<Event *, EventPtrComp> eventqueue;
    std::vector<Motorcycle *> motorlist;
  public:
    Graph()
    {
      motorgrid = new Grid ( 0.1 );
    }
    void init();
    void run();
    inline void add_motorcycle ( Motorcycle *mc )
    {
      motorlist.push_back ( mc );
      BBox *bb = motorgrid->get_bbox();
      bb->update ( mc->get_start() );
    }
    inline void add_event ( Event *ev )
    {
//       printf ( "adding " );
//       ev->print();
      eventqueue.insert ( ev );
    }
    void export_vtk ( char *name );

  private:
    void insert_motorcycle ( Motorcycle *mc );
    void handle ( Event *ev );
    void handle_switch ( Event *ev );
    void handle_crash ( Event *ev );
    Event *next_switch ( Motorcycle *mc, double *pos, int gid );
};
};

#endif