// Reconstruction - A tool for building cad models from meshes
// Copyright (C) 2010-2026 Eric Bechet, Borhen Louhichi
//
// See the LICENSE file for license information and contributions.
// Please report all bugs and problems to <bechet@cadxfem.org>.

#include "outil.h"


double Tps::tps_base_func(double r)
{
  if (r == 0.0)
    return 0.0;
  else
    return r*r * Log(r);
  //return pow(r,3);
}


std::vector<std::vector<gp_Pnt> > Tps::calc_tps(std::vector<gp_Pnt > control_points, int GRID_W, int GRID_H, double Umin, double Umax, double Vmin, double Vmax)
{

  std::vector<std::vector<gp_Pnt> > Points_BS;
  //TColgp_Array2OfPnt Points_BS(1,GRID_W+1,1,GRID_H+1);
  //float grid[GRID_W+1][GRID_H+1];

  double regularization = 0.0;
  // You We need at least 3 points to define a plane
  /*  if ( control_points.size() < 3 )
      return;
  */
  unsigned p = control_points.size();

  math_Matrix mtx_l(0,p-1+3,0,p-1+3);
  math_Vector mtx_v(0,p-1+3);
  math_Matrix mtx_orig_k(0,p-1,0,p-1);
  math_Vector mtx_x(0,p-1+3);

  double a = 0.0;
  for (unsigned i=0; i<p; ++i)
  {
    for (unsigned j=i+1; j<p; ++j)
    {
      gp_Pnt pt_i = control_points[i];
      gp_Pnt pt_j = control_points[j];
      pt_i.SetY(0.0);
      pt_j.SetY(0.0);
      double elen = pt_i.Distance(pt_j);
      mtx_l(i,j) = mtx_l(j,i) =
                     mtx_orig_k(i,j) = mtx_orig_k(j,i) =
                                         tps_base_func(elen);
      a += elen * 2; // same for upper & lower tri
    }
  }
  a /= (double)(p*p);

  // Fill the rest of L
  for (unsigned i=0; i<p; ++i)
  {
    // diagonal: reqularization parameters (lambda * a^2)
    mtx_l(i,i) = mtx_orig_k(i,i) =
                   regularization * (a*a);

    // P (p x 3, upper right)
    mtx_l(i, p+0) = 1.0;
    mtx_l(i, p+1) = control_points[i].X();
    mtx_l(i, p+2) = control_points[i].Z();

    // P transposed (3 x p, bottom left)
    mtx_l(p+0, i) = 1.0;
    mtx_l(p+1, i) = control_points[i].X();
    mtx_l(p+2, i) = control_points[i].Z();
  }
  // O (3 x 3, lower right)
  for (unsigned i=p; i<p+3; ++i)
    for (unsigned j=p; j<p+3; ++j)
      mtx_l(i,j) = 0.0;


  // Fill the right hand vector V
  for (unsigned i=0; i<p; ++i)
    mtx_v(i) = control_points[i].Y();
  mtx_v(p+0) = mtx_v(p+1) = mtx_v(p+2) = 0.0;

  // Solve the linear system "inplace"
  math_GaussLeastSquare LU_Solve(mtx_l, 1.0e-20);

  LU_Solve.Solve(mtx_v,mtx_x);

  mtx_v = mtx_x;

  double pasU= (Umax-Umin) /GRID_W, pasV= (Vmax-Vmin) /GRID_H;
  // Interpolate grid heights
  for (int i=0; i<=GRID_W; ++i)
  {
    std::vector<gp_Pnt> PntU;

    for (int j=0; j<=GRID_H; ++j)
    {
      double x=Umin + i*pasU;
      double z=Vmin + j*pasV;


      double h = mtx_v(p+0) + mtx_v(p+1) *x + mtx_v(p+2) *z;
      gp_Pnt pt_i, pt_cur(x,0,z);
      for (unsigned k=0; k<p; ++k)
      {
        pt_i = control_points[k];
        pt_i.SetY(0.0);
        h += mtx_v(k) * tps_base_func(pt_i.Distance(pt_cur));
      }
      //grid[x+GRID_W/2][z+GRID_H/2] = h;

      PntU.push_back(gp_Pnt(x,h,z));


      //std::cout<<" x : "<< x <<" z : "<< z <<" h :"<< h <<std::endl;
    }

    Points_BS.push_back(PntU);
  }

  return Points_BS;

}

