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



///**************************** Find Span **************************************************************
int Spline::findspan(const std::vector<double> &knots, int deg,double u)
{


  int nCP=knots.size()-deg-1;
  if (u>=knots[nCP]) return nCP-1;
  if (u<=knots[deg]) return deg;

  int low=deg,high=nCP;
  int mid= (low+high) /2;
  while ((u<knots[mid]) || (u>= knots[mid+1]))

  {
    if (u<knots[mid]) high=mid;
    else low=mid;

    mid= (low+high) /2;
  }

  return mid;

}
///*******************************************************************************************************
void Spline::basisfuns(const std::vector<double> &knots,int deg, int i, double para, std::vector<double> &N)
{
  N[0]=1.0;
  std::vector<double> left(deg+1);
  std::vector<double> right(deg+1);
  for (int j=1;j<=deg;++j)
  {
    left[j]=para-knots[i+1-j];
    right[j]=knots[i+j]-para;
    double saved=0.0;
    for (int r=0;r<j;++r)
    {
      double temp=N[r]/ (right[r+1]+left[j-r]);
      N[r]=saved+right[r+1]*temp;
      saved=left[j-r]*temp;
    }
    N[j]=saved;
  }
}
///*********************************************************************************************************

std::vector<double> Spline::chord_length(const std::vector<gp_Pnt > Pnts)
{

  std::vector<double> Uk(Pnts.size());

  double d =0;
  for (int i=1; i<=Pnts.size()-1; i++)
  {
    d = d + Pnts[i].Distance(Pnts[i-1]);

  }

  Uk[0] = 0.0;

  for (int i=1; i<Pnts.size()-1; i++)
  {
    Uk[i] = Uk[i-1] + (Pnts[i].Distance(Pnts[i-1])) /d;

  }

  Uk[Pnts.size()-1] = 1.0;

  return Uk;

}

std::vector<double> Spline::centripetal(const std::vector<gp_Pnt > Pnts)
{

  std::vector<double> Uk(Pnts.size());

  double d =0;
  for (int i=1; i<=Pnts.size()-1; i++)
  {
    d = d + sqrt(Pnts[i].Distance(Pnts[i-1]));

  }

  Uk[0] = 0.0;

  for (int i=1; i<Pnts.size()-1; i++)
  {
    Uk[i] = Uk[i-1] + sqrt(Pnts[i].Distance(Pnts[i-1])) /d;

  }

  Uk[Pnts.size()-1] = 1.0;

  return Uk;

}

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

std::vector<double> Spline::equally_spaced(const std::vector<double > Uk, int deg)
{
  std::vector<double> Knots(Uk.size() +deg+1);

  for (int i=0; i<=deg; i++)
  {
    Knots[i]=0.0;
  }

  for (int j=1; j<= (Uk.size()-1)-deg; j++)
  {
    Knots[j+deg]=j/ (Uk.size()-deg);
  }

  for (int i=0; i<=deg; i++)
  {
    Knots[(Knots.size()-1)-deg +i]=1.0;
  }
  return Knots;
}

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

std::vector<double> Spline::averaging(const std::vector<double > Uk, int deg)
{
  std::vector<double> Knots(Uk.size() +deg+1);

  for (int i=0; i<=deg; i++)
  {
    Knots[i]=0.0;
  }

  for (int j=1; j<= (Uk.size()-1)-deg; j++)
  {
    double S=0;
    for (int i=j; i<=j+deg-1; i++)
    {
      S = S + Uk[i];
    }
    Knots[j+deg]=S/deg;
  }

  for (int i=0; i<=deg; i++)
  {
    Knots[(Knots.size()-1)-deg +i]=1.0;
  }


  return Knots;
}

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

void Spline::OCCKnot_multiplicity(const std::vector<double > Knots, std::vector<double > &KnotsOCC, std::vector<int > &Multiples)
{
  int mult = 0;

  for (int i=1; i<=Knots.size()-1; i++)
  {
    mult=mult+1;

    if (Knots[i]>Knots[i-1])
    {
      KnotsOCC.push_back(Knots[i-1]);
      Multiples.push_back(mult);
      mult = 0;
    }

    if (i==Knots.size()-1)
    {
      KnotsOCC.push_back(Knots[i]);
      Multiples.push_back(mult+1);
    }
  }

}

///***************************************************************************************
///  Classe CurveSpline      ///  The NURBS Book
///***************************************************************************************

void CurveSpline::GlobalCurveInterp(std::vector<gp_Pnt > Pnts, int &deg, std::vector<double > &Knots, std::vector<gp_Pnt > &CPnts, int Interp_method)
{
  if (deg>=Pnts.size())
  {
    do
    {
      deg=deg-1;

    }
    while (deg>=Pnts.size());

  }

  std::vector<double> Uk;
  Spline S;
  if (Interp_method == 0) Uk = S.chord_length(Pnts);
  if (Interp_method == 1) Uk = S.centripetal(Pnts);

  Knots = S.averaging(Uk, deg);

  math_Matrix A(0, Pnts.size()-1, 0, Pnts.size()-1, 0.0);

  for (int i=0; i<=Pnts.size()-1; i++)
  {
    int span = S.findspan(Knots, deg, Uk[i]);
    std::vector<double> N(deg+1);
    S.basisfuns(Knots, deg, span, Uk[i], N);

    for (int j=span-deg; j<=span; j++)
    {
      A(i,j) =N[j- (span-deg)];

    }

  }

  math_GaussLeastSquare LU_Solve(A, 1.0e-20);

  math_Vector Qx(0,Pnts.size()-1,0.0);
  math_Vector Qy(0,Pnts.size()-1,0.0);
  math_Vector Qz(0,Pnts.size()-1,0.0);

  math_Vector Px(0,Pnts.size()-1,0.0);
  math_Vector Py(0,Pnts.size()-1,0.0);
  math_Vector Pz(0,Pnts.size()-1,0.0);


  for (int i=0; i<=Pnts.size()-1; i++)
  {
    Qx(i) =Pnts[i].X();
    Qy(i) =Pnts[i].Y();
    Qz(i) =Pnts[i].Z();
  }

  LU_Solve.Solve(Qx,Px);
  LU_Solve.Solve(Qy,Py);
  LU_Solve.Solve(Qz,Pz);

  for (int i=0; i<=Pnts.size()-1; i++)
  {
    CPnts.push_back(gp_Pnt(Px(i), Py(i), Pz(i)));

  }



}

