// Moshade - Compute average cross section for 3D shapes
// Copyright (C) 2018-2026 Eric Bechet - bechet@cadxfem.org
//
// See the LICENSE file for license information and contributions.
// Please report all bugs and problems to <bechet@cadxfem.org>.

#include "geom_op.h"
#include "pia.h"
#include "ndisplay.h"
#include <cassert>
#include <sstream>
#include <algorithm>

#define DELTA_ (1e-6)


tri_bb::tri_bb(const triangle &t) : triangle(t)
{
  max=min=pts[0];
  for (int i=1;i<3;++i)
    for (int j=0;j<3;++j)
    {
      if (max[j]<pts[i][j]) max[j]=pts[i][j];
      if (min[j]>pts[i][j]) min[j]=pts[i][j];
    }
  for (int j=0;j<3;++j)
  {
    double dd=(max[j]-min[j])*DELTA_*100;
    if (dd==0.0) dd=DELTA_;
    max[j]+=dd;
    min[j]-=dd;
  }
}

bool tri_bb::in_bb(const npoint3 &pt) const
{
  for (int j=0;j<3;++j)
  {
    if (pt[j]>max[j]) return false;
    if (pt[j]<min[j]) return false;
  }
  return true;
}

bool tri_bb::in_bb(const tri_bb &tri) const
{
  for (int j=0;j<3;++j)
  {
    if (tri.min[j]>max[j]) return false;
    if (tri.max[j]<min[j]) return false;
  }
  return true;
}

bool tri_bb::in(const npoint3 &pt,bool boundary_out) const
{
  if (in_bb(pt)) 
    return in_tri(pt,*this,boundary_out);
  else return false;
}

double tri_bb::area2(void) const
{
  return tri_area(*this);
}

double tri_bb::area(void) const
{
  double a,b,c;
  a=(pts[1]-pts[0]).norm();
  b=(pts[2]-pts[1]).norm();
  c=(pts[0]-pts[2]).norm();
  if (a < c) std::swap(a, c);
  if (a < b) std::swap(a, b);
  if (b < c) std::swap(b, c);
  return sqrt((a+(b+c))*(c-(a-b))*(c+(a-b))*(a+(b-c)))/4;
}

double tri_bb::circ(void) const
{
  double l=0.0;
  for (int i=0;i<3;++i)
  {
    double ll=0.;
    for (int j=0;j<3;++j)
    {
      double dd=(pts[i][j]-pts[(i+1)%3][j]);
      ll+=dd*dd;
    }
    l+=sqrt(ll);
  }
  return l;
}


npoint3 tri_bb::G(void) const
{
  return (pts[0]+pts[1]+pts[2])/3;
}

npoint3 tri_bb::normal(void) const
{
  npoint3 vec1=pts[1]-pts[0];
  npoint3 vec2=pts[2]-pts[1];
  npoint3 prod;
  prod.crossprod(vec1,vec2);
  double norm=prod.norm();
  if (norm>0.0) return prod/norm; else return prod;
}

void tri_bb::cut(const line& l,std::vector< tri_bb >& ret) const
{
  tri_cut_line(l,*this,ret);
}

bool tri_bb::cut(const tri_bb& tri,std::vector< tri_bb> &outside,bool disp) const
{
  if (in_bb(tri))
  {
    return tri_cut(tri, *this, outside,disp);
  }
 else
  {
    outside.push_back(*this);
    return true;
  }
}

double sign(npoint3 p1, npoint3 p2, npoint3 p3)
{
  return (p1.x() - p3.x()) * (p2.y() - p3.y()) - (p2.x() - p3.x()) * (p1.y() - p3.y());
}

bool in_tri(const npoint3 &pt, const tri_bb& tri, bool boundary_out)
{
  bool b[3];
  double s[3],sum=0.0,area;
  area=sign(tri.pts[0],tri.pts[1],tri.pts[2]);
  if (boundary_out)
  {
    double lmax=0.0;
    for (int i=0;i<3;++i)
    {
      double ll=0.;
      for (int j=0;j<3;++j)
      {
        double dd=(tri.pts[i][j]-tri.pts[(i+1)%3][j]);
        ll+=dd*dd;
      }
      if (lmax<ll) lmax=ll;
    }
    if (fabs(area)/lmax<(DELTA_*DELTA_)) return false;
  }
  for (int i=0;i<3;++i) s[i]=sign(pt,tri.pts[i],tri.pts[(i+1)%3]);
  for (int i=0;i<3;++i) sum+=s[i];
  if ((fabs(sum)/fabs(area))<DELTA_) return false;
  if (boundary_out)
  {
    if (sum>0.) for (int i=0;i<3;++i) b[i]=((s[i]/fabs(sum))<DELTA_);
    else for (int i=0;i<3;++i) b[i]=((s[i]/fabs(sum))<-DELTA_);
  }
  else
  {
    if (sum>0.) for (int i=0;i<3;++i) b[i]=((s[i]/fabs(sum))<-DELTA_);
    else for (int i=0;i<3;++i) b[i]=((s[i]/fabs(sum))<DELTA_);
  }
  return ((b[0] == b[1]) && (b[1] == b[2]));
}