std::vector<std::vector<gp_Pnt> > Tps::calc_tps_Revol(std::vector<gp_Pnt > control_points, int GRID_W, int GRID_H, double Umin, double Umax, double Vmin, double Vmax, double Rayon)
{

  std::vector<std::vector<gp_Pnt> > Points_BS;
  //TColgp_Array2OfPnt Points_BS(1,GRID_W+1,1,GRID_H+1);
  //float grid[GRID_W+1][GRID_H+1];

  double regularization = 0.0;
  // You We need at least 3 points to define a plane
  /*  if ( control_points.size() < 3 )
      return;
  */
  unsigned p = control_points.size();

  math_Matrix mtx_l(0,p-1+3,0,p-1+3);
  math_Vector mtx_v(0,p-1+3);
  math_Matrix mtx_orig_k(0,p-1,0,p-1);
  math_Vector mtx_x(0,p-1+3);

  double a = 0.0;
  for (unsigned i=0; i<p; ++i)
  {
    for (unsigned j=i+1; j<p; ++j)
    {
      gp_Pnt pt_i = control_points[i];
      gp_Pnt pt_j = control_points[j];
      pt_i.SetY(0.0);
      pt_j.SetY(0.0);
      double elen = pt_i.Distance(pt_j);
      mtx_l(i,j) = mtx_l(j,i) =
                     mtx_orig_k(i,j) = mtx_orig_k(j,i) =
                                         tps_base_func(elen);
      a += elen * 2; // same for upper & lower tri
    }
  }
  a /= (double)(p*p);

  // Fill the rest of L
  for (unsigned i=0; i<p; ++i)
  {
    // diagonal: reqularization parameters (lambda * a^2)
    mtx_l(i,i) = mtx_orig_k(i,i) =
                   regularization * (a*a);

    // P (p x 3, upper right)
    mtx_l(i, p+0) = 1.0;
    mtx_l(i, p+1) = control_points[i].X();
    mtx_l(i, p+2) = control_points[i].Z();

    // P transposed (3 x p, bottom left)
    mtx_l(p+0, i) = 1.0;
    mtx_l(p+1, i) = control_points[i].X();
    mtx_l(p+2, i) = control_points[i].Z();
  }
  // O (3 x 3, lower right)
  for (unsigned i=p; i<p+3; ++i)
    for (unsigned j=p; j<p+3; ++j)
      mtx_l(i,j) = 0.0;


  // Fill the right hand vector V
  for (unsigned i=0; i<p; ++i)
    mtx_v(i) = control_points[i].Y();
  mtx_v(p+0) = mtx_v(p+1) = mtx_v(p+2) = 0.0;

  // Solve the linear system "inplace"
  math_GaussLeastSquare LU_Solve(mtx_l, 1.0e-20);

  LU_Solve.Solve(mtx_v,mtx_x);

  mtx_v = mtx_x;

  double pasU= (Umax-Umin) /GRID_W, pasV= (Vmax-Vmin) /GRID_H;
  // Interpolate grid heights
  for (int i=0; i<=GRID_W; ++i)
  {
    std::vector<gp_Pnt> PntU;

    for (int j=0; j<=GRID_H; ++j)
    {
      double x=Rayon*i*pasU;
      double z=j*pasV;


      double h = mtx_v(p+0) + mtx_v(p+1) *x + mtx_v(p+2) *z;
      gp_Pnt pt_i, pt_cur(x,0,z);
      for (unsigned k=0; k<p; ++k)
      {
        pt_i = control_points[k];
        pt_i.SetY(0.0);
        h += mtx_v(k) * tps_base_func(pt_i.Distance(pt_cur));
      }
      //grid[x+GRID_W/2][z+GRID_H/2] = h;

      PntU.push_back(gp_Pnt(x,h,z));


      //std::cout<<" x : "<< x <<" z : "<< z <<" h :"<< h <<std::endl;
    }

    Points_BS.push_back(PntU);
  }

  return Points_BS;

}

