// Gnurbs - A curve and surface library
// Copyright (C) 2008-2026 Eric Bechet
//
// See the LICENSE file for contributions and license information.
// Please report all bugs and problems to <bechet@cadxfem.org>.
//

#include <vector>
#include <deque>
#include <cstdio>
#include "nmesh.h"

#define __npi (3.141592653589793)
//#define DEBUG

bool cmp_hedge::operator()(const hedge* e1, const hedge* e2) const
{
  double x0,x1,y0,y1;bool ok0=false;bool ok1=false;
  npoint2 p00=e1->s->pt;
  npoint2 p01;
  if (e1->n) p01=e1->n->s->pt; else p01=e1->s->pt;
  if (p00[0]>p01[0]) {npoint2 pp=p00;p00=p01;p01=pp;}
  npoint2 p10=e2->s->pt;
  npoint2  p11;
  if (e2->n) p11=e2->n->s->pt; else p11=e2->s->pt;
  if (p10[0]>p11[0]) {npoint2 pp=p10;p10=p11;p11=pp;}
  if ((p00[0]>=p10[0]) && (p00[0]<=p11[0]))
  {
    x0=p00[0];ok0=true;
  }
  if ((p01[0]>=p10[0]) && (p01[0]<=p11[0]))
  {
    x1=p01[0];ok1=true;
  }
  if ((p10[0]>=p00[0]) && (p10[0]<=p01[0]))
  {
    x0=p10[0];ok0=true;
  }
  if ((p11[0]>=p00[0]) && (p11[0]<=p01[0]))
  {
    x1=p11[0];ok1=true;
  }
  if (ok0&&ok1)
  {
    double x=(x0+x1)/2.0;
    if (p01[0]-p00[0]==0)
      y0=(p01[1]+p00[1])/2.0; 
    else 
      y0=(x-p00[0])*(p01[1]-p00[1])/(p01[0]-p00[0])+p00[1];
    if (p11[0]-p10[0]==0) 
      y1=(p11[1]+p10[1])/2.0; 
    else 
      y1=(x-p10[0])*(p11[1]-p10[1])/(p11[0]-p10[0])+p10[1];
    return y0<y1;
  }
  throw;
}

double length2(const hedge* e)
{
  npoint2 p00=e->s->pt;
  npoint2 p01=e->n->s->pt;
  return (p00-p01).norm2();
}

double length(const hedge* e)
{
  return sqrt(length2(e));
}

bool cmp_hedge_len::operator()(const hedge* e1, const hedge* e2) const
{
  if (e1==e2) return false;
  if (e1==e2->t) return false;
  if (e2==e1->t) return false;
  
  double norm0=length2(e1);
  double norm1=length2(e2);
  if (norm0<norm1) return true;
  if (norm0>norm1) return false;
  vertex *v00,*v01,*v10,*v11;
  if (e1->s < e1->n->s) {v00=e1->s;v01=e1->n->s;} else {v01=e1->s;v00=e1->n->s;}
  if (e2->s < e2->n->s) {v10=e2->s;v11=e2->n->s;} else {v11=e2->s;v10=e2->n->s;}
  if (v00<v10) return true;
  if (v00>v10) return false;
  if (v01<v11) return true;
  return false;
}




double angle( npoint2 v1,npoint2 v2) // measures the signed angle
{
  double a=atan2(v2[1], v2[0]) - atan2(v1[1], v1[0]);
  if (a>__npi) a-=2*__npi;
  if (a<=-__npi) a+=2*__npi;
  return a; //-pi to pi
}

double angle( const vertex * v) // measures the signed angle at this vertex
{
  npoint2 v1=v->e->p->s->pt-v->pt;
  npoint2 v2=v->e->n->s->pt-v->pt;
  return angle(v1,v2);
}