bool line_int(line l1, line l2,npoint3 &pt,npoint3 &uv)
{
  npoint3 &A=l1.pts[0];
  npoint3 &B=l1.pts[1];
  npoint3 &C=l2.pts[0];
  npoint3 &D=l2.pts[1];
  npoint3 L1=B-A;
  npoint3 L2=D-C;
  double d1=L1.normalize();
  double d2=L2.normalize();
  double pscal=L1.dotprod(L2);
  npoint3 L3;
  L3.crossprod(L1,L2);
  double d3=L3.normalize();
  if (fabs(d3)<DELTA_*10) return false; // "too" tangent
  
  if (
    fabs(A[0]-B[0])<fabs((A[0])+fabs(B[0]))*DELTA_ &&
    fabs(A[1]-B[1])<fabs((A[1])+fabs(B[1]))*DELTA_
      ||
    fabs(C[0]-D[0])<(fabs(C[0])+fabs(D[0]))*DELTA_ &&
    fabs(C[1]-D[1])<(fabs(C[1])+fabs(D[1]))*DELTA_) return false; // zero-length segment


    if ((
    fabs(A[0]-C[0])<fabs((A[0])+fabs(C[0]))*DELTA_ &&
    fabs(A[1]-C[1])<fabs((A[1])+fabs(C[1]))*DELTA_
      ||
    fabs(A[0]-D[0])<(fabs(A[0])+fabs(D[0]))*DELTA_ &&
    fabs(A[1]-D[1])<(fabs(A[1])+fabs(D[1]))*DELTA_)
      &&
    (fabs(B[0]-C[0])<fabs((B[0])+fabs(C[0]))*DELTA_ &&
    fabs(B[1]-C[1])<fabs((B[1])+fabs(C[1]))*DELTA_
      ||
    fabs(B[0]-D[0])<(fabs(B[0])+fabs(D[0]))*DELTA_ &&
    fabs(B[1]-D[1])<(fabs(B[1])+fabs(D[1]))*DELTA_)) return false; // same segments
  
  double denu=(D[1]-C[1])*(B[0]-A[0])-(D[0]-C[0])*(B[1]-A[1]);
  double nu=(D[0]-C[0])*(A[1]-C[1])-(D[1]-C[1])*(A[0]-C[0]);
  
  if ((denu==0.0)) return false; // tangent (but treated before)
  double u=nu/denu;
  double denv=(B[1]-A[1])*(D[0]-C[0])-(B[0]-A[0])*(D[1]-C[1]);
  double nv=(B[0]-A[0])*(C[1]-A[1])-(B[1]-A[1])*(C[0]-A[0]);
  if ((denv==0.0)) return false; // tangent (but treated before)
  double v=nv/denv;
  uv[0]=u;
  uv[1]=v;
/*
  bool outu=false;
  bool outv=false;
  bool out=false;

  if (u<DELTA_) outu=true;
  if (u<-DELTA_) out=true;
  if (u>(1-DELTA_)) outu=true;
  if (u>(1+DELTA_)) out=true;
  if (v<DELTA_) outv=true;
  if (v<-DELTA_) out=true;
  if (v>(1-DELTA_)) outv=true;
  if (v>(1+DELTA_)) out=true;*/
//  if (outu && outv) return false;
//  if (out) return false;
  if ((fabs(nu)*fabs(denu))>(fabs(nv)*fabs(denv)))
  {
    pt[0]=A[0]+(B[0]-A[0])*u;
    pt[1]=A[1]+(B[1]-A[1])*u;
    pt[2]=0.;
  }
  else
  {
    pt[0]=C[0]+(D[0]-C[0])*v;
    pt[1]=C[1]+(D[1]-C[1])*v;
    pt[2]=0.;
  }
  //  Success.
  return true;
}