///************************************ Average Plane *******************************

AveragePlane::AveragePlane(std::vector<gp_Pnt> & Pts, int NbBoundPoints, double Tol, int POption, int NOption) : myTol(Tol), myNbBoundPoints(NbBoundPoints)
{

  /// Noption = 1 --> inertial plane;  Noption = 2 --> the plane of max flux (in our case Noption =2)
  /// POption = 1 automatical parametrization; POption = 2 eigenvectors parametrization

  myPts = new TColgp_HArray1OfPnt(1, Pts.size());

  for (int i=0; i<NbBoundPoints; i++)
  {
    myPts->SetValue(i+1, Pts[i]);
  }

  POption = 2;

  gp_Vec OZ = DefPlan(NOption);

  if (OZ.SquareMagnitude() >0)
  {

    if (POption==1)
    {
      myPlane = new Geom_Plane(myG,OZ);
      myOX = myPlane->Pln().XAxis().Direction();
      myOY = myPlane->Pln().YAxis().Direction();
    }
    else
    {
      BasePlan(OZ);
      gp_Dir NDir(myOX^myOY);
      gp_Dir UDir(myOX);
      gp_Ax3 triedre(myG,NDir,UDir);
      myPlane = new Geom_Plane(triedre);
    }
    int i, nb=myPts->Length();
    gp_Pln P=myPlane->Pln();
    ElSLib::Parameters(P,myG,myUmax,myVmax);
    myUmin=myUmax;
    myVmin=myVmax;
    Standard_Real U=0,V=0;
    for (i=1; i<=nb; i++)
    {
      ElSLib::Parameters(P,myPts->Value(i),U,V);
      if (myUmax < U) myUmax=U;
      if (myUmin > U) myUmin=U;
      if (myVmax < V) myVmax=V;
      if (myVmin > V) myVmin=V;
    }
  }

  if (IsLine())
  {
    myLine = new Geom_Line(myG,myOX);
  }

}


//AveragePlane::~AveragePlane ()
//{

//}

gp_Vec AveragePlane::DefPlan(int NOption)
{

  gp_Pnt GB;
  gp_Vec A,B,C,D;
  gp_Vec OZ;
  Standard_Integer i,nb=myPts->Length();
  GB.SetCoord(0.,0.,0.);
  for (i=1; i<=nb; i++)
  {
    GB.SetCoord(1, (GB.Coord(1) +myPts->Value(i).Coord(1)));
    GB.SetCoord(2, (GB.Coord(2) +myPts->Value(i).Coord(2)));
    GB.SetCoord(3, (GB.Coord(3) +myPts->Value(i).Coord(3)));
  }
  myG.SetCoord(1, (GB.Coord(1) /nb));
  myG.SetCoord(2, (GB.Coord(2) /nb));
  myG.SetCoord(3, (GB.Coord(3) /nb));

  if (NOption==1)
  {
    gp_Ax2 Axe;
    Standard_Boolean IsSingular;
    GeomLib::AxeOfInertia(myPts->Array1(), Axe, IsSingular, myTol);

    myOX = Axe.XDirection();
    myOY = Axe.YDirection();

    OZ = Axe.Direction();

    if (myNbBoundPoints != 0 && myPts->Length() != myNbBoundPoints)
    {
      A.SetCoord(0.,0.,0.);
      for (i = 3; i <= myNbBoundPoints; i++)
      {
        B.SetCoord(myPts->Value(i-1).Coord(1)-myPts->Value(1).Coord(1),
                   myPts->Value(i-1).Coord(2)-myPts->Value(1).Coord(2),
                   myPts->Value(i-1).Coord(3)-myPts->Value(1).Coord(3));
        C.SetCoord(myPts->Value(i).Coord(1)-myPts->Value(1).Coord(1),
                   myPts->Value(i).Coord(2)-myPts->Value(1).Coord(2),
                   myPts->Value(i).Coord(3)-myPts->Value(1).Coord(3));
        D=B^C;
        A.SetCoord(1,A.Coord(1) +D.Coord(1));
        A.SetCoord(2,A.Coord(2) +D.Coord(2));
        A.SetCoord(3,A.Coord(3) +D.Coord(3));
      }
      gp_Vec OZ1 = A;
      Standard_Real theAngle = OZ.Angle(OZ1);
      if (theAngle > PI/2)
        theAngle = PI - theAngle;
      if (theAngle > PI/3)
        OZ = OZ1;
    }
  }

  else if (NOption==2)
  {
    A.SetCoord(0.,0.,0.);
    for (i = 3; i <= myNbBoundPoints; i++)
    {
      B.SetCoord(myPts->Value(i-1).Coord(1)-myPts->Value(1).Coord(1),
                 myPts->Value(i-1).Coord(2)-myPts->Value(1).Coord(2),
                 myPts->Value(i-1).Coord(3)-myPts->Value(1).Coord(3));
      C.SetCoord(myPts->Value(i).Coord(1)-myPts->Value(1).Coord(1),
                 myPts->Value(i).Coord(2)-myPts->Value(1).Coord(2),
                 myPts->Value(i).Coord(3)-myPts->Value(1).Coord(3));
      D=B^C;
      A.SetCoord(1,A.Coord(1) +D.Coord(1));
      A.SetCoord(2,A.Coord(2) +D.Coord(2));
      A.SetCoord(3,A.Coord(3) +D.Coord(3));
    }
    OZ = A;
  }
  return OZ;
}


