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

using namespace vorosweep;

int Front::prev[2] = {1, 0};
int Front::next[2] = {1, 0};

FrontLine::FrontLine ( SweepFacet *sf, SweepEdge *s0, SweepEdge *s1 )
{
  owner = sf;

  sf->set_initial_sweepedge ( 0, s0 );
//   s0->set_initial_sweepfacet ( 1, sf );
  sf->set_initial_sweepedge ( 1, s1 );
//   s1->set_initial_sweepfacet ( 0, sf );

  sf->set_frontline ( this );

  Front *fr = new Front ( this );
  s0->set_front ( 1, fr );
  s1->set_front ( 0, fr );
  frt.push_back ( fr );
  active_front = 1;
  Polygon *pol = new Polygon ( fr, s0, s1 );
  plist.push_back ( pol );
}

void FrontLine::update_sweepedge ( SweepEdge *se, SweepEdge *nse )
{
  ASSERT_ERROR(se->is_stopped(), "sweepedge should be stopped before\n");
  
  dbgprintf ( 2, "--> updating sweepedge %p to %p\n", se, nse );
  dbgprintf ( 2, "se dir --> %lf %lf\n", se->get_dir3d() [0], se->get_dir3d() [1] );
  dbgprintf ( 2, "nse dir --> %lf %lf\n", nse->get_dir3d() [0], nse->get_dir3d() [1] );
  std::list<Front *>::iterator it;
  for ( it = frt.begin(); it != frt.end(); it++ )
  {
    Front *fr = *it;
    for ( int j = 0; j < 2; j++ )
    {
      if ( fr->cse[j] == se ) // warning not select an already closed front
      {
        Polygon *pol = fr->get_polygon();
        pol->insert_edge ( fr, j, nse );
        // update front edge
        fr->update_sweepedge ( j, nse );
        return;
      }
    }
  }
  ASSERT_ERROR(0, "update sweepedge failed\n");
}

// void FrontLine::trigger ( Front *fr, SweepEdge *se )
// {
//   fr->ite = elist.begin();
//   while ( *fr->ite != se )
//     fr->ite++;
// }

Front *FrontLine::front_merge ( Front *fr0, Front *fr1 )
{
  dbgprintf ( 2, "--> merging fronts\n" );
  Front *frn = new Front ( this );
  frn->set_parents ( fr0, fr1 );
  SweepEdge *s0 = fr0->get_sweepedge ( 0 );
  SweepEdge *s1 = fr1->get_sweepedge ( 1 );
  dbgprintf ( 2, "--> ext edges : %p %p\n", s0, s1);
  frn->update_sweepedge ( 0, s0 );
  frn->update_sweepedge ( 1, s1 );
	frn->print();
  s0->print();
  s1->print();
	dbgprintf ( 2, "--> after update : s0 fr1 = %p, s1 fr0 = %p\n", s0->get_front(1), s1->get_front(0) );

  Polygon *pol0 = fr0->get_polygon();
  Polygon *pol1 = fr1->get_polygon();
  Point *common = fr0->cse[1]->get_current();

  ASSERT_ERROR(common == fr1->cse[0]->get_current(), "coherency problem for front merging\n");

  if ( pol0 == pol1 ) // same polygon
  {
    pol0->remove_hole ( fr0, fr1 );
    frn->set_polygon ( pol0 );
    frn->ite = fr1->ite;
  }
  else
  {
    pol0->merge_with ( pol1, common );
    frn->set_polygon ( pol0 );
    // remove pol1
    std::list<Polygon *>::iterator itp;
    for ( itp = plist.begin(); itp != plist.end(); itp++ )
    {
      if ( *itp == pol1 )
        itp = plist.erase ( itp );
    }
    pol0->seek_front ( frn, s1 );
  }

  // close fronts
  fr0->close();
  fr1->close();
  // remove fr0 and fr1
  dbgprintf(2, "--> erasing fronts %p and %p\n", fr0, fr1);
  std::list<Front *>::iterator itf;
  for ( itf = frt.begin(); itf != frt.end(); itf++ )
  {    
    if ( *itf == fr0 || *itf == fr1 )
    {
      itf = frt.erase ( itf );
      itf--;
    }
  }
  // insert frn
  frt.insert ( itf, frn );
  // decrement counter
  active_front--;
  // return new front
  return frn;
}