void tri_cut_line(const line& l1, const tri_bb& trc, std::vector<tri_bb> &ret)
{
  //find int points
  double umin,umax;
  double vumin,vumax;
  bool notfound=true;
  npoint3 ptumin,ptumax;
  int imin=-1,imax=-1;
  double ll=0.;
  for (int i=0;i<3;++i)
  {
    line l2(trc.pts[i],trc.pts[(i+1)%3]);
    npoint3 vec=l2.pts[1]-l2.pts[0];
    ll+=vec.norm();
    npoint3 pt;
    npoint3 uv;
    if (line_int(l1,l2,pt,uv))
    {
      if ((uv[1]>=-DELTA_) && (uv[1]<=1+DELTA_)) // inside the segment of the triangle
      {
        if (notfound) {umin=umax=uv[0]; vumin=vumax=uv[1]; ptumin=pt;ptumax=pt;notfound=false;imin=i;imax=i;}
        else
        {
          if (uv[0]<umin) { umin=uv[0];vumin=uv[1];ptumin=pt;imin=i;}
          if (uv[0]>umax) { umax=uv[0];vumax=uv[1];ptumax=pt;imax=i;}
        }
      }
    }
  }

  // zero intersections
  if (notfound) {ret.push_back(trc);return;}
  // one intersection (e.g. on a vertex)
  if (fabs(umax-umin) < DELTA_) {ret.push_back(trc);return;}
    
  // two distinct intersections
  // check if the interval is actually inside the definition
  if ((umax<-10*DELTA_) || (umin>1+10*DELTA_)) {ret.push_back(trc);return;}
  if ((umax>1+10*DELTA_) || (umin<-10*DELTA_)) {ret.push_back(trc);return;}

  // check if at least one value of the parameter is frankly OK (inside the triangle !)
  if (((vumax<10*DELTA_) || (vumax>1-10*DELTA_)) && ((vumin<10*DELTA_) || (vumin>1-10*DELTA_))) {ret.push_back(trc);return;}
  // check if some are on a vertex  
  bool minispt=false,maxispt=false;
  for (int i=0;i<3;++i)
  {
    npoint3 pt=ptumin-trc.pts[i];
    double l=pt.norm();
    if (l/ll < 10*DELTA_) {minispt=true;}
    pt=ptumax-trc.pts[i];
    l=pt.norm();
    if (l/ll < 10*DELTA_) {maxispt=true;}
  }
  // both on nodes
  if (maxispt&&minispt) {ret.push_back(trc);return;}
  // one intersection inside edge
  if (maxispt||minispt)
  {
    int ii=maxispt?imin:imax;
    npoint3 pt=maxispt?ptumin:ptumax;
    triangle tr1(pt,trc.pts[(ii+1)%3],trc.pts[(ii+2)%3]);
    triangle tr2(pt,trc.pts[(ii+2)%3],trc.pts[ii]);
    ret.push_back(tr1);
    ret.push_back(tr2);
  }
  else // both intersections inside edges
  {
    int ii;
    npoint3 dep= ptumin;
    npoint3 arr= ptumax;
    if ((imin+imax)==1)
    {
      ii=1;
      if (imin<imax)
      { 
        dep=ptumax;
        arr=ptumin;
      }
    }
    if ((imin+imax)==3)
    {
      ii=2;
      if (imin<imax) 
      {
        dep=ptumax;
        arr=ptumin;
      }
    }
    if ((imin+imax)==2)
    {
      ii=0;
      if (imin>imax)
      {
        dep=ptumax;
        arr=ptumin;
      }
    }
    triangle tr1(trc.pts[ii],dep,arr);
    triangle tr2(dep,trc.pts[(ii+1)%3],trc.pts[(ii+2)%3]);
    triangle tr3(dep,trc.pts[(ii+2)%3],arr);
    ret.push_back(tr1);
    ret.push_back(tr2);
    ret.push_back(tr3);
  }
}


