#include "mesh_op.h"
#include "stl_io.h"
#include "gm_io.h"
#include <cassert>
#include <list>
#include <map>
#include <fstream>
#include <cstring>
#include <cctype>

bool less_pts_size::operator()(const ptsize& pt1, const ptsize& pt2) const
{
  npoint3 d(pt1-pt2);
  double rad=((pt1.radius>pt2.radius?pt2.radius:pt1.radius));
  if (d.norm()>rad/1000)
  for (int i=0;i<3;++i)
  {
    double base=min[i];
    double extent=max[i]-min[i];

    double val1=((pt1[i]-base)/extent);//*(LLONG_MAX/524288)
    double val2=((pt2[i]-base)/extent);//*(LLONG_MAX/524288)
    if (val1<val2) return true;
    if (val1>val2) return false;

    /*    long long iv1=(long long) (val1);
    long long iv2=(long long) (val2);
    if (iv1<iv2) return true;
    if (iv1>iv2) return false;*/
  }
  return false;
}

bool less_pts::operator()(const npoint3& pt1, const npoint3& pt2) const
{
  npoint3 d(pt1-pt2);
  for (int i=0;i<3;++i)
  {
    if (d[i]<0) return true;
    if (d[i]>0) return false;
  }
  return false;
}


bool less_edg_uniq::operator()(const edg_uniq& edg1, const edg_uniq& edg2) const
{
  less_pts compare;
  npoint3 edg1_[2];
  npoint3 edg2_[2];
  double L=(max-min).norm();
  
  if (compare(edg1.pts[0],edg1.pts[1]))
  {
    edg1_[0]=edg1.pts[0];edg1_[1]=edg1.pts[1];
  }
  else
  {
    edg1_[0]=edg1.pts[1];edg1_[1]=edg1.pts[0];
  }
  if (compare(edg2.pts[0],edg2.pts[1]))
  {
    edg2_[0]=edg2.pts[0];edg2_[1]=edg2.pts[1];
  }
  else
  {
    edg2_[0]=edg2.pts[1];edg2_[1]=edg2.pts[0];
  }
  for (int i=0;i<3;++i)
  {
    if ((edg1_[0][i]-edg2_[0][i])/L<-tol) return true;
    if ((edg1_[0][i]-edg2_[0][i])/L>+tol) return false;
  }
  for (int i=0;i<3;++i)
  {
    if ((edg1_[1][i]-edg2_[1][i])/L<-tol) return true;
    if ((edg1_[1][i]-edg2_[1][i])/L>+tol) return false;
  }
  return false;
}


edg_uniq::edg_uniq(const npoint3& pt1, const npoint3& pt2)
{
  pts[0]=pt1;
  pts[1]=pt2;
  tri.reserve(4);
}

int tri_it::ori(const tri_it& other) const
{
  int e[3],o[3],n=0;
  for (int i=0;i<3;++i)
  {
    for (int j=0;j<3;++j)
    {
      if (pts[i]==other.pts[j]) {e[n]=i;o[n]=j;n++;}
    }
  }
  if (n<2) return 0;
  if (n==2)
  {
    int s1;
    if ((e[1]-e[0])==1) s1=1; else s1=-1;
    if ((o[1]-o[0])==-1) {return s1;}
    if ((o[1]-o[0])==1) {return -s1;}
    if ((o[1]-o[0])==-2) {return -s1;}
    if ((o[1]-o[0])==2) {return s1;}
  }
  if (n==3)
  {
    int s1;
    int s2;
    for (int j=0;j<2;++j)
    {
      if ((e[j]-e[j+1])==1) s1=1;
      if ((e[j]-e[j+1])==-1) s1=-1;
      if ((o[j]-o[j+1])==1) s2=1;
      if ((o[j]-o[j+1])==-1) s2=-1;
    }
    if (s1==s2) return 1;
    if (s1==-s2) return -1;
    return 0;
  }
  return 0;
}

tri_mesh::tri_mesh(std::string filename)
{
  load_stl(filename);
  tol=0.;
}