// int FrontLine::valid_future_event ( double *pt )
// {
//   double pn[2][2];
//   owner->get_initial_sweepedge ( 0 )->print();
//   owner->get_initial_sweepedge ( 1 )->print();
//   geom::translate2d ( owner->get_start()->p, owner->get_initial_sweepedge ( 0 )->get_dir2d(), 1.0, pn[0] );
//   geom::translate2d ( owner->get_start()->p, owner->get_initial_sweepedge ( 1 )->get_dir2d(), 1.0, pn[1] );
//   
//   dbgprintf(2, "start %lf %lf --> tri with %lf %lf -- %lf %lf\n", owner->get_start()->p[0], owner->get_start()->p[1], pn[0][0], pn[0][1], pn[1][0], pn[1][1] );
//   if ( geom::ccw ( owner->get_start()->p, pt, pn[0] ) >= 0.0 && geom::ccw ( owner->get_start()->p, pn[1], pt ) >= 0.0 )
//   {
//     dbgprintf ( 2, "ccw --> %.20lf %.20lf\n", geom::ccw ( owner->get_start()->p, pt, pn[0] ), geom::ccw ( owner->get_start()->p, pn[1], pt ) );
//     dbgprintf ( 2, "--> point %lf %lf is inside sweepfacet %p bounds\n", pt[0], pt[1], owner );
//     return 1;
//   }
//   dbgprintf ( 2, "ccw --> %.20lf %.20lf\n", geom::ccw ( owner->get_start()->p, pt, pn[0] ), geom::ccw ( owner->get_start()->p, pn[1], pt ) );
//   dbgprintf ( 2, "--> point %lf %lf is out of sweepfacet %p bounds\n", pt[0], pt[1], owner );
//   
//   // test with original sweepedges
//   geom::translate2d ( owner->get_start()->p, owner->get_original_sweepedge ( 0 )->get_dir2d(), 1.0, pn[0] );
//   geom::translate2d ( owner->get_start()->p, owner->get_original_sweepedge ( 1 )->get_dir2d(), 1.0, pn[1] );
//   if ( geom::ccw ( owner->get_start()->p, pt, pn[0] ) >= 0.0 && geom::ccw ( owner->get_start()->p, pn[1], pt ) >= 0.0 )
//   {
//     dbgprintf ( 2, "ccw --> %.20lf %.20lf\n", geom::ccw ( owner->get_start()->p, pt, pn[0] ), geom::ccw ( owner->get_start()->p, pn[1], pt ) );
//     dbgprintf ( 2, "--> point %lf %lf is inside sweepfacet %p bounds\n", pt[0], pt[1], owner );
//     return 1;
//   }
//   dbgprintf ( 2, "ccw --> %.20lf %.20lf\n", geom::ccw ( owner->get_start()->p, pt, pn[0] ), geom::ccw ( owner->get_start()->p, pn[1], pt ) );
//   dbgprintf ( 2, "--> point %lf %lf is out of sweepfacet %p bounds\n", pt[0], pt[1], owner );
//   
//   return 0;
// }

int FrontLine::valid_future_event ( double *pt )
{
  double vec[2];
  SweepEdge *se0 = owner->get_initial_sweepedge ( 0 );
  SweepEdge *se1 = owner->get_initial_sweepedge ( 1 );
  
//   se0->print();
//   se1->print();
  
  geom::vec2d ( owner->get_start()->p, pt, vec );
  
//   dbgprintf(2, "start %lf %lf --> tri with %lf %lf -- %lf %lf\n", owner->get_start()->p[0], owner->get_start()->p[1], pn[0][0], pn[0][1], pn[1][0], pn[1][1] );
  if ( geom::cross_product2d ( vec, se0->get_dir2d() ) >= 0.0 && geom::cross_product2d ( se1->get_dir2d(), vec ) >= 0.0 )
  {
    dbgprintf ( 2, "cross --> %.20lf %.20lf\n", geom::cross_product2d ( vec, se0->get_dir2d() ), geom::cross_product2d ( se1->get_dir2d(), vec ) );
    dbgprintf ( 2, "--> point %lf %lf is inside sweepfacet %p bounds\n", pt[0], pt[1], owner );
    return 1;
  }
  dbgprintf ( 2, "cross --> %.20lf %.20lf\n", geom::cross_product2d ( vec, se0->get_dir2d() ), geom::cross_product2d ( se1->get_dir2d(), vec ) );
  dbgprintf ( 2, "--> point %lf %lf is out of sweepfacet %p bounds\n", pt[0], pt[1], owner );
  
  // test with original sweepedges
  se0 = owner->get_original_sweepedge ( 0 );
  se1 = owner->get_original_sweepedge ( 1 );
  if ( geom::cross_product2d ( vec, se0->get_dir2d() ) >= 0.0 && geom::cross_product2d ( se1->get_dir2d(), vec ) >= 0.0 )
  {
    dbgprintf ( 2, "cross --> %.20lf %.20lf\n", geom::cross_product2d ( vec, se0->get_dir2d() ), geom::cross_product2d ( se1->get_dir2d(), vec ) );
    dbgprintf ( 2, "--> point %lf %lf is inside sweepfacet %p bounds\n", pt[0], pt[1], owner );
    return 1;
  }
  dbgprintf ( 2, "cross --> %.20lf %.20lf\n", geom::cross_product2d ( vec, se0->get_dir2d() ), geom::cross_product2d ( se1->get_dir2d(), vec ) );
  dbgprintf ( 2, "--> point %lf %lf is out of sweepfacet %p bounds\n", pt[0], pt[1], owner );
  
  return 0;
}

