// 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 "motorgraph.h"

using namespace motorgraph;

Event::Event ( Motorcycle *motc, double *pos, double t, eventtype what, int index_in_grid )
{
  mc = motc;
  p[0] = pos[0];
  p[1] = pos[1];
  time = t;
  type = what;
  gid = index_in_grid;
  mc->insert_event ( this );
//   printf ( "new : " );
//   print();
}

void Motorcycle::remove_earliest_event ( )
{
  std::set<Event*>::iterator it = eventqueue.begin();
  if ( it == eventqueue.end() )
    return;
//   printf ( "erasing from motorcycle queue : " );
//   ( *it )->print();
  eventqueue.erase ( it );
}

void Motorcycle::print()
{
  printf ( "motorcycle %d :\n", index );
  std::set<Event*>::iterator it = eventqueue.begin();
  for ( it = eventqueue.begin();it != eventqueue.end(); it++ )
  {
    ( *it )->print();
  }
}

// bool EventPtrComp::operator() ( const Event* a, const Event* b )
// {
//   if ( a->time < b->time )
//     return true;
//   else if ( a->time == b->time && a->mc->get_index() < b->mc->get_index() )
//     return true;
//   else
//     return false;
// }

bool EventPtrComp::operator() ( const Event* a, const Event* b )
{
  if ( a->time < b->time )
    return true;
  else if ( a->time == b->time && a->mc->get_index() < b->mc->get_index() )
    return true;
  else if ( a->time == b->time && a->mc->get_index() == b->mc->get_index() && a->mc->get_team() < b->mc->get_team() )
    return true;
  else
    return false;
}

void Grid::compute_exit ( int gid, double* orig, double* dir, int &ngid, double* exit )
{
  int i, j;
  get_indices ( gid, i, j );
  double minc[2], maxc[2];
  get_cell ( i, j, minc, maxc );
  // deal with 0 cases
  if ( dir[1] == 0.0 ) // horizontal
  {
    if ( dir[0] > 0.0 )
    {
      exit[0] = maxc[0];
      ngid = get_index ( i+1, j );
    }
    else
    {
      exit[0] = minc[0];
      ngid = get_index ( i-1, j );
    }
    exit[1] = orig[1];
    return;
  }
  if ( dir[0] == 0.0 ) // vertical
  {
    exit[0] = orig[0];
    if ( dir[1] > 0.0 )
    {
      exit[1] = maxc[1];
      ngid = get_index ( i, j+1 );
    }
    else
    {
      exit[1] = minc[1];
      ngid = get_index ( i, j-1 );
    }
    return;
  }
  // deal with general cases
  double a = dir[1]/dir[0];
  double b = orig[1]-a*orig[0];
  if ( dir[0] > 0.0 )
  {
    exit[0] = maxc[0];
    ngid = get_index ( i+1, j );
  }
  else
  {
    exit[0] = minc[0];
    ngid = get_index ( i-1, j );
  }

  exit[1] = a*exit[0] + b;

  if ( dir[1] > 0.0 && exit[1] > maxc[1] )
  {
    exit[1] = maxc[1];
    exit[0] = ( exit[1] - b ) /a;
    ngid = get_index ( i, j+1 );
  }
  else if ( dir[1] < 0.0 && exit[1] < minc[1] )
  {
    exit[1] = minc[1];
    exit[0] = ( exit[1] - b ) /a;
    ngid = get_index ( i, j-1 );
  }
}

