// Gnurbs - A curve and surface library
// Copyright (C) 2008-2026 Eric Bechet
//
// See the LICENSE file for contributions and license information.
// Please report all bugs and problems to <bechet@cadxfem.org>.
//

#include "nbspline.h"

#include <cmath>
#include <iostream>

void nbspline::P(double u_,npoint& ret) const
{
  curvepoint(u_,ret);
}

void nbspline::dP(double u_,npoint& p, npoint & dpdu) const
{
  std::vector<npoint> ders(2);
  dercurvepoint(u_,1,ders);
  p=ders[0];
  dpdu=ders[1];
}

void nbspline::d2P(double u_,npoint& p, npoint & dpdu, npoint & d2pdu2) const
{
  std::vector<npoint> ders(3);
  dercurvepoint(u_,2,ders);
  p=ders[0];
  dpdu=ders[1];
  d2pdu2=ders[2];
}

void nbspline::dPdu(double u_,npoint& ret) const
{
  dercurvepoint(u_,1,ret);
}

void nbspline::d2Pdu2(double u_,npoint& ret) const
{
  dercurvepoint(u_,2,ret);
}


int nbspline::findspan(double u) const
{
  int inter;
  if (u>=knots[nCP]) return nCP-1;
  if (u<=knots[deg]) return deg;
  int low=deg,high=nCP+1;
  int mid= (low+high) /2;
  while ((u<knots[mid-1]) || (u>= knots[mid]))
  {
    if (u<knots[mid-1]) high=mid;
    else low=mid;
    mid= (low+high) /2;
  }
  inter = mid-1;
  return inter;
}

int nbspline::basisfuns(double u,std::vector<double> &N) const
{
  std::vector<double> Nd(deg+1);
  int span=findspan(u);
  basisfuns(span,u,Nd);
  for (int i=0;i<=deg;++i)
    N[i]=Nd[i];
  return span-deg; // starting index of the d+1 shape functions relative to the cp list.
}

void nbspline::basisfuns(int i, double u,std::vector<double> &N) const
{
  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]=u-knots[i+1-j];
    right[j]=knots[i+j]-u;
    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;
  }
}

int nbspline::gradbasisfuns(double u,int nu,std::vector<double> &N) const
{
  std::vector<std::vector<double> > Nu(nu+1);
  for (int i=0;i<nu+1;++i) Nu[i].resize(deg+1);
  int span=findspan(u);
  gradbasisfuns(span,u,nu,Nu);
  for (int i=0;i<=deg;++i)
  {
    N[i]=Nu[nu][i];
  }
  return span-deg; // starting index of the d+1 shape functions relative to the cp list.
}


void nbspline::gradbasisfuns(int i, double u,int n,std::vector<std::vector<double> > &N) const
{
  std::vector<double> left(deg+1);
  std::vector<double> right(deg+1);
  std::vector<std::vector<double> > ndu(deg+1);
  for (int j=0;j<=deg;++j) ndu[j].resize(deg+1);
  std::vector<double> a[2];
  for (int j=0;j<2;++j) a[j].resize(deg+1);
  ndu[0][0]=1.0;
  for (int j=1;j<=deg;++j)
  {
    left[j]=u-knots[i+1-j];
    right[j]=knots[i+j]-u;
    double saved=0.0;
    for (int r=0;r<j;++r)
    {
      ndu[j][r] = right[r+1]+left[j-r];
      double temp=ndu[r][j-1]/ ndu[j][r];
      ndu[r][j]=saved+right[r+1]*temp;
      saved=left[j-r]*temp;
    }
    ndu[j][j]=saved;
  }
  for (int j=0;j<=deg;++j)
  {
    N[0][j]=ndu[j][deg];
  }
  int r;
  for (r=0;r<=deg;++r)
  {
    int s1=0,s2=1;
    a[0][0]=1.0;
    for (int k=1;k<=n;++k)
    {
      double d=0.0;
      int rk=r-k,pk=deg-k;
      if (r>=k)
      {
        a[s2][0]=a[s1][0]/ndu[pk+1][rk];
        d=a[s2][0]*ndu[rk][pk];
      }
      int j,j1,j2;
      if (rk>=-1) j1=1;
      else j1=-rk;
      if (r-1<=pk) j2=k-1;
      else j2=deg-r;
      for (j=j1;j<=j2;++j)
      {
        a[s2][j]=(a[s1][j]-a[s1][j-1])/ndu[pk+1][rk+j];
        d+=a[s2][j]*ndu[rk+j][pk];
      }
      if (r<=pk)
      {
        a[s2][k] = -a[s1][k-1]/ndu[pk+1][r];
        d+=a[s2][k]*ndu[r][pk];
      }
      N[k][r]=d;
      j=s1;s1=s2;s2=j;
    }
  }
  r=deg;
  for (int k=1;k<=n;++k)
  {
    for (int j=0;j<=deg;++j) N[k][j]*=r;
    r*=(deg-k);
  }
}