void tri_mesh::load_stl(std::string filename)
{
//  clear();
  load_stl_mesh(filename,*this);
  update_bb();
}

void tri_mesh::load_gm(std::string filename,bool override,double szsph,double szcyl, int ns)
{
//  clear();
  load_gm_mesh(filename,*this,override,szsph,szcyl,ns);
  update_bb();
}

void tri_mesh::update_bb(void)
{
  valid_bb=false;
  for (int i=0;i<mesh.size();++i)
  {
    update_bb(i);
  }
  compare_edge.tol=tol;
  compare_edge.min=min;
  compare_edge.max=max;
}

 // for element i;
void tri_mesh::update_bb(int i)
{
  for (int j=0;j<3;++j)
  {
    if (!valid_bb)
    {min=max=mesh[i].pts[j];valid_bb=true;}
    else
    {
      for (int k=0;k<3;++k)
      {
        if (mesh[i].pts[j][k]<min[k]) min[k]=mesh[i].pts[j][k];
        if (mesh[i].pts[j][k]>max[k]) max[k]=mesh[i].pts[j][k];
      }
    }
  }
  clear_topology();
  compare.min=min;
  compare.max=max;
}

void tri_mesh::bb(npoint3& bbmin,npoint3& bbmax) const
{
  bbmin=min;bbmax=max;
}


void tri_mesh::build_topology(bool normals,bool quiet)
{
  update_bb();
  find_unique_ent(tol,quiet);
  find_solids(normals,quiet);
  has_topo=true;
}

void tri_mesh::find_unique_ent(double tol,bool quiet)
{
  compare_edge.tol=tol;
  compare_edge.min=min;
  compare_edge.max=max;
  clear_topology();
  pts=new std::set<ptsize,less_pts_size>(compare);
  edsu=new std::set<edg_uniq,less_edg_uniq>(compare_edge);
  for (int i=0;i<mesh.size();++i)
  {
    tri_it tr;
    double circ=mesh[i].circ();
    for (int j=0;j<3;++j)
    {
      ptsize pt3(mesh[i].pts[j],circ/3);
      mesh[i].pts[j];
      std::pair<std::set<ptsize,less_pts_size>::iterator,bool> k=pts->insert(pt3);
      tr.pts[j]=k.first;
    }
    for (int j=0;j<3;++j)
    {
      edg_uniq ed(mesh[i].pts[j],mesh[i].pts[(j+1)%3]);
      ed.tri.push_back(i);
      ed.triorig=i;
      std::pair<std::set<edg_uniq,less_edg_uniq>::iterator,bool> k=edsu->insert(ed);
      if (k.second==false) // found the another triangle sharing edge
      {
        k.first->tri.push_back(i);
        k.first->triorig=i;
      }
      tr.edg[j]=k.first;
    }
    mesh_it.push_back(tr);
  }
  if (!quiet) std::cout << pts->size() << " unique points, " << mesh.size() << " triangles, " << edsu->size() << " unique edges." << std::endl ;
  int hole=0;
  int nonmanifold=0;
  for (std::set<edg_uniq,less_edg_uniq>::iterator ite=edsu->begin();ite!=edsu->end();++ite)
  {
    if (ite->tri.size()==1) hole++;
    if (ite->tri.size()>2) nonmanifold++;
  }
  if (!quiet) if (hole) std::cout << "At least one hole in shell(s) : " << hole << " one-sided edges." << std::endl;
  if (!quiet) if (nonmanifold) std::cout << "Non manifold surface(s) : " << nonmanifold << " nonmanifold edges. " << std::endl;
  if (!hole) 
  {
    //can proceed with backface culling
  }
  for (int i=0;i<mesh.size();++i) normal[i]=npoint3(0,0,0);
  for (std::set<edg_uniq,less_edg_uniq>::iterator ite=edsu->begin();ite!=edsu->end();++ite)
  {
    for (int i=0;i<ite->tri.size();++i)
    {
      if ((ite->tri[i]!=-1))
      {
        int t=ite->tri[i];
        bool found=false;
        for (int j=0;j<3;++j)
          if (mesh_it[t].edg[j]==ite) {found=true;break;}
        if (!quiet) if (!found) std::cout << "link error" ;
      }
    }
  }
}

