// 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"

using namespace vorosweep;

void Graph::handle_borderedgeswitch ( Event *ev )
{
  SweepEdge *se = ev->get_sweepedge();

  if ( se->is_stopped() )
    return;

  double *pos = ev->get_coord();
  double time = ev->get_time();
//   se->clear_events(); // not needed in borderedge switch
  se->update_current ( time );
  int gid = ev->get_gid();
  se->set_last_gid ( gid );

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

  if ( gid == -1 )
  {
    if (check_for_corner ( se, bo, time ) )
      return;
    
    switch_border ( se, bo, time );
    return;
  }

  // compute next edge switch event
  Event *next = next_borderedgeswitch ( se, bo, pos, gid );
  dbgprintf ( 5, "--> next switch event add :\n" );
  add_event ( next );
  // add sweepedge to the newly entered cell
  bordergrid->insert_sweepedge ( se, gid );
  dbgprintf ( 5, "sweepedge se dir --> %lf %lf\n",  se->get_dir2d() [0], se->get_dir2d() [1] );
  // we then generate potential crash events in border
  BorderBucket *buc = bordergrid->get_bucket ( gid );
  Event *eventlist[MAX_EVENT_LIST];
  int nbe = 0;
  buc->generate_edgecrash_events ( se, get_time(), gid, econtainer, eventlist, nbe );
//   if ( nbe )
//   {
//     // we add the earliest event of se into the priority queue
//     Event *earliest = se->get_earliest_event();
//     dbgprintf ( 5, "--> earliest event add\n" );
//     add_event ( earliest );
//   }
  add_earliest_event(se);
}

int Graph::check_for_corner ( SweepEdge *se, Border *bo, double time )
{
  if (!se->get_front(0))
  {
    SweepEdge *se1 = se->get_front(1)->get_sweepedge(1);
    if (se1->get_type() == SweepObject::BORDEREDGE)
    {
      Border *cbo = reinterpret_cast<BorderSweepEdge *>(se1)->get_border();
      if (cbo == bo->get_adj(1))
      {
        se->set_stopped();
        se1->set_stopped();
        return 1;
      }
    }
  }
  else if (!se->get_front(1))
  {
    SweepEdge *se0 = se->get_front(0)->get_sweepedge(0);
    if (se0->get_type() == SweepObject::BORDEREDGE)
    {
      Border *cbo = reinterpret_cast<BorderSweepEdge *>(se0)->get_border();
      if (cbo == bo->get_adj(0))
      {
        se->set_stopped();
        se0->set_stopped();
        return 1;
      }
    }
  }
  return 0;
}

void Graph::switch_border ( SweepEdge *se, Border *bo, double time )
{
  // find the direction of the next borderbucket
  dbgprintf ( 2, "--> switching border\n" );
  Border *nextborder = NULL;
  SweepFacet *sf = NULL;
  int bgid = -1;
  BorderSweepEdge *nse = NULL;
  double ndir[3];
  if ( geom::scalar_product2d ( se->get_dir2d(), bo->get_dir() ) > 0.0 )
  {
    nextborder = bo->get_adj ( 0 );
    sf = se->get_front ( 0 )->get_owner();
    bgid = nextborder->get_first_bucket_index();;
    sf->dir2d_to_dir3d ( nextborder->get_dir(), ndir );
    dbgprintf(2, "ndir --> %lf %lf %lf\n", ndir[0], ndir[1], ndir[2]);
    dbgprintf(2, "cross --> %lf\n", geom::cross_product2d(sf->get_original_sweepedge(1)->get_dir2d(), ndir));
    // simplest case --> 
    if ( ndir[2] >= 0.0 && geom::cross_product2d(sf->get_original_sweepedge(1)->get_dir2d(), ndir) >= 0.0)
    {
      nse = new BorderSweepEdge ( se->get_current(), ndir, time );
    }
    else
    {
      // check next bucket of nextborder
      BorderBucket *buc = nextborder->get_grid()->get_bucket(bgid);
      std::set<SweepEdge *> &elist = buc->get_edge_list();      
      for (std::set<SweepEdge *>::iterator it = elist.begin(); it != elist.end(); it++ )
      {
        BorderSweepEdge *nextse = reinterpret_cast<BorderSweepEdge *>(*it);
        if (nextse && nextse->get_front(1) && nextse->get_front(1)->get_owner() == se->get_front(0)->get_owner())
        {
          dbgprintf(2, "--> same face : merging\n");
          se->merge_current ( nextse, get_time() );
          FrontLine *fl = se->get_front(0)->get_frontline();
          fl->front_merge ( se->get_front(0), nextse->get_front(1) );
//           getchar();
          return;
        }
      }
      
//       if ( geom::cross_product2d ( bo->get_dir(), nextborder->get_dir() ) < 0.0 )
//       {
        singular_point_rotation_cw ( se, nextborder, time );
        return;
//       }
    }
  }
  else
  {
    nextborder = bo->get_adj ( 1 );
    sf = se->get_front ( 1 )->get_owner();
    bgid = nextborder->get_last_bucket_index();
    sf->dir2d_to_dir3d ( nextborder->get_dir(), ndir );
    geom::multvec3d(ndir, -1.0);
    dbgprintf(2, "ndir --> %lf %lf %lf\n", ndir[0], ndir[1], ndir[2]);
    dbgprintf(2, "cross --> %lf\n", geom::cross_product2d(ndir, sf->get_original_sweepedge(0)->get_dir2d() ));
        // simplest case --> 
    if ( ndir[2] >= 0.0 && geom::cross_product2d(ndir, sf->get_original_sweepedge(0)->get_dir2d() ) >= 0.0 )
    {
      nse = new BorderSweepEdge ( se->get_current(), ndir, time );
    }
    else
    {
      // check next bucket of nextborder
      BorderBucket *buc = nextborder->get_grid()->get_bucket(bgid);
      std::set<SweepEdge *> &elist = buc->get_edge_list();      
      for (std::set<SweepEdge *>::iterator it = elist.begin(); it != elist.end(); it++ )
      {
        BorderSweepEdge *nextse = reinterpret_cast<BorderSweepEdge *>(*it);
        if (nextse && nextse->get_front(0) && nextse->get_front(0)->get_owner() == se->get_front(1)->get_owner())
        {
          dbgprintf(2, "--> same face : merging\n");
          se->merge_current ( nextse, get_time() );
          FrontLine *fl = se->get_front(1)->get_frontline();
          fl->front_merge ( nextse->get_front(0), se->get_front(1) );
//           getchar();
          return;
        }        
      }
      
//       if ( geom::cross_product2d ( bo->get_dir(), nextborder->get_dir() ) > 0.0 )
//       {
        singular_point_rotation_ccw ( se, nextborder, time );
        return;
//       }
    }
  }

  // stopping se
  se->set_stopped();

  if ( nse )
  {
    nse->set_border ( nextborder );
    nse->update_speeds ( sf );
    FrontLine *fl = sf->get_frontline();
    fl->update_sweepedge ( se, nse );
    Event *ne = econtainer->get();
    ne->init ( nse, nextborder, nse->get_start()->p, time, Event::NEWBORDEREDGESWITCH, bgid );
    add_priority_event ( ne );
  }
}

