// 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 "vorosweep.h"
#include "border.h"
#include "sweepobject.h"

using namespace vorosweep;

void Graph::handle_facet_bordercrash ( Event *ev )
{
  SweepFacet *sf = ev->get_sweepfacet();

  Border *bo = ev->get_border();
  bo->print();

  double *pos = ev->get_coord();
  double time = ev->get_time();

  FrontLine *fl = sf->get_frontline();
  Front *fr = fl->get_including_front ( pos, time );

  if ( fr == NULL )
    return;
  
  fr->get_sweepedge(0)->print();
  fr->get_sweepedge(1)->print();
  
  // check if the border is not the same that one of the front edge is following
  if (fr->get_sweepedge(0)->get_type() == SweepObject::BORDEREDGE)
  {
    Border *cbo = reinterpret_cast<BorderSweepEdge *>(fr->get_sweepedge(0))->get_border();
    cbo->print();
    if (cbo == bo || bo == cbo->get_adj(1))
      return;
  }
  
  if (fr->get_sweepedge(1)->get_type() == SweepObject::BORDEREDGE)
  {
    Border *cbo = reinterpret_cast<BorderSweepEdge *>(fr->get_sweepedge(1))->get_border();
    cbo->print();
    if (cbo == bo || bo == cbo->get_adj(0))
      return;
  }

  // first identify which point the facet encountered
  int ptindex;
  Border *bo0 = NULL;
  Border *bo1 = NULL;
  if ( geom::sqrtdistance2d ( pos, bo->get_point ( 0 )->p ) < geom::sqrtdistance2d ( pos, bo->get_point ( 1 )->p ) )
  {
    ptindex = 0;
    bo0 = bo;
    bo1 = bo->get_adj ( 1 );
  }
  else
  {
    ptindex = 1;
    bo0 = bo->get_adj ( 0 );
    bo1 = bo;
  }

  // then create new edges
  double ndir[2][3];
  sf->dir2d_to_dir3d ( bo0->get_dir(), ndir[0]);
  sf->dir2d_to_dir3d ( bo1->get_dir(), ndir[1]);
  
  geom::multvec3d ( ndir[1], -1.0 ); // revert ndir[1]

  SweepEdge *se0 = sf->get_original_sweepedge ( 0 );
  SweepEdge *se1 = sf->get_original_sweepedge ( 1 );

  SweepEdge *nse0 = NULL;
  SweepEdge *nse1 = NULL;

  if ( ptindex == 0 )
  {
    Point *pt = new Point;
    pt->set ( bo->get_point ( 0 )->p );
    pt->p[2] = time;
    BorderSweepEdge *nsbe0 = new BorderSweepEdge ( pt, ndir[0], time );
    nsbe0->update_speeds ( sf );
    nsbe0->set_border ( bo0 );

    // add switch event
    Event *nes = econtainer->get();
    nes->init ( nsbe0, bo0, nsbe0->get_start()->p, time, Event::NEWBORDEREDGESWITCH, bo0->get_first_bucket_index() );
    add_priority_event ( nes );

    nse0 = nsbe0;

    if ( geom::cross_product2d ( ndir[1], se0->get_dir2d() ) > 0.0 )
    {
      BorderSweepEdge *nsbe1 = new BorderSweepEdge ( pt, ndir[1], time );
      nsbe1->set_border ( bo1 );
      // add switch event
      Event *nes = econtainer->get();
      nes->init ( nsbe1, bo1, nsbe1->get_start()->p, time, Event::NEWBORDEREDGESWITCH, bo1->get_last_bucket_index() );
      add_priority_event ( nes );
      nsbe1->update_speeds ( sf );
      nse1 = nsbe1;
    }
    else
    {
      nse1 = singular_point_rotation_ccw ( sf, bo->get_adj(1), pt, time );
    }
    nse1->update_speeds ( sf );
  }
  else
  {
    Point *pt = new Point;
    pt->set ( bo->get_point ( 1 )->p );
    pt->p[2] = time;
    
    if ( geom::cross_product2d ( se1->get_dir2d(), ndir[0] ) >= 0.0 )
    {
      BorderSweepEdge *nsbe0 = new BorderSweepEdge ( pt, ndir[0], time );
      nsbe0->set_border ( bo0 );
      // add switch event
      Event *nes = econtainer->get();
      nes->init ( nsbe0, bo0, nsbe0->get_start()->p, time, Event::NEWBORDEREDGESWITCH, bo0->get_first_bucket_index() );
      add_priority_event ( nes );
      nsbe0->update_speeds ( sf );
      nse0 = nsbe0;
    }
    else
    {
      nse0 = singular_point_rotation_cw ( sf, bo->get_adj(0), pt, time );
    }

//     printf("ndir --> %lf %lf %lf\n", ndir[1][0], ndir[1][1], ndir[1][2]);
    BorderSweepEdge *nsbe1 = new BorderSweepEdge ( pt, ndir[1], time );
    nsbe1->update_speeds ( sf );
    nsbe1->set_border ( bo1 );
    // add switch event
    Event *nes = econtainer->get();
    nes->init ( nsbe1, bo1, nsbe1->get_start()->p, time, Event::NEWBORDEREDGESWITCH, bo1->get_last_bucket_index() );
    add_priority_event ( nes );

    nse1 = nsbe1;
  }

  fl->insert_edges ( nse0, nse1, pos, time ); // note the change in orientation
  
  add_earliest_event(sf);
//   nse0->print();
//   nse1->print();
//   fl->print();
//   getchar();
}