/*

bool tri_cut(const tri_bb& tri, const tri_bb& trc, std::vector<tri_bb> &outside)
{
  // check which points of tri are in trc
  std::vector<tri_bb> newtr;
  bool nonew=false;
  std::pair<point,color> x;
  std::stringstream ss; 
  newtr.push_back(trc);
  data_container *data;
  ndisplay *d;
 
//  insert nodes
  for (int i=0;i<3;++i)
  {

    int sz=newtr.size();// no need to look for new triangles
    for (int j=0;j<sz;++j)
    {
//      if (in_tri(tri.pts[i],newtr[j],true))
      if (newtr[j].in(tri.pts[i],true))
      {
        triangle newtris[3];
        for (int k=0;k<3;++k)
        {
          newtris[k]=triangle(tri.pts[i],newtr[j].pts[k],newtr[j].pts[(k+1)%3]);
        }
        newtr[j]=newtris[0];
        newtr.push_back(newtris[1]);
        newtr.push_back(newtris[2]);
        break;
      }
    }
  }
  for (int i=0;i<3;++i)
  {
    line l1(tri.pts[i],tri.pts[(i+1)%3]);
    std::vector<tri_bb> newtr2;
    for (int j=0;j<newtr.size();++j)
    {
      std::vector<tri_bb> newtr_;
      tri_cut_line(l1,newtr[j],newtr_);
      for (int l=0;l<newtr_.size();++l) newtr2.push_back(newtr_[l]);
    }
    newtr=newtr2;
  }
  
  for (int i=0;i<newtr.size();++i)
  {
    int szn=newtr.size();
    npoint3 bary(0.,0.,0.);
    for (int j=0;j<3;++j)
      for (int k=0;k<3;++k)
        bary[k]+=newtr[i].pts[j][k]/3.0;
    if (!tri.in(bary,false))
       outside.push_back(newtr[i]);
  }
   if (newtr.size()==1&&outside.size()==1) nonew=true;
  return nonew;
}

*/



bool tri_cut(const tri_bb& tri, const tri_bb& trc, std::vector<tri_bb> &outside,bool disp)
{
  // check which points of tri are in trc
  std::vector<tri_bb> newtr;
  bool nonew=false;
  std::pair<point,color> x;
  std::stringstream ss; 
  newtr.push_back(trc);
  data_container *data;
  ndisplay *d;
  if (disp)
  {
    data=new data_container;
    d=new ndisplay;
  }

//  insert nodes
  for (int i=0;i<3;++i)
  {
    if (disp)
    {
      x=std::make_pair(tri.pts[i],color(0,255,255));
      ss << i ;
      x.first.info=ss.str();
      data->add_text(1,x);
      data->add_point(tri.pts[i]);
      ss.str("");
      x=std::make_pair(trc.pts[i],color(0,255,255));
      ss << i ;
      x.first.info=ss.str();
      data->add_text(1,x);
      data->add_point(trc.pts[i]);
      ss.str("");
    }
    int sz=newtr.size();// no need to look for new triangles
    for (int j=0;j<sz;++j)
    {
      if (newtr[j].in(tri.pts[i],true))
      {
        tri_bb newtris[3];
        int n=0;
        for (int k=0;k<3;++k)
        {
          newtris[n]=triangle(tri.pts[i],newtr[j].pts[k],newtr[j].pts[(k+1)%3]);
          double c=newtris[n].circ();
          if (newtris[n].area2()>(DELTA_*c*c/10000.))
            n++;
        }
        if (n)
          newtr[j]=newtris[0];
        else
        {
          std::vector<tri_bb>::iterator ik = newtr.begin() + j;
          newtr.erase(ik);j--;
        }
        if (n>1) newtr.push_back(newtris[1]);
        if (n>2) newtr.push_back(newtris[2]);
        break;
      }
    }
  }
  for (int i=0;i<3;++i)
  {
    line l1(tri.pts[i],tri.pts[(i+1)%3]);
    std::vector<tri_bb> newtr2;
    for (int j=0;j<newtr.size();++j)
    {
      std::vector<tri_bb> newtr_;
      tri_cut_line(l1,newtr[j],newtr_);
      for (int l=0;l<newtr_.size();++l)
      {
        double c=newtr_[l].circ();
        if (newtr_[l].area2()>(DELTA_*c*c/10000.))
          newtr2.push_back(newtr_[l]);
      }
    }
    newtr=newtr2;
  }
  
  if (disp)
  {
      properties p=data->getproptriangles();
      p.c=color(255,255,255);
      p.c.A=50;
      p.edgeon=true;
      p.edgecolor=color(0,0,0);
      data->setproptriangles(p);
      data->add_triangle(tri);
  }
  
  for (int i=0;i<newtr.size();++i)
  {
    npoint3 bary(0.,0.,0.);
    for (int j=0;j<3;++j)
      for (int k=0;k<3;++k)
        bary[k]+=newtr[i].pts[j][k]/3.0;
    if (!tri.in(bary,false))
    {
      outside.push_back(newtr[i]);
      if (disp)
      {
        ss << i ;
        x.first.info=ss.str();ss.str("");
        data->add_text(1,x);
        data->add_point(bary);
        data->setcolortriangles(color(255,0,255));
        data->add_triangle(newtr[i]);
      }
    }
    else   
    {
      if (disp)
      {
        x.first.info=ss.str();ss.str("");
        data->add_text(1,x);
        data->add_point(bary);
        data->setcolortriangles(color(0,255,255));
        data->add_triangle(newtr[i]);
      }
    }
  }
  if (disp)
  {
    d->init_data(*data);
    d->display();
    delete d;
    delete data;
  }
  if (newtr.size()==1&&outside.size()==1) nonew=true;
  return nonew;
}

