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

#ifdef ENABLE_CLOTHOID
#include <iostream>
#include <fstream>
#include <cmath>
#include <cstdlib>

void nrspline::P(double u_, npoint& ret) const
{
  double posbar=0;
  int posi;
  int n=nb_CP();
  for (posi=0;posi<n-1;++posi)
  {
    posbar= (u_-u(posi)) / (u(posi+1)-u(posi));
    if ((posbar>=0.0) && (posbar<=1.0)) break;
  }
  if (posi==n-1) posi--;
  double pos=u_-u(posi);
  segments[posi].P(pos,ret);
}


void nrspline::compute_FD(void)
{
  type=FiniteDifferences;
  for(int i=0;i<nb_CP();++i)
  {
    npoint der;
    if (i==0)
    {
      if (periodic)
        der=CP(i+1)-CP(nb_CP()-2);
      else
        der=CP(i+1)-CP(i);
    }
    else if (i==nb_CP()-1)
    {
      if (periodic)
        der=CP(1)-CP(i-1);
      else
        der=CP(i)-CP(i-1);
    }
    else der=CP(i+1)-CP(i-1);
    th[i]=atan2(der[1],der[0]);
  }
  compute_segments();
}

void nrspline::compute_cardinal(double c) // same as FD
{
  card=c;
  type=Cardinal;
  compute_FD();
}


void nrspline::compute_natural(void)
{
  compute_FD();
  type=Natural;
  int maxiter=40;
  double maxerr=1e-12;
  
  int niter=0;

  double err=1e6;

  std::vector<double> curvgaps(nb_CP());
  std::vector<double> dcurvgaps(nb_CP());
  std::vector<double> dslope(nb_CP());
  
//  std::cout << "errinit ";
  err=get_curvature_gaps(curvgaps);
//  std::cout << err << std::endl;
  
  while ((err>maxerr)&&(niter++<maxiter))
  {
    get_curvature_der(dcurvgaps);
    for(int i=0;i<nb_CP();++i)
    {
      dslope[i]=-curvgaps[i]/dcurvgaps[i];// approximate jacobian (diagonal)
    }
    add_to_slopes(dslope);
    compute_segments();
    err=get_curvature_gaps(curvgaps);
  }
  if (err>maxerr) std::cout << "not converged for G2 err="<<  err << std::endl;
}

void nrspline::set_slopes(std::vector<double> &slopes)
{
  for(int i=0;i<nb_CP();++i)
    th[i]=slopes[i];
}

void nrspline::add_to_slopes(std::vector<double> &dslopes)
{
  for(int i=0;i<nb_CP();++i)
  {
    th[i]+=dslopes[i];
    if (th[i]>M_PI) th[i]-=2*M_PI;
    if (th[i]<M_PI) th[i]+=2*M_PI;
  }
}

void nrspline::compute_segments()
{
  double s=0;
  u(0)=0;
  for(int i=0;i<nb_CP()-1;++i)
  {
    segments[i]=nclothoid(CP(i),th[i],CP(i+1),th[i+1]);
    s+=segments[i].max_u();
    u(i+1)=s;
  }
}

void nrspline::print()
{
  for(int i=0;i<nb_CP()-1;++i)
    std::cout << CP(i)<< " "  << th[i] << " " << CP(i+1) <<  " " << th[i+1] << std::endl;
}

void nrspline::Display(data_container& data) const    // default version : uses P(u)
{
  properties p,p1;
  p=data.getproppoints();
  p1=p;
  if (p.nb_uid<2) p.nb_uid=2;
  for (int i=0;i< nb_CP();i++)
  {
    p.uids[1]=i;
    data.setproppoints(p);
    data.add_point(CP(i));
  }
  data.setproppoints(p1);
  for (int i=0;i<nb_CP()-1;++i) segments[i].Display(data);
}

double nrspline::get_curvature_gaps(std::vector<double> &curv)
{
  double err=0.0;
  for(int i=0;i<nb_CP();++i)
  {
    double delt;
    if (i==0) 
    {
      if (periodic)
        delt=-segments[nb_CP()-2].Kappa(segments[nb_CP()-2].max_u())+segments[i].Kappa(0);
      else
        delt=segments[i].Kappa(0);
    }
    else if (i==nb_CP()-1)
    {
      if (periodic)
        delt=-segments[i-1].Kappa(segments[i-1].max_u())+segments[0].Kappa(0);
      else
        delt=-segments[i-1].Kappa(segments[i-1].max_u());
    }
    else delt=-segments[i-1].Kappa(segments[i-1].max_u())+segments[i].Kappa(0);
    curv[i]=delt;
    err+=fabs(delt);
  }
  return err;
}


void nrspline::get_curvature_der(std::vector<double> &curv) // approximate jacobian (diagonal)
{
  double delta=1e-5;
  for(int i=0;i<nb_CP();++i)
  {
    double delt=0;
    if (i==0)
    {
      if (periodic)
      {
        nclothoid np0(CP(nb_CP()-2),th[nb_CP()-2],CP(nb_CP()-1),th[nb_CP()-1]+delta);
        nclothoid np1(CP(i),th[i]+delta,CP(i+1),th[i+1]);
        delt=np1.Kappa(0)-np0.Kappa(np0.max_u());
        delt-=-segments[nb_CP()-2].Kappa(segments[nb_CP()-2].max_u())+segments[i].Kappa(0);
      }
      else
      {
        nclothoid np(CP(i),th[i]+delta,CP(i+1),th[i+1]);
        delt=np.Kappa(0);
        delt-=segments[i].Kappa(0);
      }
    }
    else if (i==nb_CP()-1)
    {
      if (periodic)
      {
        nclothoid np0(CP(i-1),th[i-1],CP(i),th[i]+delta);
        nclothoid np1(CP(0),th[0]+delta,CP(1),th[1]);
        delt=np1.Kappa(0)-np0.Kappa(np0.max_u());
        delt-=-segments[i-1].Kappa(segments[i-1].max_u())+segments[0].Kappa(0);
      }
      else
      {
        nclothoid np(CP(i-1),th[i-1],CP(i),th[i]+delta);
        delt=-np.Kappa(np.max_u());
        delt-=-segments[i-1].Kappa(segments[i-1].max_u());
      }
    }
    else 
    {
      nclothoid np0(CP(i-1),th[i-1],CP(i),th[i]+delta);
      nclothoid np1(CP(i),th[i]+delta,CP(i+1),th[i+1]);
      delt=np1.Kappa(0)-np0.Kappa(np0.max_u());
      delt-=-segments[i-1].Kappa(segments[i-1].max_u())+segments[i].Kappa(0);
    }
    curv[i]=delt/delta;
  }
}

void nrspline::update()
{
  switch (type)
  {
    case FiniteDifferences :
      compute_FD();
      break;
    case Cardinal :
      compute_cardinal(card);
      break;
    case Natural :
      compute_natural();
      break;
  }
}
#endif //ENABLE_CLOTHOID