bool legal(npoint2 p1,npoint2 p2,npoint2 p3,npoint2 p4)
{
//  1) test if angles are such that it is possible to swap
  double angle1=angle(p4-p1,p3-p1);
  double angle2=angle(p3-p2,p4-p2);
  
  
  if (angle1>0 && angle2>0)
  {
//    check for distances (to test)
    double dist1=(p1-p2).norm2();
    double dist2=(p4-p3).norm2();
//    return (dist1<=dist2); //cannot swap if (d1<=d2)
// check angles instead of radius
    double amin,a,bmin,b;
    amin=fabs(angle(p1-p3,p2-p3));
    a=fabs(angle(p1-p4,p2-p4));if (a<amin) amin=a;
    a=fabs(angle(p3-p1,p2-p1));if (a<amin) amin=a;
    a=fabs(angle(p2-p1,p4-p1));if (a<amin) amin=a;
    a=fabs(angle(p3-p2,p1-p2));if (a<amin) amin=a;
    a=fabs(angle(p1-p2,p4-p2));if (a<amin) amin=a;
    
    bmin=fabs(angle1);
    b=fabs(angle2);if (b<bmin) bmin=b;
    b=fabs(angle(p1-p3,p4-p3));if (b<bmin) bmin=b;
    b=fabs(angle(p4-p3,p2-p3));if (b<bmin) bmin=b;
    b=fabs(angle(p3-p4,p1-p4));if (b<bmin) bmin=b;
    b=fabs(angle(p2-p4,p3-p4));if (b<bmin) bmin=b;
    return (amin>=bmin); //cannot swap if angular vector decreases.
  }
  else return true; // cannot swap because non convex
}

bool legal(hedge *e)
{
  // edge should have a twin
  if (e)
  {
    if (e->t)
    {
      npoint2 p1=e->s->pt;
      npoint2 p2=e->n->s->pt;
      npoint2 p3=e->p->s->pt;
      npoint2 p4=e->t->p->s->pt;
      return legal(p1,p2,p3,p4);
    }
    else return true; // legal = should not swap
  }
  else return true;// legal = should not swap
}


void swap(hedge* e,swap_type& set_edges) // swap the edge e (no geo checks are made)
{
  set_edges.erase(e);
  set_edges.erase(e->t);
  set_edges.erase(e->p);
  set_edges.erase(e->n);
  set_edges.erase(e->t->p);
  set_edges.erase(e->t->n);

  
  hedge *t=e->t;
  vertex *es=e->t->p->s;
  vertex *ets=e->p->s;
  if (e->s->e==e) e->s->e=e->t->n;
  if (e->t->s->e==e->t) e->t->s->e=e->n;
  e->p->n=t->n;
  e->n->p=t->p;
  e->t->n->p=e->p;
  e->t->p->n=e->n;
  e->p->p=e;
  e->n->n=e->t;
  e->t->p->p=e->t;
  e->t->n->n=e;
  hedge *tmp=e->n;
  e->n=e->p;
  e->p=e->t->n;
  e->t->n=e->t->p;
  e->t->p=tmp;
  e->s=es;
  e->t->s=ets;
  
  if (!legal(e->n)) set_edges.insert(e->n);
  if (!legal(e->p)) set_edges.insert(e->p);
  if (!legal(e->t->n)) set_edges.insert(e->t->n);
  if (!legal(e->t->p)) set_edges.insert(e->t->p);
}


void links(std::vector<hedge*> &contour) // here we suppose minimal info stored : loops (either clockwise or ccw), only start point and next hedge are set, no twins.
{
  for (int i=0;i<contour.size();++i)
  {
    hedge* e=contour[i];
    e->corr=0x0;
    e->s->e=e;
    e->n->p=e;
    e->t=0x0;
  }
}

