// 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 "grid.h"
#include "border.h"
#include "event.h"

using namespace vorosweep;

double basic_grid::epsilon = 1e-16;

int BorderBucket::potential_crash ( double *start0, double *start1, double *dir0, double *dir1, double *crashpos )
{
  if ( dir0[0]*dir1[0] < 0.0 && geom::lines2d_intersection ( start0, start1, dir0, dir1, basic_grid::epsilon, crashpos ) )
  {
    dbgprintf ( 5, "--> EDGE - BORDER intersection found\n" );
    return 1;
  }
  return 0;
}

// Be careful here because we are doing computations in the border space
// so that coordinates must be translated into the real 3d space
void BorderBucket::generate_edgecrash_events ( SweepEdge *se, double ctime, int gid, Container<Event> *econt, Event **eventlist, int &nbe )
{
//   nbe = 0; // set nbe to zero !!
  se->print();
  dbgprintf ( 2, "edgelist size --> %d\n", ( int ) edgelist.size() );
  if ( edgelist.size() <= 1 )
    return;
  double sept2d[2], sedir2d[2];
  edge3d_to_edge2d ( se, sept2d, sedir2d );
  std::set<SweepEdge *>::iterator it;
  for ( it = edgelist.begin(); it != edgelist.end(); it++ )
  {
    SweepEdge *s2 = *it;
//     printf ( "se --> %p : dir %lf %lf %lf (stopped : %d)\n", s2, s2->get_dir3d() [0], s2->get_dir3d() [1], s2->get_dir3d() [2], s2->is_stopped() );
    if ( s2 != se && !s2->is_stopped() )
    {
      s2->print();
      double crashpos2d[2], crashtime[2];
      double s2pt2d[2], s2dir2d[2];
      edge3d_to_edge2d ( s2, s2pt2d, s2dir2d );

      if ( potential_crash ( sept2d, s2pt2d, sedir2d, s2dir2d, crashpos2d ) )
      {
        double crashpos3d[3];
        pt2d_to_pt3d ( crashpos2d, crashpos3d );
        dbgprintf ( 2, "EDGE - EDGE : crashpos --> %lf %lf %lf\n", crashpos3d[0], crashpos3d[1], crashpos3d[2] );
        crashtime[0] = s2->time_to_position ( crashpos3d );
        dbgprintf ( 2, "crashtime --> %lf\n", crashtime[0] );
        if ( crashtime[0] >= ctime )
        {
          Event *ne = econt->get();
          ne->init ( se, s2, crashpos3d, crashtime[0], Event::EDGE_EDGECRASH, gid );
          eventlist[nbe++] = ne;
        }
      }
    }
  }
//   getchar();
}

void Grid1d::compute_exit ( int gid, double* orig, double* vec, int &ngid, double* exit )
{
  double dot = geom::scalar_product3d ( vec, get_dir() );
//   printf ( "dot --> %lf\n", dot );
  if ( dot > 0.0 )
  {
    ngid = gid+1;
    if ( ngid >= nbx )
    {
      exit[0] = pt[1]->p[0];
      exit[1] = pt[1]->p[1];
      ngid = -1;
      return;
    }
    exit[0] = pt[0]->p[0] + ngid*blx*get_dir() [0];
    exit[1] = pt[0]->p[1] + ngid*blx*get_dir() [1];
  }
  else
  {
    ngid = gid-1;
    if ( ngid < 0 )
    {
      exit[0] = pt[0]->p[0];
      exit[1] = pt[0]->p[1];
      ngid = -1;
      return;
    }
    exit[0] = pt[0]->p[0] + gid*blx*get_dir() [0];
    exit[1] = pt[0]->p[1] + gid*blx*get_dir() [1];
  }
}