void tri_mesh::find_solids(bool normals,bool quiet)
{
  std::set<int> unk;
  for (int i=0;i<mesh.size();++i) unk.insert(i);
  std::list<int> bnd;
  std::map<int,int> known;
  int numshells=0;
  if (normals) vol=0;
  while (!unk.empty())
  {
    std::vector<int> shell;
    shell.reserve(mesh.size());
    // take one element
    numshells++;
    int ele=*unk.begin();
    unk.erase(unk.begin());
    // compute normal and orientation
    int ori=1;//mesh[ele].ori(mesh[ele]);
    normal[ele]=mesh[ele].normal()*ori;
    bnd.push_back(ele);
    shell.push_back(ele);
    known[ele]=ori;
    while (!bnd.empty())
    {
      int ele=*bnd.begin();
      bnd.pop_front();
      int ori=known[ele];
      for (int i=0;i<3;++i)
      {
        int nei=mesh_it[ele].edg[i]->getneighbor(ele);
        if (nei!=-1)
        {
          unk.erase(nei);
          if (known.find(nei)==known.end())
          {
            bnd.push_back(nei);
            int newori=mesh_it[nei].ori(mesh_it[ele])*ori;
            known[nei]=newori;
          // compute normal
            if (normals)
            {
              normal[nei]=mesh[nei].normal()*newori;
            }
            shell.push_back(nei);
          }
        }
      }
    }
    if (normals)
    {
      // compute subvolume to orient normals correctly (pointing outwards)...
      double v=0.0;
      for (int i=0;i<shell.size();++i)
      {
        double vv=fabs(tri_area(mesh[shell[i]]));
        if (normal[shell[i]][2]<0) vv*=-1;
        v+=vv*mesh[shell[i]].G()[2];
      }
      if (v<0.0) // reverse all normals
      {
        for (int i=0;i<shell.size();++i)
        {
          normal[shell[i]]*=-1;
        }
      }
      vol+=fabs(v);
    }
  }
//  std::cout << " known=" << known.size() << std::endl;
  if (!quiet) std::cout << "Numshells=" << numshells ;
  // check for convexity
  convex=true;
  double psm=0;
  double psp=0;
  if (numshells!=1)
    convex=false;
//  else
  {
    // check all edges
    for (std::set<edg_uniq,less_edg_uniq>::iterator it=edsu->begin();it!=edsu->end();++it)
    {
      if (it->tri.size()!=2)
      {
        convex=false;
      }
      else
      {

        npoint3 p1=mesh[it->tri[0]].G();
//        npoint3 v1=*(it->pts[0])-p1;
//        npoint3 v2=*(it->pts[1])-p1;
        npoint3 v1=it->pts[0]-p1;
        npoint3 v2=it->pts[1]-p1;

//        npoint3 p2=mesh[it->tri[1]].G();
//        npoint3 vect=p2-p1;
        npoint3 n1=normal[it->tri[0]];
        npoint3 n2=normal[it->tri[1]];
        npoint3 vn;
        vn.crossprod(v1,v2);
        vn.normalize();
        double ps=vn.dotprod(n1);
        int i1=0,i2=0;
        if (ps>=0) i2=1;else i1=1;
//        npoint3 l=*(it->pts[i1])-*(it->pts[i2]);
        npoint3 l=it->pts[i1]-it->pts[i2];
        npoint3 dir;dir.crossprod(n1,l);
        double ps2=dir.dotprod(n2);
        if (ps2<-1e-8)
        {
          convex=false;
          psm-=ps2;
        }
        else
          psp+=ps2;
      }
    }
  }
  if (!quiet) if (convex==false) std::cout << ", not " ; else std::cout << ", ";
  if (!quiet) std::cout << "convex. ";
  if (!quiet) std::cout << "Concavity factor="<< psm/(psp+psm)<< std::endl;
}