// this routine generate as much as facet as needed to circumvent obstacle
void Graph::singular_point_rotation_ccw ( SweepEdge *se, Border *nextborder, double time )
{
  dbgprintf(2, "--> COUNTERCLOCKWISE rotation\n");
  SweepFacet *sf = se->get_front ( 1 )->get_owner();
  Convex_generator *cg = sf->get_generator();
    
  int gid = bucketgrid->get_index ( se->get_current()->p );

  double *normal;
  double ndir[3];

  int index = sf->get_pos();
  int nbfacet = 0;
  
  SweepEdge *lastse = new SweepEdge ( se->get_current(), sf->get_original_sweepedge ( 0 )->get_dir3d(), time );
  lastse->update_speeds ( cg->get_next_facet ( index ) );
  lastse->set_init();
  
  // stopping se
  se->set_stopped();
  se->get_front ( 1 )->get_frontline()->update_sweepedge ( se, 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 ( nextborder->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 ( se->get_current(), 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", nextborder->get_dir()[0], nextborder->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 ( nextborder->get_dir(), se0->get_dir2d() ), geom::cross_product2d ( se1->get_dir2d(), nextborder->get_dir() ));
    
    if ( geom::cross_product2d ( nextborder->get_dir(), se0->get_dir2d() ) > 0.0 || geom::cross_product2d ( se1->get_dir2d(), nextborder->get_dir() ) > 0.0 )
    {
      nse = new SweepEdge ( se->get_current(), 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 ( se->get_current(), ndir, time );
      nbse->set_border ( nextborder );
      nse = nbse;
      // add switch event
      Event *nes = econtainer->get();
      nes->init ( nse, nextborder, nse->get_start()->p, time, Event::NEWBORDEREDGESWITCH, nextborder->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::cross_product2d ( nextborder->get_dir(), se0->get_dir2d() ) > 0.0 || geom::cross_product2d ( se1->get_dir2d(), nextborder->get_dir() ) > 0.0 );

}

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

  double *normal;
  double ndir[3];

  int index = sf->get_pos();
  int nbfacet = 0;
  
  SweepEdge *lastse = new SweepEdge ( se->get_current(), sf->get_original_sweepedge ( 1 )->get_dir3d(), time );
  lastse->update_speeds ( cg->get_prev_facet ( index ) );
  lastse->set_init();
  
  // stopping se
  se->set_stopped();
  se->get_front ( 0 )->get_frontline()->update_sweepedge ( se, 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 ( nextborder->get_dir(), ndir );
    dbgprintf ( 2, "ndir --> %lf %lf %lf\n", ndir[0], ndir[1], ndir[2] );
    
    SweepFacet *nsf = new SweepFacet ( se->get_current(), 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", nextborder->get_dir()[0], nextborder->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 ( nextborder->get_dir(), se0->get_dir2d() ), geom::cross_product2d ( se1->get_dir2d(), nextborder->get_dir() ));
  
    if ( geom::cross_product2d ( nextborder->get_dir(), se0->get_dir2d() ) < 0.0 || geom::cross_product2d ( se1->get_dir2d(), nextborder->get_dir() ) < 0.0 )
    {
      nse = new SweepEdge ( se->get_current(), 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 ( se->get_current(), ndir, time );
      nbse->set_border ( nextborder );
      nse = nbse;
      // add switch event
      Event *nes = econtainer->get();
      nes->init ( nse, nextborder, nse->get_start()->p, time, Event::NEWBORDEREDGESWITCH, nextborder->get_first_bucket_index() );
      add_priority_event ( nes );
    }
    
    nsf->set_pos ( previndex );
    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::cross_product2d ( nextborder->get_dir(), se0->get_dir2d() ) < 0.0 || geom::cross_product2d ( se1->get_dir2d(), nextborder->get_dir() ) < 0.0 );

}