// 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 "RecBSplineLM.h"


double Nip(int p,int m,std::vector<double> &knots, int i, double u)
{
  if ((i==0 && u== knots[0]) || (i==m-p-1 && u== knots[m]))
  {
    return 1.0;
  }

  if ((u < knots[i]) || (u > knots[i+p+1]))
  {
    return 0.0;
  }


  double *grand_n=new double[p+1];
  for (int j=0; j<=p; j++)
  {
    if (u >= knots[i+j] && u < knots[i+j+1]) grand_n[j] = 1.0;
    else grand_n[j] = 0.0;

  }

  for (int k=1; k<=p; k++)
  {
    double saved;
    if (grand_n[0] == 0.0) saved = 0.0;
    else saved = ((u-knots[i]) *grand_n[0]) / (knots[i+k]-knots[i]);

    for (int j=0; j<p-k+1; j++)
    {
      double Uleft = knots[i+j+1];
      double Uright = knots[i+j+k+1];
      if (grand_n[j+1] == 0.0)
      {
        grand_n[j] = saved;
        saved =0.0;
      }
      else
      {
        double temp = grand_n[j+1]/ (Uright - Uleft);
        grand_n[j] = saved + (Uright - u) *temp;
        saved = (u-Uleft) *temp;
      }

    }

  }

  return grand_n[0];

  delete [] grand_n;

}