void tri_mesh::clear_topology(void)
{
  mesh_it.clear();
  has_topo=false;
  if (pts) 
  {
    pts->clear();
    delete pts;
    pts=NULL;
  }
if (edsu) 
  {
    edsu->clear();
    delete edsu;
    edsu=NULL;
  }
}

void tri_mesh::clear(void)
{
  clear_topology();
  mesh.clear();
  normal.clear();
  min=max=npoint3(0,0,0);
}

void tri_mesh::add(tri_bb tr, npoint3 n)
{
  mesh.push_back(tr);
  normal.push_back(n);
}

void tri_mesh::transform(const tri_mesh &other, double matf[4][4])
{
  clear();
  for (int i=0;i<other.mesh.size();++i)
  {
    triangle tr;
    tri_transfo(other.mesh[i],matf,tr);
    npoint n1(other.normal[i]),n2;
    n1[3]=0.;
    npt_transfo(matf,n1,n2);
    npoint3 n(n2[0],n2[1],n2[2]);
    add(tr,n);
  }
}

void tri_mesh::project(const tri_mesh &other,int axis)
{
  clear();
  for (int i=0;i<other.mesh.size();++i)
  {
    triangle tr;
    npoint3 n(0.);
    tri_project(other.mesh[i],axis,tr);
    if (other.normal[i][axis]<0.)
      n[axis]=-1.0;
    else 
      if (other.normal[i][axis]>0.)
        n[axis]=1.0;      
    add(tr,n);
  }
}

void tri_mesh::cull(const tri_mesh &other)
{
 clear();
 for (int i=0;i<other.mesh.size();++i)
 {
   if (other.normal[i][2]<0.0||other.normal[i].norm()==0.0)
     add(other.mesh[i],other.normal[i]);
 } 
}

void tri_mesh::display(data_container &data) const
{
  for (int i=0;i<mesh.size();++i) 
  {
    data.add_triangle(mesh[i]);
/*    npoint3 bary(0.,0.,0.);
    for (int j=0;j<3;++j)
      for (int k=0;k<3;++k)
        bary[k]+=mesh[i].pts[j][k]/3.0;
    line l(bary,bary+normal[i]*0.1);
    data.add_line(l);
*/
  }
  
  if (has_topo) // display exceptional edges (having only 1 or >2 neighbors)
  {
    for (std::set<edg_uniq,less_edg_uniq>::iterator it=edsu->begin();it!=edsu->end();++it)
    {
      if (it->tri.size()!=2)
      {
        line l(it->pts[0],it->pts[1]);
        data.add_line(l);
        data.add_point(it->pts[0]);
        data.add_point(it->pts[1]);
      }
    }
  }
}


void tri_mesh::shade(const tri_mesh &other)
{
  clear();
  double sharea=0.;
  double prarea=0.;
  assert(other.mesh.size()>0);
  mesh.push_back(other.mesh[0]);
  sharea+=fabs(tri_area(other.mesh[0]));
  prarea+=fabs(tri_area(other.mesh[0]));
  int etape=0;
  for (int i=1;i<other.mesh.size();++i)
  {
    std::vector<tri_bb> meshtemp;
    meshtemp.push_back(other.mesh[i]);
    for (int j=0;j<mesh.size();++j)
    {
      for (int k=0;k!=meshtemp.size();++k)
      {
        std::vector<tri_bb> meshtemp2;
        etape++;
        bool nonew = false;
//        if (meshtemp[k].area()>1e-6*meshtemp[k].circ()*meshtemp[k].circ())
          nonew=meshtemp[k].cut(mesh[j],meshtemp2/*,etape>40000000*/);
        if (!nonew)
        {
          std::vector<tri_bb>::iterator ik = meshtemp.begin() + k;
          meshtemp.erase(ik);k--;
          for (int l=0;l<meshtemp2.size();++l)
            meshtemp.push_back(meshtemp2[l]);
        }
      }
    }
    prarea+=fabs(tri_area(other.mesh[i]));
    for (int l=0;l<meshtemp.size();++l)
    {
      mesh.push_back(meshtemp[l]);
      sharea+=fabs(tri_area(meshtemp[l]));
    }
/*    if (! (i%1000) ) 
      std::cout << "Tri#: " <<i << " shade area: " << sharea << " pr. area: " << prarea << " tot tris: " << mesh.size() << std::endl;*/
  }
  normal.resize(mesh.size());
}