int FrontLine::valid_event ( double *pt, double time )
{
  Front *fr = get_including_front ( pt, time );
  if ( fr )
    return 1;
  else
    return 0;
}

void FrontLine::insert_edges ( SweepEdge *s0, SweepEdge *s1, double *pos, double time )
{
  dbgprintf ( 2, "--> inserting edges\n" );
  Front *fr = get_including_front ( pos, time );
  
  ASSERT_ERROR(fr, "we did not find the front containing the point %lf %lf\n", pos[0], pos[1]);
  
//   if ( fr )
//   {
    dbgprintf ( 2, "--> splitting front\n" );
    Polygon *pol = fr->get_polygon();
    pol->insert_edges ( fr, s0, s1 );
    fr->split ( s0, s1 );
    substitute_front_children ( fr );
    active_front++;    
//   }
//   else
//   {
//     printf ( "/!\\ --> error : we did not find the front containing the point" );
//     getchar();
//   }
}

int FrontLine::seek_front ( Front *fr )
{
  for ( itfr = frt.begin(); itfr != frt.end(); itfr++ )
  {
    if ( ( *itfr ) == fr )
      return 1;
  }
  return 0;
}

void FrontLine::substitute_front_children ( Front *fr )
{
  if ( seek_front ( fr ) )
  {
    itfr = frt.erase ( itfr );
    if ( fr->child[0] )
      frt.insert ( itfr, fr->child[0] );
    if ( fr->child[1] )
      frt.insert ( itfr, fr->child[1] );
  }
}

void FrontLine::erase_front ( Front *fr )
{
  if ( seek_front ( fr ) )
  {
    itfr = frt.erase ( itfr );
  }
}

void FrontLine::close_front ( Front *fr )
{
  if ( fr->isopen() )
  {
    dbgprintf ( 2, "--> closing front\n" );
    fr->close();
    active_front--;    
    if (active_front == 0)
      get_owner()->set_stopped();
  }
}

// provide informations on where is the point *pos on the frontline
Front *FrontLine::get_including_front ( double *pos, double time )
{
  std::list<Front *>::iterator it;
  for ( it = frt.begin(); it != frt.end(); it++ )
  {
    Front *fr = *it;
    if ( fr->isopen() && fr->inside ( pos, time ) )
      return fr;
  }
  return NULL;
}

void FrontLine::update_current ( double time )
{
  for ( itfr = frt.begin(); itfr != frt.end(); itfr++ )
  {
    Front *fr = *itfr;
    fr->update_current ( time );
//     if ( !fr->isopen() )
//       itfr = frt.erase ( itfr );
  }
}

void FrontLine::print()
{
  std::list<Front *>::iterator it;
  for ( it = frt.begin(); it != frt.end(); it++ )
  {
    Front *fr = *it;
    fr->print();
  }
//   std::list<SweepEdge*> list = get_edges();
//   dbgprintf ( 1, "frontline --> %d edges\n", ( int ) list.size() );
//   for ( std::list<SweepEdge *>::iterator it = list.begin(); it != list.end(); it++ )
//     dbgprintf ( 1, "--> %lf %lf %lf\n", ( *it )->p[0], ( *it )->p[1], ( *it )->p[2] );
}

Polygon::Polygon ( Front *fr, SweepEdge *s0, SweepEdge *s1 )
{
  fr->set_polygon ( this );
  elist.push_back ( s0 );
  elist.push_back ( s1 );
  fr->ite = elist.begin();
  fr->ite++;
}

// routine launched for export
void Polygon::generate_point_list ( )
{
  vlist.clear();
  std::list<SweepEdge *>::iterator it = elist.begin();
  Point *cur = NULL;
  Point *last = NULL;
  SweepEdge *selast = NULL;
  dbgprintf ( 3, "--> frontline point generation :\n" );
  vlist.push_back ( ( *elist.begin() )->get_start() ); // first point
  do
  {
    SweepEdge *se = *it;
    dbgprintf ( 3, "se --> %p : start %p -- current %p\n", se, se->get_start(), se->get_current() );
    if ( selast && selast != ( *elist.begin() ) && selast->get_start() == se->get_start() )
      vlist.push_back ( se->get_start() );
    cur = se->get_current();
    dbgprintf ( 3, "cur --> %p\n", cur );
    if ( cur != last )
      vlist.push_back ( cur );
    last = cur;
    selast = se;
    it++;
  }
  while ( it != elist.end() );
//   printf("result : \n");
//   for ( std::list<Point *>::iterator itv = vlist.begin(); itv != vlist.end(); itv++ )
//     printf("--> pt %p = %lf %lf %lf\n", (*itv), (*itv)->p[0], (*itv)->p[1], (*itv)->p[2]);
}