Handle(Geom_BSplineSurface) RecBSplineLM::Surface()
{

  int udeg = Surf->UDegree();
  int vdeg = Surf->VDegree();

  cout << " "<< udeg << " "<< vdeg << " ";

  int nPCu = Surf->NbUPoles();
  int nPCv = Surf->NbVPoles();

  TColgp_Array2OfPnt PointsC(1,nPCu,1,nPCv);
  Surf->Poles(PointsC);

  math_Vector P0(1,3*nPCu*nPCv);
  int tt = 1;

  for (int i=0; i<nPCu; ++i)
  {
    for (int j=0; j<nPCv; ++j)
    {

      gp_Pnt Pnt = PointsC(i+1,j+1);
      P0(tt) = Pnt.X();
      tt = tt+1;
      P0(tt) = Pnt.Y();
      tt = tt+1;
      P0(tt) = Pnt.Z();
      tt = tt+1;

      //p = npoint(Pnts(i+1,j+1).X(),Pnts(i+1,j+1).Y(),Pnts(i+1,j+1).Z(),W(i+1,j+1));
      //surf.CP(i,j)=p;
      //std::cout<<" i j: "<< i <<"   "<< j <<" xyz : "<< Pnt.X() <<"   "<< Pnt.Y() <<"   "<< Pnt.Z() <<"   "<<std::endl;

    }
  }

  int nbknotsU = Surf->NbUKnots();
  int nbknotsV = Surf->NbVKnots();

  TColStd_Array1OfReal Ku(1,nbknotsU);
  TColStd_Array1OfReal Kv(1,nbknotsV);

  Surf->UKnots(Ku);
  Surf->VKnots(Kv);

  TColStd_Array1OfInteger Mu(1,nbknotsU);
  TColStd_Array1OfInteger Mv(1,nbknotsV);

  Surf->UMultiplicities(Mu);
  Surf->VMultiplicities(Mv);

  std::vector<double> knotsu;

  //int ind=0;
  for (int i=0; i<nbknotsU; ++i)
  {
    for (int j=0; j<Mu(i+1); ++j)
    {

      knotsu.push_back(Ku(i+1));
      //surf.u(ind)=UKnots(i+1);
      //ind++;
      std::cout<<" U : "<< Ku(i+1) <<std::endl;
    }
  }

  //ind++;

  std::vector<double> knotsv;

  //ind=0;
  for (int i=0; i<nbknotsV; ++i)
  {
    //std::cout<<" Mv : "<< Mv(i+1) <<std::endl;
    for (int j=0; j<Mv(i+1); ++j)
    {
      knotsv.push_back(Kv(i+1));

      //surf.v(ind)=VKnots(i+1);
      //ind++;
      std::cout<<" V : "<< Kv(i+1) <<std::endl;

    }
  }

  int numPointsControle = nPCu*nPCv;
  int numPointsContraint = pts.size();

  npoint3 PointsContraint[numPointsContraint];

  for (int i=0; i<numPointsContraint; i++)
  {
    PointsContraint[i] = pts[i];

  }


  double Lamda = 0.0001;
  int maxIteration = 3;

  for (int ii0 = 0; ii0 < maxIteration; ++ii0)
  {

    Lamda = Lamda*0.04;
    math_Matrix F0(1, numPointsContraint, 1, 3*numPointsControle, 0.0);

    for (int ii1 = 1; ii1 <= numPointsContraint; ++ii1)
    {
      int jj0 = 1;

      for (int jj1 = 1; jj1 <= numPointsControle; ++jj1)
      {
        double xyz[3];
        double uv[2];

        xyz[0]=PointsContraint[ii1-1].x();
        xyz[1]=PointsContraint[ii1-1].y();
        xyz[2]=PointsContraint[ii1-1].z();

        GeomAPI_ProjectPointOnSurf Proj(gp_Pnt(PointsContraint[ii1-1].x(),PointsContraint[ii1-1].y(),PointsContraint[ii1-1].z()),Surf);
        double distance = Proj.LowerDistance();
        double U,V;
        Proj.LowerDistanceParameters(U,V);
        gp_Pnt PntProj = Proj.NearestPoint();
        xyz[0] = PntProj.X();
        xyz[1] = PntProj.Y();
        xyz[2] = PntProj.Z();

        gp_Pnt P;
        gp_Vec D1U;
        gp_Vec D1V;
        gp_Vec D2U;
        gp_Vec D2V;
        gp_Vec D2UV;
        Surf->D2(U,V,P,D1U,D1V,D2U,D2V,D2UV);
        double mult = D2U*gp_Vec(PntProj,gp_Pnt(PointsContraint[ii1-1].x(),PointsContraint[ii1-1].y(),PointsContraint[ii1-1].z()));

        if (mult>0.0) distance = -distance;

        //std::cout<<" U : "<< U <<std::endl;
        //std::cout<<" V : "<< V <<std::endl;
        //std::cout<<" distance : "<< distance <<std::endl;

        //gp_Pnt2d Pnt_Ins2D(U,V);

        //surf.inverser(uv,xyz,0.001);
        //surf.evaluer(uv,xyz);
        //double distance = sqrt(pow(xyz[0]-PointsContraint[ii1-1].x(),2)+pow(xyz[1]-PointsContraint[ii1-1].y(),2)+pow(xyz[2]-PointsContraint[ii1-1].z(),2));

        //cout << distance << " ";
        //cout << endl;

        //cout << uv[0] << " "<< uv[1] << " ";
        //cout << endl;

        div_t divresult;
        divresult = div(jj1-1,nPCv);

        //cout << divresult.quot << " "<< divresult.rem << " ";
        //cout << endl;

        //int interu = surf.findspan(knots, surf.nb_knots_u(), 2, uv[0]);
        //div0
        //double grand_nu[3];
        //surf.get_valeur_fonction(interu, uv[0], 2, knots, grand_nu);
        //double Nu = grand_nu[2];

        double Nu = Nip(udeg,knotsu.size()-1,knotsu, divresult.quot, U);
        double Nv = Nip(vdeg,knotsv.size()-1,knotsv, divresult.rem, V);

        //cout << " Nipu "<< Nu << " Nipv "<< Nv << " ";
        //cout << endl;

        //std::vector<double> N_u;
        //surf.basisfuns(knots,7, 2, interu, uv[0], N_u);
        //double Nu = N_u[2];

        //int interv = surf.findspan(knots, surf.nb_knots_v(), 2, uv[1]);
        //double grand_nv[3];
        //surf.get_valeur_fonction(divresult.quot, uv[0], 2, knots, grand_nv);
        //double Nv = grand_nv[2];

        //cout << " distance "<< distance << " "<< " ";
        //cout << endl;

        double d_dxi, d_dyi, d_dzi;

        if (distance<1e-3)
        {
          d_dxi = 0.0;
          d_dyi = 0.0;
          d_dzi = 0.0;

        }
        else
        {

          d_dxi = (2* (xyz[0]-PointsContraint[ii1-1].x()) *Nu*Nv) /distance;
          d_dyi = (2* (xyz[1]-PointsContraint[ii1-1].y()) *Nu*Nv) /distance;
          d_dzi = (2* (xyz[2]-PointsContraint[ii1-1].z()) *Nu*Nv) /distance;

        }


        //cout << " d_dxi "<< d_dxi << " d_dyi "<< d_dyi << " d_dzi "<< d_dzi << " ";

        F0(ii1,jj0) = d_dxi;
        jj0 = jj0+1;
        F0(ii1,jj0) = d_dyi;
        jj0 = jj0+1;
        F0(ii1,jj0) = d_dzi;
        jj0 = jj0+1;

      }



    }

    math_Matrix U(1, 3*numPointsControle, 1, 3*numPointsControle, 0.0);

    math_Matrix F0T = F0.Transposed();

    U=F0T*F0;

    math_Vector dP0(1,numPointsContraint);

    for (int ii1 = 1; ii1 <= numPointsContraint; ++ii1)
    {

      /*double xyz[3];
      double uv[2];
      xyz[0]=PointsContraint[ii1-1].x(); xyz[1]=PointsContraint[ii1-1].y(); xyz[2]=PointsContraint[ii1-1].z();
      surf.inverser(uv,xyz,0.001);
      surf.evaluer(uv,xyz);*/
      GeomAPI_ProjectPointOnSurf Proj(gp_Pnt(PointsContraint[ii1-1].x(),PointsContraint[ii1-1].y(),PointsContraint[ii1-1].z()),Surf);
      double distance = Proj.LowerDistance();

      double U,V;
      Proj.LowerDistanceParameters(U,V);
      gp_Pnt PntProj = Proj.NearestPoint();

      gp_Pnt P;
      gp_Vec D1U;
      gp_Vec D1V;
      gp_Vec D2U;
      gp_Vec D2V;
      gp_Vec D2UV;
      Surf->D2(U,V,P,D1U,D1V,D2U,D2V,D2UV);
      double mult = D2U*gp_Vec(PntProj,gp_Pnt(PointsContraint[ii1-1].x(),PointsContraint[ii1-1].y(),PointsContraint[ii1-1].z()));

      if (mult>0.0) distance = -distance;


      dP0(ii1) = distance;

      // verifier
      //dP0(ii1) = sqrt(pow(xyz[0]-PointsContraint[ii1-1].x(),2)+pow(xyz[1]-PointsContraint[ii1-1].y(),2)+pow(xyz[2]-PointsContraint[ii1-1].z(),2));

      //cout << dP0(ii1) << " ";
      //cout << endl;
    }

    math_Vector V(1,3*numPointsControle,0.0);

    V = F0T*dP0;

    double J0 = 0.0;

    for (int ii1 = 1; ii1 <= numPointsContraint; ++ii1)
    {

      /*double xyz[3];
            double uv[2];
            xyz[0]=PointsContraint[ii1-1].x(); xyz[1]=PointsContraint[ii1-1].y(); xyz[2]=PointsContraint[ii1-1].z();
            surf.inverser(uv,xyz,0.001);
            surf.evaluer(uv,xyz);*/
      GeomAPI_ProjectPointOnSurf Proj(gp_Pnt(PointsContraint[ii1-1].x(),PointsContraint[ii1-1].y(),PointsContraint[ii1-1].z()),Surf);
      double distance = Proj.LowerDistance();

      J0 = J0 + distance*distance;

      //J0 = J0 + (pow(xyz[0]-PointsContraint[ii1-1].x(),2)+pow(xyz[1]-PointsContraint[ii1-1].y(),2)+pow(xyz[2]-PointsContraint[ii1-1].z(),2));

    }

    double Jnew = 0.0;

    math_Vector Pnew(1,3*numPointsControle);

    int iteration = 0;

    Handle(Geom_BSplineSurface) Surf2 = Surf;

    do
    {

      Lamda = Lamda*10;

      math_Matrix H(1,3*numPointsControle, 1,3*numPointsControle, 0.0);

      math_Matrix U0(1,3*numPointsControle,1,3*numPointsControle,0.0);

      for (int ii1 = 1; ii1 <= 3*numPointsControle; ++ii1)
      {

        U0(ii1,ii1) = Lamda* (1+U(ii1,ii1));

      }

      H = U+U0;

      math_Vector X(1,3*numPointsControle,0.0);
      math_Gauss M(H, 1.0e-6);
      M.Solve((-1.0*V),X);

      Pnew = P0+X;

      int tt=1;

      for (int ii0 = 1; ii0 <= nPCu; ++ii0)
      {

        for (int jj0 = 1; jj0 <= nPCv; ++jj0)
        {
          double x = Pnew(tt);
          tt = tt+1;
          double y = Pnew(tt);
          tt = tt+1;
          double z = Pnew(tt);
          tt = tt+1;

          //std::cout<< ii0 << " "<< jj0 << " "<< x << " "<< y << " "<< z << " " <<std::endl;

          //npoint PC(x,y,z,1.0);

          //surf.set_CP(jj0,ii0,PC);

          Surf2->SetPole(ii0, jj0, gp_Pnt(x,y,z));

        }
      }








      for (int ii1 = 1; ii1 <= numPointsContraint; ++ii1)
      {

        GeomAPI_ProjectPointOnSurf Proj(gp_Pnt(PointsContraint[ii1-1].x(),PointsContraint[ii1-1].y(),PointsContraint[ii1-1].z()),Surf2);
        double distance = Proj.LowerDistance();

        Jnew = Jnew + distance*distance;


      }




      iteration = iteration + 1;

    }
    while ((Jnew >= J0) && (iteration !=6));



    if (Jnew<J0)
    {

      P0 = Pnew;

      int tt=1;

      for (int ii0 = 1; ii0 <= nPCu; ++ii0)
      {

        for (int jj0 = 1; jj0 <= nPCv; ++jj0)
        {
          double x = Pnew(tt);
          tt = tt+1;
          double y = Pnew(tt);
          tt = tt+1;
          double z = Pnew(tt);
          tt = tt+1;

          //std::cout<< ii0 << " "<< jj0 << " "<< x << " "<< y << " "<< z << " " <<std::endl;

          //npoint PC(x,y,z,1.0);

          //surf.set_CP(jj0,ii0,PC);

          Surf->SetPole(ii0, jj0, gp_Pnt(x,y,z));

        }
      }

    }




  }

  return Surf;



}