void AveragePlane::BasePlan(gp_Vec& OZ)
{
  math_Matrix M(1, 3, 1, 3);
  M.Init(0.);
  gp_Vec Proj;
  Standard_Integer i,nb=myPts->Length();
  Standard_Real scal;

  for (i=1; i<=nb; i++)
  {
    Proj.SetCoord(1,myPts->Value(i).Coord(1) - myG.Coord(1));
    Proj.SetCoord(2,myPts->Value(i).Coord(2) - myG.Coord(2));
    Proj.SetCoord(3,myPts->Value(i).Coord(3) - myG.Coord(3));
    scal = Proj.Coord(1) *OZ.Coord(1)
           + Proj.Coord(2) *OZ.Coord(2)
           + Proj.Coord(3) *OZ.Coord(3);
    scal /= OZ.Coord(1) *OZ.Coord(1)
            + OZ.Coord(2) *OZ.Coord(2)
            + OZ.Coord(3) *OZ.Coord(3);
    Proj.SetCoord(1,Proj.Coord(1) - scal*OZ.Coord(1));
    Proj.SetCoord(2,Proj.Coord(2) - scal*OZ.Coord(2));
    Proj.SetCoord(3,Proj.Coord(3) - scal*OZ.Coord(3));
    M(1,1) += Proj.Coord(1) *Proj.Coord(1);
    M(2,2) += Proj.Coord(2) *Proj.Coord(2);
    M(3,3) += Proj.Coord(3) *Proj.Coord(3);
    M(1,2) += Proj.Coord(1) *Proj.Coord(2);
    M(1,3) += Proj.Coord(1) *Proj.Coord(3);
    M(2,3) += Proj.Coord(2) *Proj.Coord(3);
  }
  M(2,1) = M(1,2) ;
  M(3,1) = M(1,3) ;
  M(3,2) = M(2,3) ;
  math_Jacobi J(M);
  Standard_Real n1,n2,n3;
  math_Vector V1(1,3),V2(1,3),V3(1,3);
  n1=J.Value(1);
  n2=J.Value(2);
  n3=J.Value(3);

  Standard_Real r1 = Min(Min(n1,n2),n3), r2;
  Standard_Integer m1, m2, m3;
  if (r1==n1)
  {
    m1 = 1;
    r2 = Min(n2,n3);
    if (r2==n2)
    {
      m2 = 2;
      m3 = 3;
    }
    else
    {
      m2 = 3;
      m3 = 2;
    }
  }
  else
  {
    if (r1==n2)
    {
      m1 = 2 ;
      r2 = Min(n1,n3);
      if (r2==n1)
      {
        m2 = 1;
        m3 = 3;
      }
      else
      {
        m2 = 3;
        m3 = 1;
      }
    }
    else
    {
      m1 = 3 ;
      r2 = Min(n1,n2);
      if (r2==n1)
      {
        m2 = 1;
        m3 = 2;
      }
      else
      {
        m2 = 2;
        m3 = 1;
      }
    }
  }
  J.Vector(m1,V1);
  J.Vector(m2,V2);
  J.Vector(m3,V3);

  std::cout<<" n1 : "<< n1 <<" n2 "<< n2 <<" n3 "<< n3 <<std::endl;
  std::cout<<" m1 : "<< m1 <<" m2 "<< m2 <<" m3 "<< m3 <<std::endl;

  std::cout<<" V1 : "<< V1(1) <<"   "<< V1(2) <<"   "<< V1(3) <<std::endl;
  std::cout<<" V2 : "<< V2(1) <<"   "<< V2(2) <<"   "<< V2(3) <<std::endl;
  std::cout<<" V3 : "<< V3(1) <<"   "<< V3(2) <<"   "<< V3(3) <<std::endl;

  if (((Abs(n1) <=myTol) && (Abs(n2) <=myTol))
      || ((Abs(n2) <=myTol) && (Abs(n3) <=myTol))
      || ((Abs(n1) <=myTol) && (Abs(n3) <=myTol)))
  {
    myOX.SetCoord(V3(1),V3(2),V3(3));
    myOY.SetCoord(0,0,0);
  }
  else
  {
    myOX.SetCoord(V3(1),V3(2),V3(3));
    myOY.SetCoord(V2(1),V2(2),V2(3));
  }
}