// this routine generate as much as facet as needed to circumvent obstacle
SweepEdge *Graph::singular_point_rotation_ccw ( SweepFacet *sf, Border *bo, Point *from, double time )
{
  dbgprintf(2, "--> COUNTERCLOCKWISE rotation\n");
//   int bgid = bo->get_grid()->get_bucket_size( )-1;
//   sf->dir2d_to_dir3d ( nextborder->get_dir(), ndir );
  Convex_generator *cg = sf->get_generator();
    
  int gid = bucketgrid->get_index ( from->p );

  double *normal;
  double ndir[3];

  int index = sf->get_pos();
  int nbfacet = 0;
  
  SweepEdge *lastse = new SweepEdge ( from, sf->get_original_sweepedge ( 0 )->get_dir3d(), time );
  lastse->update_speeds ( cg->get_next_facet ( index ) );
  lastse->set_init();
  
  SweepEdge *inse1 = lastse;

  // add switch event
  Event *nes = econtainer->get();
  nes->init ( lastse, lastse->get_start()->p, time, Event::NEWEDGESWITCH, gid );
  add_priority_event ( nes );   
  
  SweepEdge *se0 = NULL;
  SweepEdge *se1 = NULL;  
  
  dbgprintf ( 2, "init normal --> %lf %lf %lf\n", sf->get_plane() [0], sf->get_plane() [1], sf->get_plane() [2] );
  do
  {
    SweepFacet *nextsf = cg->get_next_facet ( index );
    int nextindex = nextsf->get_pos();
    normal = nextsf->get_plane();
    dbgprintf ( 2, "checking with normal --> %lf %lf %lf\n", normal[0], normal[1], normal[2] );
    nextsf->dir2d_to_dir3d ( bo->get_dir(), ndir );
    geom::multvec3d(ndir, -1.0);
    dbgprintf ( 2, "ndir --> %lf %lf %lf\n", ndir[0], ndir[1], ndir[2] );
        
    SweepFacet *nsf = new SweepFacet ( from, normal, nextsf->get_dir3d(), time );
    SweepEdge *nse;
    
    se0 = nextsf->get_original_sweepedge ( 0 );
    se1 = nextsf->get_original_sweepedge ( 1 );
    
    dbgprintf(2, "border dir --> %lf %lf\n", bo->get_dir()[0], bo->get_dir()[1]);    
    dbgprintf(2, "dir 0 --> %lf %lf\n", se0->get_dir2d()[0], se0->get_dir2d()[1]);
    dbgprintf(2, "dir 1 --> %lf %lf\n", se1->get_dir2d()[0], se1->get_dir2d()[1]);    
    dbgprintf(2, "cross --> %lf %lf\n", geom::cross_product2d ( bo->get_dir(), se0->get_dir2d() ), geom::cross_product2d ( se1->get_dir2d(), bo->get_dir() ));
    
    if ( geom::cross_product2d ( bo->get_dir(), se0->get_dir2d() ) > 0.0 || geom::cross_product2d ( se1->get_dir2d(), bo->get_dir() ) > 0.0 )
//     if (!geom::is_between(bo->get_dir(), se0->get_dir2d(), se1->get_dir2d()))
    {
      nse = new SweepEdge ( from, se0->get_dir3d(), time );
      nse->set_init();
      // add switch event
      Event *nes = econtainer->get();
      nes->init ( nse, nse->get_start()->p, time, Event::NEWEDGESWITCH, gid );
      add_priority_event ( nes );
    }
    else
    {
      BorderSweepEdge *nbse = new BorderSweepEdge ( from, ndir, time );
      nbse->set_border ( bo );
      nse = nbse;
      // add switch event
      Event *nes = econtainer->get();
      nes->init ( nse, bo, nse->get_start()->p, time, Event::NEWBORDEREDGESWITCH, bo->get_last_bucket_index() );
      add_priority_event ( nes );
    }
    
    nsf->set_pos ( nextindex );
    nsf->set_xyspeed ( nextsf->get_xyspeed() );
    nsf->set_generator ( cg );
    cg->add_sweepfacet ( nsf );
    new FrontLine ( nsf, nse, lastse );
    nse->update_speeds ( nsf );
    
    // add switch event
    Event *nfs = econtainer->get();
    nfs->init ( nsf, nsf->get_start()->p, time, Event::NEWFACETSWITCH, gid );
    add_priority_event ( nfs );
    
    index = nextindex;
    lastse = nse;
    nbfacet++;
  }
//   while (!geom::is_between(bo->get_dir(), se0->get_dir2d(), se1->get_dir2d()));
  while ( geom::cross_product2d ( bo->get_dir(), se0->get_dir2d() ) > 0.0 || geom::cross_product2d ( se1->get_dir2d(), bo->get_dir() ) > 0.0 );
//   while ( ndir[2] <= 0.0 );

  return inse1;
}