void gethedges(vertex* v, std::deque<hedge*>& hv) // get all hedges leaving vertex, ordered counterclockwise. Prerequisite is that hedge pointed by v is a leaving one..
{
  hv.clear();
  hedge *e=v->e;
  if (e)
  {
    if (e->s->e!=e) throw;
    if (e->s!=v) throw;
    hedge *ini=e,*last;
    while (e)
    {
      hv.push_back(e);
      last=e;
      e=e->t;
      if (e)
        e=e->n;
      if (e==ini) break;
    }
    if (e!=ini) // go the other way round because twins do not always exist
    {
      e=ini->p;
      while (e && e!=last)
      {
        e=e->t;
        if (e)
        {
          hv.push_front(e);
          e=e->p;
        }
      }
    }
  }
}

int findpos(vertex* v, std::deque<hedge*>& hv) // find where v is with respect to the list of hedges originating from the same vertex
{
  for (int i=0;i<hv.size();++i)
  {
    npoint2 dep=hv[i]->s->pt;
    npoint2 arr1=hv[i]->n->s->pt;
    npoint2 arr2=hv[i]->p->s->pt;
    npoint2 arr=v->pt;
    npoint2 v1=arr1-dep;
    npoint2 v2=arr2-dep;
    npoint2 vv=arr-dep;
    double angle1=angle(v1,v2);if (angle1<0) angle1+=2*__npi; //0 to 2pi
    double anglev=angle(v1,vv);if (anglev<0) anglev+=2*__npi; //0 to 2pi
    if (anglev<angle1)
      return i;
  }
  return -1;
}

void split(std::vector<hedge*> &contour,std::vector<vertex*> &diagonals) // make sure the diagonals belong to the global half-edge data structure...
{
  if (diagonals.size()%2) throw; // should be multiple of 2
  for(int i=0;i<diagonals.size();i+=2)
  {
    std::deque<hedge*> a1,a2;
    gethedges(diagonals[i],a1);
    gethedges(diagonals[i+1],a2);
    int p1=findpos(diagonals[i+1],a1);
    int p2=findpos(diagonals[i],a2);
    hedge *l1=a1[p1];
    hedge *l2=a2[p2];
    hedge *h1=new hedge(diagonals[i]);
    hedge *h2=new hedge(diagonals[i+1]);
    h1->p=l1->p;
    h1->n=l2;
    h1->t=h2;
    l1->p->n=h1;
    l1->p=h2;
    h2->p=l2->p;
    h2->n=l1;
    h2->t=h1;
    l2->p->n=h2;
    l2->p=h1;
    contour.push_back(h1);
    contour.push_back(h2);
  }
}

void print( std::vector<std::pair<vertex*,int> > &stack)
{
  std::cout << "stack : " << stack.size() << std::endl;
  for (int i=stack.size()-1;i>=0;--i)
  {
    std::cout << stack[i].first->pt[0] << "," << stack[i].first->pt[1];
    if (stack[i].second == 0) std::cout << " ext" << std::endl;
    if (stack[i].second == -1) std::cout << " bottom" << std::endl;
    if (stack[i].second == 1) std::cout << " top" << std::endl;
  }
}


void triangulate(std::vector<hedge*> &contour,std::vector< vertex* >& tris,bool split_contour)
{
/*  for (int i=0;i<E.size();++i)
  {
    line l(E[i]->s->pt,E[i]->n->s->pt);
    
    data.add_line(l);
    point p(E[i]->s->pt);
    char mess[80];
    sprintf(mess,"%f,%f",p.pts[0],p.pts[1]);
    p.info=mess;
    data.add_point(p);
    color cc=color(0,255,255);
    std::pair<point,color> tmp(p,cc);
//    data.add_text(0,tmp);
  }
*/
//  datap->setcolorlines(color(255,0,255));
 std::vector<vertex*> diags;
  monotone(contour,diags);
/*  for (int i=0;i<diags.size();i+=2)
  {
    line l(diags[i]->pt,diags[i+1]->pt);
    datap->add_line(l);
  }
*/
  std::vector<hedge*> loops;
  find_all_loops(contour,loops);
  std::cout << "monotone loops : "  << loops.size() << std::endl;
  for (int i=0;i<loops.size();++i)
  {
    diags.clear();
    triangulate_loop(loops[i],tris,diags);
    if (split_contour) split(contour,diags);
  }
}