double tri_mesh::area2(void) const
{
  double a=0.0;
  for (int i=0;i<mesh.size();++i)
    a+=fabs(tri_area(mesh[i]));
  return a;
}

double tri_mesh::area(void) const
{
  double a=0.0;
  for (int i=0;i<mesh.size();++i)
    a+=mesh[i].area();
  return a;
}

double tri_mesh::volume(void) const
{
  double v=0.0;
  if ((vol<0) && (normal.size()==mesh.size()))
  {
    for (int i=0;i<mesh.size();++i)
    {
      double vv=fabs(tri_area(mesh[i]));
      if (normal[i][2]<0) vv*=-1;
      v+=vv*mesh[i].G()[2];
    }
  }
  else v=vol;
  return v;
}


void tri_mesh::add_sphere(npoint3 center, double r,int n)
{
  npoint3 pt11,pt12,pt21,pt22;
  for (int i=-n+1;i<n-1;++i)
  {
    double th1=i*(Pi/(2*(n-1)));
    double th2=(i+1)*(Pi/(2*(n-1)));
    for (int j=0;j<4*n-4;++j)
    {

      double phi1=j*(Pi/(2*(n-1)));
      double phi2=(j+1)*(Pi/(2*(n-1)));
      if (j==4*n-4-1)
        phi2=0;    
      if (i==-n+1)
      {
        pt11=center+r*npoint3(cos(th1),0,sin(th1));
        pt12=center+r*npoint3(cos(th1),0,sin(th1));
        pt21=center+r*npoint3(cos(th2)*cos(phi1),cos(th2)*sin(phi1),sin(th2));
        pt22=center+r*npoint3(cos(th2)*cos(phi2),cos(th2)*sin(phi2),sin(th2));
      }
      else if (i==n-2)
      {
        pt11=center+r*npoint3(cos(th1)*cos(phi1),cos(th1)*sin(phi1),sin(th1));
        pt12=center+r*npoint3(cos(th1)*cos(phi2),cos(th1)*sin(phi2),sin(th1));
        pt21=center+r*npoint3(cos(th2),0,sin(th2));
        pt22=center+r*npoint3(cos(th2),0,sin(th2));
      }
      else
      {
        pt11=center+r*npoint3(cos(th1)*cos(phi1),cos(th1)*sin(phi1),sin(th1));
        pt12=center+r*npoint3(cos(th1)*cos(phi2),cos(th1)*sin(phi2),sin(th1));
        pt21=center+r*npoint3(cos(th2)*cos(phi1),cos(th2)*sin(phi1),sin(th2));
        pt22=center+r*npoint3(cos(th2)*cos(phi2),cos(th2)*sin(phi2),sin(th2));
      }
      triangle tr1(pt11,pt12,pt22);
      triangle tr2(pt11,pt22,pt21);
      if (i>-n+1) add(tr1,npoint3(0,0,0));
      if (i<n-2) add(tr2,npoint3(0,0,0));
    }
  }
}