void AveragePlane::MinMaxBox(double& Umin, double& Umax, double& Vmin, double& Vmax)
{
  Umax=myUmax;
  Umin=myUmin;
  Vmax=myVmax;
  Vmin=myVmin;
}

Handle(Geom_Plane) AveragePlane::Plane()
{
  Standard_NoSuchObject_Raise_if(IsLine() , "Cannot use the function 'AveragePlane::Plane()', the Object is a 'Geom_Line'");
  return myPlane;
}

Handle(Geom_Line) AveragePlane::Line()
{
  Standard_NoSuchObject_Raise_if(IsPlane() , "Cannot use the function 'AveragePlane::Line()', the Object is a 'Geom_Plane'");
  return myLine;
}

bool AveragePlane::IsPlane()
{
  gp_Vec OZ=myOX^myOY;
  if (OZ.SquareMagnitude() <=1e-6)
    return false;
  else
    return true;

}

bool AveragePlane::IsLine()
{
  gp_Vec OZ=myOX^myOY;
  if (OZ.SquareMagnitude() <=1e-6)
    return true;
  else
    return false;
}


///***********************************************************************************************



/// Thibaud Mouton
Bucket::Bucket(gp_Pnt min, gp_Pnt max, double enlarge, int max_size)
{
  gp_Pnt center((min.X() +max.X()) /2.0, (min.Y() +max.Y()) /2.0, (min.Z() +max.Z()) /2.0);
  gp_Pnt semidiag((max.X()-min.X()) /2.0, (max.Y()-min.Y()) /2.0, (max.Z()-min.Z()) /2.0);

  _min.SetCoord(center.X() - semidiag.X() - enlarge, center.Y() - semidiag.Y() - enlarge, center.Z() - semidiag.Z() - enlarge);
  _max.SetCoord(center.X() + semidiag.X() + enlarge, center.Y() + semidiag.Y() + enlarge, center.Z() + semidiag.Z() + enlarge);

  double dmax = _max.X()-_min.X();
  if (dmax < _max.Y()-_min.Y())
    dmax = _max.Y()-_min.Y();
  if (dmax < _max.Z()-_min.Z())
    dmax = _max.Z()-_min.Z();

  _ref_size = dmax/ (double) max_size;

  _nx = floor((_max.X()-_min.X()) /_ref_size) +1;
  _ny = floor((_max.Y()-_min.Y()) /_ref_size) +1;
  _nz = floor((_max.Z()-_min.Z()) /_ref_size) +1;

  _maxn = _nx;
  if (_maxn < _ny)
    _maxn = _ny;
  if (_maxn < _nz)
    _maxn = _nz;

  _min.SetCoord(center.X()- (double) _nx*_ref_size/2.0, center.Y()- (double) _ny*_ref_size/2.0, center.Z()- (double) _nz*_ref_size/2.0);
  _max.SetCoord(center.X() + (double) _nx*_ref_size/2.0, center.Y() + (double) _ny*_ref_size/2.0, center.Z() + (double) _nz*_ref_size/2.0);

  _bucket.resize(_nx*_ny*_nz);
  _flag.resize(_nx*_ny*_nz, 0);
  _request_id = 0;
}