#undef DELTA_


double tri_area(const triangle& tri)
{
  point_t tr[3];
  for (int i=0;i<3;++i)
  {
    tr[i].x=tri.pts[i].x();
    tr[i].y=tri.pts[i].y();

  }
  float b=pia_area(tr, 3, tr,3);
  return b;
}


double tri_nc_area(const triangle& tri1, const triangle& tri2)
{
  point_t tr1[3];
  point_t tr2[3];
  for (int i=0;i<3;++i)
  {
    tr1[i].x=tri1.pts[i].x();
    tr2[i].x=tri2.pts[i].x();
    tr1[i].y=tri1.pts[i].y();
    tr2[i].y=tri2.pts[i].y();
  }
  float a=pia_area(tr1, 3, tr2,3);
  float b=pia_area(tr2, 3, tr2,3);
  float c=fabs(b)-fabs(a);
  assert(c>=0.0f);
  return c;
}

//double common_area(triangle tri,triangle tro);


void npt_transfo(const double mat[4][4],const npoint &src, npoint &dest)
{
  for (int i=0;i<4;++i)
  {
    dest[i]=0.0;
    for (int j=0;j<4;++j)
      dest[i]+=mat[i][j]*src[j];
  }
}



void fill_rot_mat(int axis,double angle,double mat[4][4])
{
  for (int i=0;i<4;++i)
    for (int j=0;j<4;++j)
      mat[i][j]=0;
  mat[3][3]=1.0;
  if (axis==0) //X
  {
    mat[1][1]=mat[2][2]=cos(angle);
    mat[1][2]=-sin(angle);
    mat[2][1]=sin(angle);
    mat[axis][axis]=1.0;
  }else 
  if (axis==1) //Y
  {
    mat[0][0]=mat[2][2]=cos(angle);
    mat[0][2]=sin(angle);
    mat[2][0]=-sin(angle);
    mat[axis][axis]=1.0;
  }else 
  if (axis==2) //Z
  {
    mat[0][0]=mat[1][1]=cos(angle);
    mat[0][1]=-sin(angle);
    mat[1][0]=sin(angle);
    mat[axis][axis]=1.0;
  }
  else 
    for (int j=0;j<3;++j)
      mat[j][j]=1.0; // identity
}

void fill_tr_mat(int axis,double value,double mat[4][4])
{
  for (int i=0;i<4;++i)
    for (int j=0;j<4;++j)
      mat[i][j]=(i==j?1.0:0.0);
    mat[axis][3]=value;
}

void mat_prod(const double m1[4][4],const double m2[4][4],double mo[4][4])
{
  for (int i=0;i<4;++i)
    for (int j=0;j<4;++j)
    {
      mo[i][j]=0.0;
      for (int k=0;k<4;++k)
        mo[i][j]+=m1[i][k]*m2[k][j];
    }
}

void tri_transfo(const triangle &tri,const double mat[4][4],triangle &tro)
{
  for (int i=0;i<3;++i)
  {
    npoint pt(tri.pts[i]);
    npoint pt2;
    npt_transfo(mat,pt,pt2);
    tro.pts[i]=npoint3(pt2);
  }
}

void tri_project(const triangle& tri, int axis, triangle& tro)
{
  for (int i=0;i<3;++i)
  {
    tro.pts[i]=tri.pts[i];
    tro.pts[i][axis]=0.0;
  }
}

bool compare_area(const triangle &i,const triangle &j) 
{
  return(fabs(tri_area(i))>fabs(tri_area(j)));
}

void sort_area(std::vector<triangle> &source)
{
  std::sort(source.begin(),source.end(),compare_area);
}