void triangulate_loop(hedge *start, std::vector<vertex*> &tris,std::vector<vertex*> &diagonals)
{
  std::vector<vertex *> top,bottom;
  std::vector<std::pair<vertex*,int> > stack;
  hedge *e=start;
  vertex *v=e->s;
  e->corr=v;
  // find extreme
  hedge *min=e;
  hedge *max=e;
  int n=1;
  e=e->n;
  while (e!=start)
  {
    vertex *v=e->s;
    e->corr=v;
    if ((*v)<*(min->s)) min=e;
    if (*(max->s)<(*v)) max=e;
    e=e->n;
    ++n;
  }
  stack.reserve(n);
  top.reserve(n);
  bottom.reserve(n);
  e=min;
  e=e->n;
  while (e!=max)
  {
    vertex *v=e->s;
    bottom.push_back(v);
    e=e->n;
  }
  e=e->n;
  while (e!=min)
  {
    vertex *v=e->s;
    top.push_back(v);
    e=e->n;
  }
  std::vector<std::pair<vertex*,int> > list;list.reserve(n);
  std::vector<vertex *>::iterator itbot=bottom.begin();
  std::vector<vertex *>::reverse_iterator ittop=top.rbegin();
  list.push_back(std::pair<vertex*,int>(min->s,0));
  while (ittop!=top.rend()||itbot!=bottom.end())
  {
    if (ittop==top.rend())
    {
      list.push_back(std::pair<vertex*,int>(*itbot,-1));
      itbot++;
    }
    else if (itbot==bottom.end())
    {
      list.push_back(std::pair<vertex*,int>(*ittop,+1));
      ittop++;
    }
    else if ((**itbot)<(**ittop))
    {
      list.push_back(std::pair<vertex*,int>(*itbot,-1));
      itbot++;
    }
    else
    {
      list.push_back(std::pair<vertex*,int>(*ittop,+1));
      ittop++;
    }
  }
  list.push_back(std::pair<vertex*,int>(max->s,0));
  stack.push_back(list[0]);
  stack.push_back(list[1]);
  for (int j=2;j<=list.size()-2;++j)
  {
    if (stack.rbegin()->second!=list[j].second)
    {
      for (int i=stack.size()-1;i>0;--i)
      {
        tris.push_back(list[j].first);
        tris.push_back(stack[i].first);
        diagonals.push_back(list[j].first);
        diagonals.push_back(stack[i].first);
        tris.push_back(stack[i-1].first);
      }
      stack.clear();
      stack.push_back(list[j-1]);
      stack.push_back(list[j]);
    }
    else
    {
      npoint2 v1=stack[stack.size()-1].first->pt-list[j].first->pt;
      for (int i=stack.size()-1;i>0;--i)
      {
        npoint2 v2=stack[i-1].first->pt-list[j].first->pt;
        double Angle=angle(v1,v2);
        if (Angle*list[j].second>0)
        {
          v1=v2;
          tris.push_back(list[j].first);
          tris.push_back(stack[i].first);
          tris.push_back(stack[i-1].first);
          diagonals.push_back(list[j].first);
          diagonals.push_back(stack[i-1].first);
          stack.pop_back();
        }  else break;
      }
      stack.push_back(list[j]);
    }
  }
  for (int i=stack.size()-1;i>0;--i)
  {
    tris.push_back(list[list.size()-1].first);
    tris.push_back(stack[i].first);
    tris.push_back(stack[i-1].first);
    if (i>1)
    {
      diagonals.push_back(list[list.size()-1].first);
      diagonals.push_back(stack[i-1].first);
    }
  }
  stack.clear();
}