int Bucket::contain(gp_Pnt *pt)
{
  if (pt->X() < _min.X())
    return false;
  if (pt->Y() < _min.Y())
    return false;
  if (pt->Z() < _min.Z())
    return false;
  if (pt->X() > _max.X())
    return false;
  if (pt->Y() > _max.Y())
    return false;
  if (pt->Z() > _max.Z())
    return false;

  return true;
}

void Bucket::get_coord(gp_Pnt *pt, int &x, int &y, int &z)
{
  x = (pt->X()-_min.X()) /_ref_size;
  y = (pt->Y()-_min.Y()) /_ref_size;
  z = (pt->Z()-_min.Z()) /_ref_size;
}

int Bucket::get_index(int x, int y, int z)
{
  return x + y*_nx + z*_nx*_ny;
}

void Bucket::add_point(gp_Pnt *pt)
{
  int x, y, z;
  get_coord(pt, x, y, z);
  int index = get_index(x, y, z);
  _bucket[index].push_back(pt);
}

gp_Pnt *Bucket::get_closest_in_bucket(int index, gp_Pnt *pt, double *dist)
{
  gp_Pnt *found = NULL;
  double dd = (*dist) * (*dist);
  for (int i = 0; i < _bucket[index].size(); i++)
  {
    gp_Pnt *pb = _bucket[index][i];
    gp_Vec ab;
    ab= gp_Vec(*pt, *pb);
    double d = ab.SquareMagnitude();
    if (d < dd)
    {
      found = pb;
      dd = d;
    }
  }
  _flag[index] = _request_id;

  *dist = sqrt(dd);
  return found;
}


gp_Pnt *Bucket::get_closest(gp_Pnt *pt)
{
  gp_Pnt *found = NULL;
  double dist = INFINITY;

  _request_id++;

  int x, y, z;
  get_coord(pt, x, y, z);

  int index = get_index(x, y, z);

  int level = 0;
  gp_Pnt *tmp;

  int imax;
  int imin;
  int jmax;
  int jmin;
  int kmax;
  int kmin;

  while ((found == NULL) || (level < 2) && (level < _maxn))
  {
    imin = x-level;
    imax = x+level;
    jmin = y-level;
    jmax = y+level;
    kmin = z-level;
    kmax = z+level;
    if (imin < 0)
      imin = 0;
    if (imax > _nx-1)
      imax = _nx-1;
    if (jmin < 0)
      jmin = 0;
    if (jmax > _ny-1)
      jmax = _ny-1;
    if (kmin < 0)
      kmin = 0;
    if (kmax > _nz-1)
      kmax = _nz-1;

    for (int k = kmin; k <= kmax; k++)
      for (int j = jmin; j <= jmax; j++)
        for (int i = imin; i <= imax; i++)
        {
          index = get_index(i, j, k);
          if (_flag[index] != _request_id)
          {
            tmp = get_closest_in_bucket(index, pt, &dist);
            if (tmp != NULL)
              found = tmp;
          }
        }
    level++;
  }
  return found;
}