void nbspline::curvepoint(double u,npoint &C) const
{
  std::vector<double> N(deg+1);
  int span=findspan(u);
  basisfuns(span,u,N);
  for (int i=0;i<4;++i) C[i]=0.;
  for (int i=0;i<=deg;++i)
  {
    C=C+val[span-deg+i]*N[i];
  }
}

void nbspline::dercurvepoint(double u,int nu,npoint &C) const
{
  std::vector<std::vector<double> > N(nu+1);
  for (int i=0;i<nu+1;++i) N[i].resize(deg+1);
  int span=findspan(u);
  gradbasisfuns(span,u,nu,N);
  for (int i=0;i<4;++i) C[i]=0.;
  for (int i=0;i<=deg;++i)
  {
    C=C+val[span-deg+i]*N[nu][i];
  }
}

void nbspline::dercurvepoint(double u,int nu,std::vector<npoint> &dp) const
{
  std::vector<std::vector<double> > N(nu+1);
  for (int i=0;i<nu+1;++i) N[i].resize(deg+1);
  dp.resize(nu+1);
  int span=findspan(u);
  gradbasisfuns(span,u,nu,N);
  for (int j=0;j<nu+1;++j)
    for (int i=0;i<4;++i) {dp[j][i]=0.;}
  for (int j=0;j<nu+1;++j)
  {
    for (int i=0;i<=deg;++i)
    {
      dp[j]=dp[j]+val[span-deg+i]*N[j][i];
    }
  }
}


void nbspline::findspanmult(double u,int &k,int &s)
{
  s=0;
  if (u>=knots[nCP])
  {
    k=nCP;
    while (knots[nCP]==knots[k+s])
    {
      s++;
      if (s>deg) break;
    }
    return;
  }
  if (u<=knots[deg])
  {
    k=deg;
    while (knots[deg]==knots[k-s])
    {
      s++;
      if (s>deg) break;
    }
    return;
  }
  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;
  }
  k=mid;
  while (u==knots[k-s])
  {
    s++;
    if (s>deg) break;
  }
}


void nbspline::insertknot(double u,int r)
{
  int k,s;
  findspanmult(u,k,s);
  if ((r+s) >deg) r=deg-s;
  if (r>0)
  {
    insertknot_(u,k,s,r);
  }
}

void nbspline::insertknot_(double u,int k,int s,int r)
{
  std::vector<double> newknots(nknots+r);
  std::vector<npoint> newval(nCP+r);
  int newnCP=nCP+r;
  int newnknots=nknots+r;
  int mp=nCP+deg;

  std::vector<npoint> R(nCP+r);
  for (int i=0;i<=k;++i) newknots[i]=knots[i];
  for (int i=1;i<=r;++i) newknots[i+k]=u;
  for (int i=k+1;i<=mp;++i) newknots[i+r]=knots[i];

  for (int i=0;i<=k-deg;++i) newval[i]=val[i];
  for (int i=k-s;i<nCP;++i) newval[i+r]=val[i];
  for (int i=0;i<=deg-s;++i) R[i]=val[k-deg+i];
  int L=0;
  for (int j=1;j<=r;++j)
  {
    L=k-deg+j;
    for (int i=0;i<=deg-j-s;++i)
    {
      double alpha= (u-knots[L+i]) / (knots[i+k+1]-knots[L+i]);
      R[i]=R[i+1]*alpha+R[i]* (1.0-alpha);
    }
    newval[L]=R[0];
    newval[k+r-j-s]=R[deg-j-s];
  }
  for (int i=L+1;i<k-s;++i)
  {
    newval[i]=R[i-L];
  }
  knots=newknots;
  val=newval;
  nCP=newnCP;
  nknots=newnknots;
}