void Grid1d::print()
{
  dbgprintf ( 1, "## grid : start %lf %lf, end %lf %lf\n", pt[0]->p[0], pt[0]->p[1], pt[1]->p[0], pt[1]->p[1] );
  dbgprintf ( 1, "## --> %d ( dx %lf )\n## ==>", nbx, blx );
  for ( int i = 0; i < nbx+1; i++ )
    dbgprintf ( 1, " (%lf, %lf)", pt[0]->p[0] + i*blx*dir[0], pt[0]->p[1] + i*blx*dir[1] );
  dbgprintf ( 1, "\n" );
  dbgprintf ( 1, "## dir --> %lf %lf\n", dir[0], dir[1] );
}

void Grid2d::init ( int nbx, int nby )
{
  bb->lock();
  nb[0] = nbx;
  nb[1] = nby;
  objectgrid = new Bucket[nb[0]*nb[1]];
  bl[0] = bb->get_lenght ( 0 ) / ( double ) nb[0];
  bl[1] = bb->get_lenght ( 1 ) / ( double ) nb[1];

//   // set bounds
//   double *start = bb->get_min();
//   for ( int i = 0; i < nb[0]; i++ )
//     for ( int j = 0; j < nb[1]; j++ )
//     {
//       int id = get_index ( i, j );
//       objectgrid[id].set_bbox(start[0]+i*bl[0], start[1]+j*bl[1], start[0]+(i+1)*bl[0], start[1]+(j+1)*bl[1]);
//     }

  tprintf ( "Init grid --> %d * %d ( dx %lf, dy %lf )\n", nb[0], nb[1], bl[0], bl[1] );
}

void Grid2d::compute_exit ( int gid, double* orig, double* dir, int &ngid, double* exit )
{
//     double epsilon = 1e-16;
  int i, j;
  get_indices ( gid, i, j );
  double minc[2], maxc[2];
  get_cell ( i, j, minc, maxc );
  // deal with 0 cases
  if ( fabs ( dir[1] ) < basic_grid::epsilon ) // 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 ( fabs ( dir[0] ) < basic_grid::epsilon ) // 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;
  }
  dbgprintf ( 5, "general case : %.20lf %.20lf\n", dir[0], dir[1] );
  // deal with general cases
  double a = dir[1]/dir[0];
  double b = orig[1]-a*orig[0];
  dbgprintf ( 5, "exit computation : %lf %lf\n", a, b );
  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 Bucket::potential_crash ( SweepEdge *se, SweepFacet *sf, double *crashpos )
{
  dbgprintf ( 2, "checking potential crash between se %p and sf %p\n", se, sf );
  if ( geom::line3d_plane_intersection ( se->get_start()->p, se->get_dir3d(), sf->get_plane(), basic_grid::epsilon, crashpos ) && geom::sqrtdistance2d ( se->get_start()->p, crashpos ) > basic_grid::epsilon )
  {
    dbgprintf ( 5, "--> EDGE - FACET intersection found\n" );
    if ( sf->get_frontline()->valid_future_event ( crashpos ) )
      return 1;
  }
  return 0;
}

int Bucket::potential_crash ( SweepEdge *se, Border *bo, double *crashpos )
{
  if ( geom::lines2d_intersection ( se->get_start()->p, bo->get_point ( 0 )->p, se->get_dir2d(), bo->get_dir(), basic_grid::epsilon, crashpos ) && geom::sqrtdistance2d ( se->get_start()->p, crashpos ) > basic_grid::epsilon )
  {
    dbgprintf ( 5, "--> EDGE - BORDER intersection found\n" );
    return 1;
  }
  return 0;
}