void print(const status_type &status)
{
  std::cout << "size : " << status.size() << std::endl;
  for (status_type::iterator it=status.begin();it!=status.end();++it)
  {
    std::cout << (*it)->s->pt[0] << "," << (*it)->s->pt[1] << " to "<< (*it)->n->s->pt[0] << "," << (*it)->n->s->pt[1] << " corr: " << (*it)->corr->pt[0] << "," << (*it)->corr->pt[1] << std::endl;
  }
}

data_container *datap=0x0;

void printg(const status_type &status)
{
//  std::cout << "size : " << status.size() << std::endl;
  int i=0;
  for (status_type::iterator it=status.begin();it!=status.end();++it,++i)
  {
    datap->add_line(line((*it)->s->pt,(*it)->n->s->pt));
    (*it)->corr->pt;
    point p(((*it)->s->pt+(*it)->n->s->pt)/2.0);
    char mess[80];
    sprintf(mess,"%d",i);
    p.info=mess;
    color cc=color(0,255,255);
    std::pair<point,color> tmp(p,cc);
    datap->add_text(0,tmp);
    p=point((*it)->corr->pt);
    p.info=mess;
    std::pair<point,color> tmp2(p,cc);
    datap->add_text(0,tmp2);
  }
}

void clear_tag(std::vector<hedge*> &contour)
{
  for (int i=0;i<contour.size();++i)
    contour[i]->corr=0x0;
}

void tag_loop(hedge* start)
{
  hedge *e=start;
  vertex *v=e->s;
  e->corr=v;
  e=e->n;
  while (e!=start)
  {
    vertex *v=e->s;
    e->corr=v;
    e=e->n;
  }
}

int find_next_loop(std::vector<hedge*> &contour,int start)
{
  for (int i=start;i<contour.size();++i)
    if (!contour[i]->corr) return i;
  return -1;
}

void find_all_loops(std::vector<hedge*> &contour,std::vector<hedge*> &loops)
{
  int i=0;
  clear_tag(contour);
  if (contour.size()>0 && contour[0])
  {
    while (i>=0)
    {
      tag_loop(contour[i]);
      loops.push_back(contour[i]);
      i=find_next_loop(contour,i);
    }
  }
}