void Polygon::merge_with ( Polygon *mpol, Point* pt )
{
  dbgprintf ( 2, "--> merging polygons\n" );
  std::list<SweepEdge *> nlist; // new list
//   std::list<SweepEdge *>::iterator itn = nlist.begin();
  // iterator on old lists
  std::list<SweepEdge *>::iterator it0 = elist.begin();
  std::list<SweepEdge *>::iterator it1 = mpol->elist.begin();

  // find pt in elist0 and elist1
  while ( ( *it0 )->get_current() != pt )
    it0++;
  while ( ( *it1 )->get_current() != pt )
    it1++;
  dbgprintf ( 2, "it0 --> %p : start %p -- current %p, stopped ? %d\n", ( *it0 ), ( *it0 )->get_start(), ( *it0 )->get_current(), ( *it0 )->is_stopped() );
  dbgprintf ( 2, "it1 --> %p : start %p -- current %p, stopped ? %d\n", ( *it1 ), ( *it1 )->get_start(), ( *it1 )->get_current(), ( *it1 )->is_stopped() );

  // increment it1 for convenient insertion
  it1++;
  nlist.insert ( nlist.end(), elist.begin(), it0 );
//   SweepEdge *se0 = *nlist.rbegin();
  nlist.insert ( nlist.end(), it1, mpol->elist.end() );
//   SweepEdge *se1 = *nlist.rbegin();
  nlist.insert ( nlist.end(), mpol->elist.begin(), it1 );
  nlist.insert ( nlist.end(), it0, elist.end() );
  elist.assign ( nlist.begin(),nlist.end() );
}

Front::Front ( FrontLine *line )
{
  fl = line;
  pol = NULL;
  parent[0] = parent[1] = NULL;
  child[0] = child[1] = NULL;
  cse[0] = cse[1] = NULL;
  state = Front::OPEN;
  trait = Front::PRIMARY;
}

int Front::inside ( double *pt, double t )
{
  if ( cse[0]->is_stopped() && cse[1]->is_stopped() )
  {
    close();
    return 0;
  }
  double pse0[2];
  cse[0]->position_at_time ( t, pse0 );
  double pse1[2];
  cse[1]->position_at_time ( t, pse1 );
  dbgprintf ( 1, "cheking inside front %lf %lf < %lf %lf < %lf %lf\n", pse0[0], pse0[1], pt[0], pt[1], pse1[0], pse1[1] );
  if ( geom::point_inside_segment2d ( pse0, pse1, 1e-12, pt ) )
  {
    dbgprintf ( 2, "... ok\n" );
    return 1;
  }
  dbgprintf ( 2, "... not ok\n" );
  return 0;
}

void Front::split ( SweepEdge *s0, SweepEdge *s1 )
{
  Front *frn0 = new Front ( fl );
  Front *frn1 = new Front ( fl );
  set_children ( frn0, frn1 );
  cse[0]->set_front ( 1, frn0 );
  s0->set_front ( 0, frn0 );
  s1->set_front ( 1, frn1 );
  cse[1]->set_front ( 0, frn1 );
  frn0->set_polygon ( pol );
  frn1->set_polygon ( pol );
  frn0->ite = ite;
  frn1->ite = ite;
  for ( int j = 0; j < 2; j++ )
    frn0->ite--;
}


int Front::self_check()
{
  return 0;
}

void Front::print()
{
  dbgprintf ( 2, "## FRONT %p :\n", this );
  dbgprintf ( 2, "## se0 %p --> start %lf %lf, dir %lf %lf\n", cse[0], cse[0]->get_start()->p[0], cse[0]->get_start()->p[1], cse[0]->get_dir3d() [0], cse[0]->get_dir3d() [1] );
  dbgprintf ( 2, "## se1 %p --> start %lf %lf, dir %lf %lf\n", cse[1], cse[1]->get_start()->p[0], cse[1]->get_start()->p[1], cse[1]->get_dir3d() [0], cse[1]->get_dir3d() [1] );
}

void Front::update_current ( double time )
{
  if ( !cse[0]->is_stopped() )
    cse[0]->update_current ( time );
  if ( !cse[1]->is_stopped() )
    cse[1]->update_current ( time );

//   if ( cse[0]->is_stopped() && cse[1]->is_stopped() )
//     close();
}