///***************************************************************************************
///  Classe SurfaceSpline        ///  The NURBS Book
///***************************************************************************************

void SurfaceSpline::SurfMeshParams(std::vector<std::vector<gp_Pnt > > Qkl, std::vector<double > &Uk, std::vector<double > &Vl)
{
  int n = Qkl.size() - 1;
  int m = Qkl[0].size() - 1;

  gp_Pnt Q[n+1][m+1];
  for (int k=0; k<=n; k++)
  {
    std::vector<gp_Pnt > Qk = Qkl[k];
    for (int l=0; l<=m; l++)
    {
      Q[k][l]= Qk[l];
    }
  }

  int num = m+1;

  Uk[0] = 0.0;
  Uk[n] = 1.0;
  for (int k=1; k<n; k++) Uk[k] = 0.0;

  for (int l=0; l<=m; l++)
  {
    double total = 0.0;
    std::vector<double > cds(n);
    for (int k=1; k<=n; k++)
    {
      cds[k-1] = (Q[k][l]).Distance(Q[k-1][l]);
      total = total + cds[k-1];
    }
    if (total ==0.0) num = num-1;
    else
    {
      double d = 0.0;
      for (int k=1; k<n; k++)
      {
        d=d+cds[k-1];
        Uk[k] = Uk[k]+ d/total;

      }
    }
  }

  if (num==0) return;
  for (int k=1; k<n; k++) Uk[k] = Uk[k]/num;

  num = n+1;

  Vl[0] = 0.0;
  Vl[m] = 1.0;
  for (int l=1; l<m; l++) Vl[l] = 0.0;

  for (int k=0; k<=n; k++)
  {
    double total = 0.0;
    std::vector<double > cds(m);
    for (int l=1; l<=m; l++)
    {
      cds[l-1] = (Q[k][l]).Distance(Q[k][l-1]);
      total = total + cds[l-1];
    }
    if (total ==0.0) num = num-1;
    else
    {
      double d = 0.0;
      for (int l=1; l<m; l++)
      {
        d=d+cds[l-1];
        Vl[l] = Vl[l]+ d/total;

      }
    }
  }

  if (num==0) return;
  for (int l=1; l<m; l++) Vl[l] = Vl[l]/num;
}

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

void SurfaceSpline::GlobalSurfInterp(std::vector<std::vector<gp_Pnt > > Qkl, int &degU, int &degV, std::vector<std::vector<gp_Pnt > > &Pij, std::vector<double > &KnotsU, std::vector<double > &KnotsV)
{
  int n = Qkl.size() - 1;
  int m = Qkl[0].size() - 1;

  if (degU>=n+1)
  {
    do
    {
      degU=degU-1;

    }
    while (degU>=n+1);

  }
  if (degV>=m+1)
  {
    do
    {
      degV=degV-1;

    }
    while (degV>=m+1);

  }

  std::vector<double > Uk(n+1);
  std::vector<double > Vl(m+1);

  SurfMeshParams(Qkl, Uk, Vl);

  Spline S;
  KnotsU = S.averaging(Uk, degU);
  KnotsV = S.averaging(Vl, degV);

  gp_Pnt Q[n+1][m+1];
  for (int k=0; k<=n; k++)
  {
    std::vector<gp_Pnt > Qk = Qkl[k];
    for (int l=0; l<=m; l++)
    {
      Q[k][l]= Qk[l];
    }
  }

  CurveSpline CS;

  gp_Pnt Rkl[n+1][m+1];

  for (int l=0; l<=m; l++)
  {
    std::vector<gp_Pnt > Pnts(n+1);
    for (int k=0; k<=n; k++) Pnts[k]=Q[k][l];

    std::vector<double > Knots;
    std::vector<gp_Pnt > R_kl;

    CS.GlobalCurveInterp(Pnts, degU, Knots, R_kl);

    for (int k=0; k<=n; k++) Rkl[k][l]=R_kl[k];

  }

  gp_Pnt P_ij[n+1][m+1];

  for (int k=0; k<=n; k++)
  {
    std::vector<gp_Pnt > Pnts(m+1);
    for (int l=0; l<=m; l++) Pnts[l]=Rkl[k][l];

    std::vector<double > Knots;
    std::vector<gp_Pnt > R_ij;

    CS.GlobalCurveInterp(Pnts, degV, Knots, R_ij);

    for (int l=0; l<=m; l++) P_ij[k][l]=R_ij[l];
  }

  for (int k=0; k<=n; k++)
  {
    std::vector<gp_Pnt > PPij(m+1);
    for (int l=0; l<=m; l++)
    {
      PPij[l] = P_ij[k][l];
    }

    Pij.push_back(PPij);
  }

}