int nbspline::deleteknot(int k, int s,double TOL)
{
  // search real index k (end of repetition sequence)
  if (k<0||k>=nknots) return 0;
  double u=knots[k];
  int r=1;
  int k1=k;
  int k2=k;
  while ((++k1)<nknots) if (knots[k1]!=u) break; k1--;
  while ((--k2)>=0) if (knots[k2]!=u) break; k2++;
  r=k1-k2+1;
  k=k1;
  if (k<=deg||k>=nknots-deg-1) return 0;
  if (s>r) s=r;
  int cnt=0;
  for (int t=0;t<s;++t)
  {
     // now compute the series of new CP 
    int i=k-deg;
    int j=k-r;
    int ii=1;
    int jj=j-i+1;
    std::vector<npoint> nval((j-i)+3);
    nval[0]=val[k-deg-1];
    nval[j-i+2]=val[j+1];
    while (j-i>0)
    {
      double alphai=(u-knots[i])/(knots[i+deg+1]-knots[i]);
      double alphaj=(u-knots[j])/(knots[j+deg+1]-knots[j]);
      nval[ii]=(val[i]-(1.0-alphai)*nval[ii-1])/alphai;
      nval[jj]=(val[j]-alphaj*nval[jj+1])/(1.0-alphaj);
      ++i;++ii;
      --j;--jj;
    }
    //check if the curve is changed or not
    double err=1.0;
    if (i==j)
    {
      double alphai=(u-knots[i])/(knots[i+deg+1]-knots[i]);
      err =(alphai*nval[ii+1]+(1.0-alphai)*nval[ii-1]-val[i]).norm();
    }
    else
    {
      err =(nval[ii-1]-nval[jj+1]).norm();
    }
    if ((err<TOL)||(TOL < 0.0))
    {
      i=k-deg;
      j=k-r;
      while (j-i>0)
      {
        val[i]=nval[i-k+deg+1];
        val[j]=nval[j-k+deg+1];
        ++i;
        --j;
      }
      knots.erase(knots.begin()+k);
      val.erase(val.begin()+j);
      nCP--;
      nknots--;
      k--;
      r--;
      cnt++;
    }
    else
      break;
  }
  return cnt;
}



void nbspline::extend(double u)
{
  std::vector<std::vector<npoint> > M(deg+1);
  for (int i=0;i<deg+1;++i) M[i].resize(deg+1);

  std::vector<double> newknots(nknots);
  std::vector<npoint> newval(nCP);

  for (int i=0;i<=deg;++i) newknots[i]=u;
  for (int i=deg+1;i<nCP+deg;++i) newknots[i]=knots[i];
  for (int i=0;i<=deg;++i)
  {
    M[deg][deg-i]=val[i];
  }
  for (int j=deg;j>0;--j)
    for (int k=j;k>0;--k)
    {
      M[j-1][k-1]= (M[j][k]* (newknots[j+deg+1-k]-newknots[j]) +M[j][k-1]* (newknots[j]-knots[deg])) * (1/ (newknots[j+deg+1-k]-knots[deg]));
    }
  for (int i=0;i<nCP;++i)
  {
    newval[i]=val[i];
  }
  for (int i=0;i<=deg;++i)
  {
    newval[i]=M[i][0];
  }
  knots=newknots;
  val=newval;
}

void nbspline::saturate()
{
  int curn=deg;
  double curk;
  while (curn< (nknots-deg))
  {
    curk=knots[curn];
    insertknot(curk,deg);
    while ((knots[curn]==curk) && (curn< (nknots-deg))) curn++;
  }
}

int nbspline::clean(double TOL)
{
  int k=deg;
  int tot=0;
  double u=knots[k];
  while (k<nknots-deg-1)
  {
    if (u==knots[k]) ++k;
    else
    {
      tot+=deleteknot(k,deg,TOL);
      u=knots[k];
    }
  }
  return tot;
}

void nbspline::degree_elevation()
{
  saturate();
  nbspline buf;
  buf.knots.reserve(nb_knots() + (nb_knots()-2) /degree());
  buf.val.reserve(nb_CP() + (nb_CP()-1) /degree());
  buf.knots.resize(0);
  buf.val.resize(0);
  int i=0;
  for (;i<nb_knots()-1;++i)
  {
    buf.knots.push_back(knots[i]);
    if ((knots[i]-knots[i+1]) !=0) buf.knots.push_back(knots[i]);
  }
  buf.knots.push_back(knots[i]);
  buf.knots.push_back(knots[i]);
  i=0;
  buf.val.push_back(val[i]);
  ++i;
  while (i<nb_CP())
  {
    for (int j=1;j<=deg;++j)
    {
      buf.val.push_back(val[i-1]+ ((double)(deg+1.-j) / (deg+1.)) * (val[i]-val[i-1]));
      ++i;
    }
    buf.val.push_back(val[i-1]);
  }
  buf.deg=deg+1;
  buf.nknots=buf.knots.size();
  buf.nCP=buf.val.size();
  buf.clean();
  *this=buf;
}
void nbspline::recursivedivide(nbspline &bs,int deep) const
{
  bs=*this;
  bs.saturate();
  for (int i=0;i<deep;++i)
  {
    double prev=bs.knots[bs.deg];
    for (int j=bs.deg+1;j<bs.nknots-deg;++j)
    {
      if (bs.knots[j]>prev)
      {
        double buf=prev;
        prev=bs.knots[j];
        bs.insertknot((bs.knots[j]+buf) /2,bs.deg);
      }
    }
  }
}


// (m+1 knots)

double nbspline::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;
}


void nbspline::initknots(double start,double end)
{
  double span=(end-start)/(nknots-2*deg-1);
  int i=0,j=1;
  for (;i<deg+1;++i)
    u(i)=start;
  for (;i<nknots-deg-1;++i,++j)
    u(i)=start+span*j;
  for (;i<nknots;++i)
    u(i)=end;
}