void Bucket::generate_facetcrash_events ( SweepFacet *sf, double ctime, int gid, Container<Event> *econt, Event **eventlist, int &nbe )
{
  if ( edgelist.empty() )
    return;

  dbgprintf ( 2, "edgelist size --> %d\n", ( int ) edgelist.size() );
  std::set<SweepEdge *>::iterator it;
  for ( it = edgelist.begin(); it != edgelist.end(); it++ )
  {
    SweepEdge *se = *it;

    se->print();
    double crashpos[3], crashtime;

//     if (se->is_stopped()) // TODO --> this speedup the algorithm
//     {
//       edgelist.erase(it);
//     }

    if ( !se->is_stopped() && se->is_init() && se->compatible_event ( sf ) && potential_crash ( se, sf, crashpos ) )
    {
      dbgprintf ( 2, "EDGE - FACET : crashpos --> %lf %lf %lf\n", crashpos[0], crashpos[1], crashpos[2] );
      crashtime = se->time_to_position ( crashpos );
      dbgprintf ( 2, "crashtime --> %lf\n", crashtime );
      dbgprintf ( 2, "ctime : %lf < crashtime : %lf < next switch : %lf\n", ctime, crashtime, se->get_next_switch() );
      if ( crashtime >= ctime && crashtime < se->get_next_switch() )
      {
        Event *ne = econt->get();
        ne->init ( se, sf, crashpos, crashtime, Event::EDGE_FACETCRASH, gid );
        eventlist[nbe++] = ne;
      }
    }
  }
}

void Bucket::generate_facetcrash_events ( SweepEdge *se, double ctime, int gid, Container<Event> *econt, Event **eventlist, int &nbe )
{
  if ( se->is_stopped() || !se->is_init() || facetlist.empty() )
    return;

  dbgprintf ( 2, "facetlist size --> %d\n", ( int ) facetlist.size() );

  std::set<SweepFacet *>::iterator it;
  for ( it = facetlist.begin(); it != facetlist.end(); it++ )
  {
    SweepFacet *sf = ( *it );
    sf->print();
    double crashpos[3], crashtime;

    if ( se->compatible_event ( sf ) && potential_crash ( se, sf, crashpos ) )
    {
      dbgprintf ( 2, "EDGE - FACET : crashpos --> %lf %lf %lf\n", crashpos[0], crashpos[1], crashpos[2] );
      crashtime = se->time_to_position ( crashpos );
      dbgprintf ( 2, "crashtime --> %lf\n", crashtime );
      dbgprintf ( 2, "ctime : %lf < crashtime : %lf < next switch : %lf\n", ctime, crashtime, se->get_next_switch() );
      if ( crashtime >= ctime && crashtime < se->get_next_switch() )
      {
        Event *ne = econt->get();
        ne->init ( se, sf, crashpos, crashtime, Event::EDGE_FACETCRASH, gid );
        eventlist[nbe++] = ne;
      }
    }
  }
}

void Bucket::generate_bordercrash_events ( SweepEdge *se, double ctime, int gid, Container<Event> *econt, Event **eventlist, int &nbe )
{
  dbgprintf ( 2, "borderlist size --> %d\n", ( int ) borderlist.size() );

  if ( se->is_stopped() || !se->is_init() || borderlist.empty() )
    return;

  std::set<Border *>::iterator it;
  for ( it = borderlist.begin(); it != borderlist.end(); it++ )
  {
    Border *bo = ( *it );
    double crashpos[2], crashtime;
    bool good_face_of_border = ( geom::cross_product2d ( se->get_dir2d(), bo->get_dir() ) >= 0.0 );
    if ( good_face_of_border && potential_crash ( se, bo, crashpos ) )
    {
      dbgprintf ( 2, "EDGE - BORDER : crashpos --> %lf %lf\n", crashpos[0], crashpos[1] );
      crashtime = se->time_to_position ( crashpos );
      dbgprintf ( 2, "crashtime --> %lf\n", crashtime );
      if ( crashtime >= ctime && crashtime < se->get_next_switch() )
      {
        Event *ne = econt->get();
        ne->init ( se, bo, crashpos, crashtime, Event::EDGE_BORDERCRASH, gid );
        eventlist[nbe++] = ne;
      }
    }
  }
}