int BucketTrajectory::potential_crash ( BucketTrajectory *bt, double *crashpos )
{
  // check colinear cases
  if ( geom::colinear_lines2d ( get_mc()->get_coeff(), bt->get_mc()->get_coeff(), 1e-12 ) == 2 )
  {
    if ( get_bounds ( 0 ) [0] == bt->get_bounds ( 1 ) [0] && get_bounds ( 0 ) [1] == bt->get_bounds ( 1 ) [1]
         && get_bounds ( 1 ) [0] == bt->get_bounds ( 0 ) [0] && get_bounds ( 1 ) [1] == bt->get_bounds ( 0 ) [1] )
    {
      crashpos[0] = ( get_bounds ( 0 ) [0] + get_bounds ( 1 ) [0] ) / 2.0; // warning : does not take speed into account
      crashpos[1] = ( get_bounds ( 0 ) [1] + get_bounds ( 1 ) [1] ) / 2.0;
      return 1;
    }
    else if ( get_bounds ( 1 ) [0] == bt->get_bounds ( 1 ) [0] && get_bounds ( 1 ) [1] == bt->get_bounds ( 1 ) [1] )
    {
      if ( geom::point_inside_segment2d ( get_bounds ( 0 ), get_bounds ( 1 ), 1e-12, bt->get_bounds ( 0 ) ) )
      {
        crashpos[0] = bt->get_bounds ( 0 ) [0];
        crashpos[1] = bt->get_bounds ( 0 ) [1];
        return 1;
      }
      else
      {
        crashpos[0] = get_bounds ( 0 ) [0];
        crashpos[1] = get_bounds ( 0 ) [1];
        return 1;
      }
    }
  }

  if ( geom::segments2d_intersection ( get_mc()->get_coeff(), get_bounds ( 0 ), get_bounds ( 1 ), bt->get_mc()->get_coeff(), bt->get_bounds ( 0 ), bt->get_bounds ( 1 ), 1e-12, crashpos ) )
    return 1;
  return 0;
}

void Bucket::generate_crash_events ( Motorcycle *mc, int gid, Event **eventlist, int &nbe )
{
  BucketTrajectory *cbt = trajectorylist.find ( mc )->second;
  std::map<Motorcycle *, BucketTrajectory *>::iterator it = trajectorylist.begin();
  for ( it = trajectorylist.begin(); it != trajectorylist.end(); it++ )
  {
    BucketTrajectory *bt = it->second;
    Motorcycle *omc = bt->get_mc();
//     printf("moto %d <--> moto %d\n", mc->get_index(), omc->get_index());
    if ( omc != mc && omc->get_team() != mc->get_team())
    {
      double crashpos[2], crashtime[2];
      if ( cbt->potential_crash ( bt, crashpos ) )
      {
        crashtime[0] = mc->time_to_position ( crashpos );
        crashtime[1] = omc->time_to_position ( crashpos );

        if ( crashtime[0] > crashtime[1] )
        {
          Event *ne = new Event ( mc, crashpos, crashtime[0], Event::CRASH, gid );
          eventlist[nbe++] = ne;
        }
        else if ( crashtime[0] < crashtime[1] )
        {
          Event *ne = new Event ( omc, crashpos, crashtime[1], Event::CRASH, gid );
          eventlist[nbe++] = ne;
        }
        else // crashtime[0] == crashtime[1]
        {
          Event *ne = new Event ( mc, crashpos, crashtime[0], Event::CRASH, gid );
          eventlist[nbe++] = ne;
          ne = new Event ( omc, crashpos, crashtime[1], Event::CRASH, gid );
          eventlist[nbe++] = ne;
        }
      }
    }
  }
}

void Graph::insert_motorcycle ( Motorcycle *mc )
{
  int gid = motorgrid->get_index ( mc->get_start() );
  Event *ne = new Event ( mc, mc->get_start(), 0.0, Event::SWITCH, gid ); // init event at time 0
  add_event ( ne );
}

void Graph::handle ( Event *ev )
{
  Motorcycle *mc = ev->get_mc();
//   printf ( "handling " );
//   ev->print();
  mc->remove_earliest_event();
  if ( mc->is_crashed() )
    return;
  if ( ev->get_type() == Event::CRASH )
    handle_crash ( ev );
  else
    handle_switch ( ev );
}

Event *Graph::next_switch ( Motorcycle *mc, double *pos, int gid )
{
  double exit[2];
  int ngid;
  motorgrid->compute_exit ( gid, pos, mc->get_dir(), ngid, exit );
  // compute theorical time to next switch
  double tn = mc->time_to_position ( exit );
  Event *ne = new Event ( mc, exit, tn, Event::SWITCH, ngid );
  return ne;
}