void monotone(std::vector<hedge*> &contour,std::vector<vertex*> &diagonals)
{
  evt_queue_type events;
  diagonals.clear();
// 0- build connectivities
  links(contour);
// 1- classify the events;
  for (int i=0;i<contour.size();++i)
  {
    hedge* e=contour[i];
    e->corr=0x0; // not correpondent yet
    e->s->type=nil;
    double Angle=angle(e->s);
    if (*(e->s)<*(e->p->s) && (*(e->s)<*(e->n->s)))
    {
      if (Angle<=0)
        e->s->type=start;
      else
        e->s->type=separate;
    } else
    if ((*(e->p->s)<*(e->s)) && (*(e->n->s)<*(e->s)))
    {
      if (Angle<=0)
        e->s->type=end;
      else
        e->s->type=fuse;
    }
    else
    {
      if (*(e->p->s)<*(e->n->s)) // interior above 
        e->s->type=regular_A;
      else
        e->s->type=regular_B;
    }
    events.insert(*(e->s)); // only take starting point (all edges are connected!)
  }
  int i=0;
  #ifdef DEBUG 
  for (evt_queue_type::iterator it=events.begin();it!=events.end();++it,++i)
  {
    std::cout << i ;
    switch(it->type)
    {
      case start:
      {
        std::cout << " start " << it->pt[0] << "," << it->pt[1] << std::endl;
      } break;

      case end: 
      {
        std::cout << " end   " << it->pt[0] << "," << it->pt[1] << std::endl;
      } break;

      case separate:
      {
        std::cout << " separ " << it->pt[0] << "," << it->pt[1] << std::endl;
      } break;

      case fuse: 
      {
        std::cout << " fuse  " << it->pt[0] << "," << it->pt[1] << std::endl;
      } break;

      case regular_A : // polygon above event
      {
        std::cout << " reg_A  " << it->pt[0] << "," << it->pt[1] << std::endl;
      } break;

      case regular_B :  //polygon below event
      {
        std::cout << " reg_B  " << it->pt[0] << "," << it->pt[1] << std::endl;
      } break;
      default :
        throw;
    }
  }
#endif
  i=0;
  status_type status;
// 2- loop over events and process them
  for (evt_queue_type::iterator it=events.begin();it!=events.end();++it,++i)
  {
#ifdef DEBUG
    std::cout << std::endl << "---- " <<  i ;
#endif
    status_type::iterator its;
    switch(it->type)
    {
      case start:
      {
        its=status.insert(it->e).first;
        it->e->sptr=its;
        it->e->corr=it->e->s;
#ifdef DEBUG
        std::cout << " start " << it->pt[0] << "," << it->pt[1] << std::endl;
        print(status); 
#endif
      } break;

      case end: 
      {
        if (it->e->p->corr->type==fuse)
        { 
          diagonals.push_back(it->e->s);
          diagonals.push_back(it->e->p->corr);
        }
        status.erase(it->e->p->sptr);
#ifdef DEBUG
        std::cout << " end   " << it->pt[0] << "," << it->pt[1] << std::endl;
        print(status);
#endif
      } break;

      case separate:
      {

        vertex v=*it;
        hedge ee(&v);
        its = status.lower_bound(&ee);
        if (its != status.begin()) its--; else { throw;};
        diagonals.push_back(it->e->s);
        diagonals.push_back((*its)->corr);
        (*its)->corr=it->e->s;
        it->e->corr=it->e->s;
        its=status.insert(it->e).first;
        it->e->sptr=its;
#ifdef DEBUG
        std::cout << " separ " << it->pt[0] << "," << it->pt[1] << std::endl;
        print(status);
#endif
      } break;

      case fuse: 
      {
        if (it->e->p->corr->type==fuse)
        {
          diagonals.push_back(it->e->s);
          diagonals.push_back(it->e->p->corr);
        }
        status.erase(it->e->p->sptr);
        vertex v=*it;
        hedge ee(&v);
        its = status.lower_bound(&ee);
        if (its != status.begin()) its--; else { throw;};
        if ((*its)->corr->type==fuse)
        {
          diagonals.push_back(it->e->s);
          diagonals.push_back((*its)->corr);
        }
        (*its)->corr=it->e->s;
#ifdef DEBUG
        std::cout << " fuse  " << it->pt[0] << "," << it->pt[1] << std::endl;
        print(status);
#endif
      } break;

      case regular_A : // polygon above event
      {

        if (it->e->p->corr->type==fuse)
        {
          diagonals.push_back(it->e->s);
          diagonals.push_back(it->e->p->corr);
        }
        status.erase(it->e->p->sptr);
        its=status.insert(it->e).first;
        it->e->sptr=its;
        it->e->corr=it->e->s;
#ifdef DEBUG
        std::cout << " reg_A  " << it->pt[0] << "," << it->pt[1] << std::endl;
        print(status);
#endif
      } break;

      case regular_B :  //polygon below event
      {
        vertex v=*it;
        hedge e(&v);
        its = status.lower_bound(&e);
        if (its != status.begin()) its--; else { throw;};
        if ((*its)->corr->type==fuse)
        {
          diagonals.push_back(it->e->s);
          diagonals.push_back((*its)->corr);
        }
        (*its)->corr=it->e->s;
#ifdef DEBUG
        std::cout << " reg_B  " << it->pt[0] << "," << it->pt[1] << std::endl;
        print(status);
#endif
      } break;
      default :
        throw;
    }
  }
  split(contour,diagonals);  
}