void tri_mesh::add_cylinder(npoint3 center1, npoint3 center2, double r,bool close1,bool close2,int n,npoint3 ori1,npoint3 ori2, npoint3 dtop)
{
  npoint3 dir31;
  npoint3 dir11,dir21,dtemp;
  npoint3 dir32;
  npoint3 dir12,dir22;

  if (ori1.norm()!=0.0)
    dir31=ori1;
  else 
    dir31=center2-center1;
  if (ori2.norm()!=0.0)
    dir32=ori2;
  else
    dir32=center2-center1;
  
  dir31.normalize();
  dir32.normalize();
  npoint3 cross;
  cross.crossprod(dir31,dir32);
  double cn=cross.norm();
  if (cn<0.05)
  {
    if (fabs(dir31[0])<fabs(dir31[1]))
      dtemp=npoint3(1.0,0,0);
    else
      dtemp=npoint3(0.0,1.0,0);
  }
  else
  {
    dtemp=cross;
    dtemp.normalize();
  }
  if (dtop.norm()!=0.0) dtemp=dtop;
  dir11=dtemp-dtemp.dotprod(dir31)*dir31;
  dir11.normalize();
  dir21.crossprod(dir11,dir31);
  dir21.normalize();
  dir12=dtemp-dtemp.dotprod(dir32)*dir32;
  dir12.normalize();
  dir22.crossprod(dir12,dir32);
  dir22.normalize();
  for (int j=0;j<4*n-4;++j)
  {
    double phi1=j*(Pi/(2*(n-1)));
    double phi2=(j+1)*(Pi/(2*(n-1)));
    if (j==4*n-4-1)
     phi2=0;
    npoint3 pt11=center1+r*dir11*cos(phi1)+r*dir21*sin(phi1);
    npoint3 pt12=center1+r*dir11*cos(phi2)+r*dir21*sin(phi2);
    npoint3 pt21=center2+r*dir12*cos(phi1)+r*dir22*sin(phi1);
    npoint3 pt22=center2+r*dir12*cos(phi2)+r*dir22*sin(phi2);
    triangle tr1(pt11,pt12,pt22);
    triangle tr2(pt11,pt22,pt21);
    triangle tr01(center1,pt11,pt12);
    triangle tr02(center2,pt21,pt22);
    add(tr1,npoint3(0,0,0));
    add(tr2,npoint3(0,0,0));
    if (close1)
      add(tr01,npoint3(0,0,0));
    if (close2)
      add(tr02,npoint3(0,0,0));
  }
}



// create helix

void tri_mesh::add_helix(double R, double r,int nb_parts,int n)
{
  for (int i=0;i<nb_parts;++i)
  {
    double u1=4*i*Pi/nb_parts;
    double u2=4*(i+1)*Pi/nb_parts;
    npoint3 base1(R*cos(u1),R*sin(u1),u1*r);
    npoint3 vec1(R*-sin(u1),R*cos(u1),r);
    vec1.normalize();
    npoint3 base2(R*cos(u2),R*sin(u2),u2*r);
    npoint3 vec2(R*-sin(u2),R*cos(u2),r);
    vec2.normalize();
    if (i==0)
      add_cylinder(base1,base2,r,true,false,n,vec1,vec2,npoint3(0,0,1));
    else
      if (i==(nb_parts-1))
        add_cylinder(base1,base2,r,false,true,n,vec1,vec2,npoint3(0,0,1));
      else
        add_cylinder(base1,base2,r,false,false,n,vec1,vec2,npoint3(0,0,1));
  }
}


int shade(double th,double phi,const tri_mesh &mesh,double &sharea,double &prarea,tri_mesh *meshsh)
{
  double mat1[4][4];
  double mat2[4][4];
  double matf[4][4];

  fill_rot_mat(0,phi,mat1); // first rotate around x
  fill_rot_mat(1,th,mat2); // then rotate around y
  mat_prod(mat2,mat1,matf);
  tri_mesh meshpr;
  tri_mesh meshtr;
  meshtr.transform(mesh,matf);
  tri_mesh culled;
  culled.cull(meshtr);
  meshpr.project(culled,2);
  //sort_area(meshpr);
  tri_mesh meshsh_;
  meshsh_.shade(meshpr);
  if (meshsh)
  {
    double invmatf[4][4];
    fill_rot_mat(1,-th,mat2); // rotate around y
    fill_rot_mat(0,-phi,mat1); // then rotate around x
    mat_prod(mat1,mat2,invmatf);
    meshsh->transform(meshsh_,invmatf);
  }
  sharea=meshsh_.area2();
  prarea=meshpr.area2();
  return meshsh_.size();
}