// this routine generate as much as facet as needed to circumvent obstacle
SweepEdge *Graph::singular_point_rotation_cw ( SweepFacet *sf, Border *bo, Point *from, double time )
{  
  dbgprintf(2, "--> CLOCKWISE rotation\n");
//   int bgid = 0;
  Convex_generator *cg = sf->get_generator();
  
  int gid = bucketgrid->get_index ( from->p );

  double *normal;
  double ndir[3];

  int index = sf->get_pos();
  int nbfacet = 0;
  
  SweepEdge *lastse = new SweepEdge ( from, sf->get_original_sweepedge ( 1 )->get_dir3d(), time );
  lastse->update_speeds ( cg->get_prev_facet ( index ) );
  lastse->set_init();
  
  SweepEdge *inse0 = lastse;
  
  // add switch event
  Event *nes = econtainer->get();
  nes->init ( lastse, lastse->get_start()->p, time, Event::NEWEDGESWITCH, gid );
  add_priority_event ( nes );   
  
  SweepEdge *se0 = NULL;
  SweepEdge *se1 = NULL;
      
  dbgprintf ( 2, "init normal --> %lf %lf %lf\n", sf->get_plane() [0], sf->get_plane() [1], sf->get_plane() [2] );
  do
  {
    SweepFacet *prevsf = cg->get_prev_facet ( index );
    int previndex = prevsf->get_pos();
    normal = prevsf->get_plane();
    dbgprintf ( 2, "checking with normal --> %lf %lf %lf\n", normal[0], normal[1], normal[2] );
    prevsf->dir2d_to_dir3d ( bo->get_dir(), ndir );
    dbgprintf ( 2, "ndir --> %lf %lf %lf\n", ndir[0], ndir[1], ndir[2] );
    
    SweepFacet *nsf = new SweepFacet ( from, normal, prevsf->get_dir3d(), time );
    SweepEdge *nse;
    
    se0 = prevsf->get_original_sweepedge ( 0 );
    se1 = prevsf->get_original_sweepedge ( 1 );
    
    dbgprintf( 2, "border dir --> %lf %lf\n", bo->get_dir()[0], bo->get_dir()[1]);
    dbgprintf( 2, "dir 0 --> %lf %lf\n", se0->get_dir2d()[0], se0->get_dir2d()[1]);
    dbgprintf( 2, "dir 1 --> %lf %lf\n", se1->get_dir2d()[0], se1->get_dir2d()[1]);    
    dbgprintf( 2, "cross --> %lf %lf\n", geom::cross_product2d ( bo->get_dir(), se0->get_dir2d() ), geom::cross_product2d ( se1->get_dir2d(), bo->get_dir() ));
  
//     if ( geom::cross_product2d ( bo->get_dir(), se0->get_dir2d() ) < 0.0 || geom::cross_product2d ( se1->get_dir2d(), bo->get_dir() ) < 0.0 )
    if (!geom::is_between(bo->get_dir(), se0->get_dir2d(), se1->get_dir2d()))
    {
      nse = new SweepEdge ( from, se1->get_dir3d(), time );
      nse->set_init();
      // add switch event
      Event *nes = econtainer->get();
      nes->init ( nse, nse->get_start()->p, time, Event::NEWEDGESWITCH, gid );
      add_priority_event ( nes );
    }
    else
    {
      BorderSweepEdge *nbse = new BorderSweepEdge ( from, ndir, time );
      nbse->set_border ( bo );
      nse = nbse;
      // add switch event
      Event *nes = econtainer->get();
      nes->init ( nse, bo, nse->get_start()->p, time, Event::NEWBORDEREDGESWITCH, bo->get_first_bucket_index() );
      add_priority_event ( nes );
    }
    
    nsf->set_pos ( previndex );
//     printf("prev speed --> %lf\n", prevsf->get_xyspeed());
    nsf->set_xyspeed ( prevsf->get_xyspeed() );
    nsf->set_generator ( cg );
    cg->add_sweepfacet ( nsf );
    new FrontLine ( nsf, lastse, nse );
    nse->update_speeds ( nsf );
    
    // add switch event
    Event *nfs = econtainer->get();
    nfs->init ( nsf, nsf->get_start()->p, time, Event::NEWFACETSWITCH, gid );
    add_priority_event ( nfs );
        
    index = previndex;
    lastse = nse;
    nbfacet++;
  }
  while (!geom::is_between(bo->get_dir(), se0->get_dir2d(), se1->get_dir2d()));
//   while ( geom::cross_product2d ( bo->get_dir(), se0->get_dir2d() ) < 0.0 || geom::cross_product2d ( se1->get_dir2d(), bo->get_dir() ) < 0.0 );
//   while ( ndir[2] <= 0.0 );

  return inse0;
}