Handle(Geom_BSplineSurface) RecBSplineLM::SurfaceP()
{


  GeomPlate_BuildPlateSurface BPSurf(Surf,3,10,3);

  for (int j=0; j<pts.size(); ++j)
  {

    double x, y, z;
    //fscanf(pFile, "%lf %lf %lf\n", &x, &y, &z);
    npoint3 Pt = pts[j];
    x = Pt.x();
    y = Pt.y();
    z = Pt.z();
    gp_Pnt P(x,y,z);

    Handle(GeomPlate_PointConstraint) PCont= new GeomPlate_PointConstraint(P,0);
    BPSurf.Add(PCont);

    //std::cout<<"x : "<<  x <<" y : "<<  y <<" z : "<<  z <<std::endl;

  }


  BPSurf.Perform();
  Standard_Integer MaxSeg = 8;
  Standard_Integer MaxDegree=3;
  //Standard_Integer CritOrder=4;
  Standard_Real dmax,Tol;
  Handle(GeomPlate_Surface) PSurf1 = BPSurf.Surface();
  dmax = Max(0.0001,10*BPSurf.G0Error());
  Tol=0.0;
  GeomPlate_MakeApprox Mapp1(PSurf1,Tol,MaxSeg,MaxDegree,dmax,0);
  Handle(Geom_BSplineSurface) Surf1 = Mapp1.Surface();

  return Surf1;



}