#define MAX_EVENT_LIST 10000
void Graph::handle_switch ( Event *ev )
{
  Motorcycle *mc = ev->get_mc();
  double *pos = ev->get_coord();
  mc->clear_events();
  mc->update_current ( pos, ev->get_time() );
  int gid = ev->get_gid ( );
  if ( gid == -1 )
  {
    mc->set_stopped();
    return;
  }
  // compute next switch event
  Event *next = next_switch ( mc, pos, gid );
//   printf ( "--> next switch event add :\n" );
  add_event ( next );
  // add mc to the newly entered cell
  motorgrid->insert_motorcycle ( mc, pos, next->get_coord(), gid );
  // check for potential crash events in cell gid
  Bucket *buc = motorgrid->get_bucket ( gid );
//   printf ( "--> generating crash events :\n" );
  Event *eventlist[MAX_EVENT_LIST];
  int nbe = 0;
  buc->generate_crash_events ( mc, gid, eventlist, nbe );
  for ( int i = 0; i < nbe; i++ )
  {
    Event *ne = eventlist[i];
    //     printf ( "--> listing crash events :\n" );
    //     ne->print();
    Motorcycle *nmc = ne->get_mc();
    //     nmc->print();
    if ( nmc != mc && nmc->get_earliest_event() == ne )
      add_event ( ne );
  }
  // we add the earliest event of mc into the priority queue
  Event *earliest = mc->get_earliest_event();
//   printf ( "--> earliest event add :\n" );
  add_event ( earliest );
}

void Graph::handle_crash ( Event *ev )
{
  Motorcycle *mc = ev->get_mc();
  double *pos = ev->get_coord();
  int gid = ev->get_gid ( );
  Bucket *buc = motorgrid->get_bucket ( gid );
  buc->set_crash ( mc, pos );
  mc->set_crashed();
  mc->clear_events();
  mc->update_current ( pos, ev->get_time() );
}

void Graph::init()
{
  int sqrtnb = ( int ) floor ( sqrt ( motorlist.size() ) ) +1;
  motorgrid->init ( sqrtnb, sqrtnb );
  motorgrid->export_vtk ( "motorbucket" );
  printf ( "init --> %d motorcycles\n", ( int ) motorlist.size() );
  for ( int i = 0; i < ( int ) motorlist.size(); i++ )
  {
    Motorcycle *mc = motorlist[i];
    insert_motorcycle ( mc );
  }
}

void Graph::run()
{
  printf ( "running --> %d events in queue\n", ( int ) eventqueue.size() );
  while ( !eventqueue.empty() )
  {
    std::multiset<Event*>::iterator it = eventqueue.begin();
    Event *ev = ( *it );
//     printf ( "erasing from main queue " );
//     ev->print();
    eventqueue.erase ( it );
    handle ( ev );
  }
}