void Bucket::generate_bordercrash_events ( SweepFacet *sf, double ctime, int gid, Container<Event> *econt, Event **eventlist, int &nbe )
{
  dbgprintf ( 2, "borderpointlist size --> %d\n", ( int ) borderpointlist.size() );

  if ( borderpointlist.empty() )
    return;

  std::set<std::pair<Border *, int> >::iterator it;
//   std::set<Border *>::iterator it;
  for ( it = borderpointlist.begin(); it != borderpointlist.end(); it++ )
  {
    Border *bo = ( *it ).first;
    int idpt = ( *it ).second;
    
//     Border *adjbo[2];
//     adjbo[0] = *it;
//     adjbo[1] = adjbo[0]->get_adj(1);

//     for ( int i = 0; i < 2; i++ )
//     {
//       Border *bo = adjbo[i];
//       Point *pt = adjbo[i]->get_point ( i );
      Point *pt = bo->get_point ( idpt );
      double crashpos[3], crashtime;

      if ( vertical_crash ( pt->p, 1.0, sf, crashpos ) )
      {
        bool good_face_of_border = ( geom::cross_product2d ( sf->get_plane(), bo->get_dir() ) >= 0.0 );
        if ( good_face_of_border && sf->get_frontline()->valid_future_event ( pt->p ) )
        {
          dbgprintf ( 2, "FACET - BORDER : crashpos --> %lf %lf %lf\n", crashpos[0], crashpos[1], crashpos[2] );
          crashtime = sf->time_to_position ( crashpos );
          dbgprintf ( 2, "crashtime --> %lf\n", crashtime );
          if ( crashtime >= ctime )
          {
            if ( ( idpt == 0 && geom::scalar_product2d ( sf->get_plane(), bo->get_dir() ) > 0.0 ) || ( idpt == 1 && geom::scalar_product2d ( sf->get_plane(), bo->get_dir() ) < 0.0 ) )
            {
              Event *ne = econt->get();
              ne->init ( sf, bo, crashpos, crashtime, Event::FACET_BORDERCRASH, gid );
              eventlist[nbe++] = ne;
            }
          }
        }
      }
//     }
  }
}

int Bucket::vertical_crash ( double *pt, double z, SweepFacet *sf, double *crashpos )
{
  static double dir[3] = {0.0, 0.0, 1.0}; // vertical ray
  dir[2] = z;
  if ( geom::line3d_plane_intersection ( pt, dir, sf->get_plane(), basic_grid::epsilon, crashpos ) )
  {
//     printf ( "crashpos --> %lf %lf %lf\n", crashpos[0], crashpos[1], crashpos[2] );
    if ( geom::sqrtdistance2d ( sf->get_start()->p, crashpos ) > basic_grid::epsilon )
    {
      dbgprintf ( 2, "--> VERTICAL RAY - FACET intersection found\n" );
      return 1;
    }
  }
  return 0;
}

int Bucket::point_already_covered ( double *pt, double ctime )
{
  if ( facetlist.empty() )
    return 0;
  dbgprintf ( 2, "facetlist size --> %d\n", ( int ) facetlist.size() );
  std::set<SweepFacet *>::iterator it;
  for ( it = facetlist.begin(); it != facetlist.end(); it++ )
  {
    SweepFacet *sf = ( *it );
    double crashpos[3];
    if ( vertical_crash ( pt, -1.0, sf, crashpos ) )
    {
      dbgprintf ( 2, "VERTICAL RAY - FACET : crashpos --> %lf %lf %lf\n", crashpos[0], crashpos[1], crashpos[2] );
      if ( crashpos[2] > 0.0 && crashpos[2] < ctime )
      {
        return 1;
      }
    }
  }
  return 0;
}

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

  tprintf ( "Writing 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 ) );
    }

  fprintf ( fp_rw, "SCALARS nb_faces 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_bucket ( get_index ( i, j ) )->get_sweepfacet_size() );
    }

  fclose ( fp_rw );
}