void Graph::export_vtk ( char *name )
{
  char filename[100];
  static int count = 0;
  sprintf ( filename, "%s_%d.vtk", name, count++ );

  printf ( " write file --> %s\n", filename );
  FILE *fp_rw = fopen ( filename, "w" );

  fprintf ( fp_rw, "# vtk DataFile Version 2.0\n" );
  fprintf ( fp_rw, "Really cool data\n" );
  fprintf ( fp_rw, "ASCII\n" );
  fprintf ( fp_rw, "DATASET POLYDATA\n" );
  fprintf ( fp_rw, "POINTS %d float\n", 2* ( int ) motorlist.size() );

  for ( int i = 0; i < ( int ) motorlist.size(); i++ )
  {
    Motorcycle *mc = motorlist[i];
    fprintf ( fp_rw, "%lf %lf %lf\n", mc->get_start() [0], mc->get_start() [1], 0.0 );
    fprintf ( fp_rw, "%lf %lf %lf\n", mc->get_current() [0], mc->get_current() [1], 0.0 );
  }

  fprintf ( fp_rw, "VERTICES %d %d\n", 2* ( int ) motorlist.size(), 2*2* ( int ) motorlist.size() );
  for ( int i = 0; i < 2* ( int ) motorlist.size(); i++ )
  {
    fprintf ( fp_rw, "%d %d\n", 1, i );
  }

  fprintf ( fp_rw, "LINES %d %d\n", ( int ) motorlist.size(), 3* ( int ) motorlist.size() );
  for ( int i = 0; i < ( int ) motorlist.size(); i++ )
  {
    fprintf ( fp_rw, "%d %d %d\n", 2, 2*i, 2*i+1 );
  }

  fprintf ( fp_rw, "CELL_DATA %d\n", 3* ( int ) motorlist.size() );
  fprintf ( fp_rw, "SCALARS index int 1\n" );
  fprintf ( fp_rw, "LOOKUP_TABLE default\n" );
  for ( int i = 0; i < ( int ) motorlist.size(); i++ )
  {
    Motorcycle *mc = motorlist[i];
    fprintf ( fp_rw, "%d %d\n", mc->get_index(), mc->get_index() );
  }
  for ( int i = 0; i < ( int ) motorlist.size(); i++ )
  {
    Motorcycle *mc = motorlist[i];
    fprintf ( fp_rw, "%d\n", mc->get_index() );
  }
  
  fprintf ( fp_rw, "SCALARS team int 1\n" );
  fprintf ( fp_rw, "LOOKUP_TABLE default\n" );
  for ( int i = 0; i < ( int ) motorlist.size(); i++ )
  {
    Motorcycle *mc = motorlist[i];
    fprintf ( fp_rw, "%d %d\n", mc->get_team(), mc->get_team() );
  }
  for ( int i = 0; i < ( int ) motorlist.size(); i++ )
  {
    Motorcycle *mc = motorlist[i];
    fprintf ( fp_rw, "%d\n", mc->get_team() );
  }

  fclose ( fp_rw );
}

void Grid::export_vtk ( char *name )
{
  char filename[100];
  static int count = 0;
  sprintf ( filename, "%s_%d.vtk", name, count++ );

  printf ( " write file --> %s\n", filename );
  FILE *fp_rw = fopen ( filename, "w" );

  fprintf ( fp_rw, "# vtk DataFile Version 2.0\n" );
  fprintf ( fp_rw, "Really cool data\n" );
  fprintf ( fp_rw, "ASCII\n" );
  fprintf ( fp_rw, "DATASET POLYDATA\n" );
  fprintf ( fp_rw, "POINTS %d float\n", ( nb[0]+1 ) * ( nb[1]+1 ) );

  for ( int j = 0; j <= nb[1]; j++ )
    for ( int i = 0; i <= nb[0]; i++ )
    {
      fprintf ( fp_rw, "%lf %lf %lf\n", bb->get_min() [0] + i*bl[0], bb->get_min() [1] + j*bl[1], 0.0 );
    }

  fprintf ( fp_rw, "POLYGONS %d %d\n", nb[0]*nb[1], 5*nb[0]*nb[1] );
  for ( int j = 0; j < nb[1]; j++ )
    for ( int i = 0; i < nb[0]; i++ )
    {
      fprintf ( fp_rw, "%d %d %d %d %d\n", 4, i+j* ( nb[0]+1 ), i+1+j* ( nb[0]+1 ), i+1+ ( j+1 ) * ( nb[0]+1 ), i+ ( j+1 ) * ( nb[0]+1 ) );
    }

  fprintf ( fp_rw, "CELL_DATA %d\n", nb[0]*nb[1] );
  fprintf ( fp_rw, "SCALARS index int 1\n" );
  fprintf ( fp_rw, "LOOKUP_TABLE default\n" );
  for ( int j = 0; j < nb[1]; j++ )
    for ( int i = 0; i < nb[0]; i++ )
    {
      fprintf ( fp_rw, "%d\n", get_index ( i, j ) );
    }

  fclose ( fp_rw );
}