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

#ifdef ENABLE_CLOTHOID

nclothoid::nclothoid(npoint2 P0,double theta0,npoint2 P1,double theta1) : Cl(P0.array(),theta0,P1.array(),theta1)
{
}

/// \brief Minimum allowed value of the parameter.
/// \return Minimum allowed value of the parameter.
double nclothoid::min_u(void) const
{
  return 0;
}

/// \brief Maximum allowed value of the parameter.
/// \return Maximum allowed value of the parameter.
double nclothoid::max_u(void) const
{
  return Cl.length();
}

/// \brief Point along the curve for parameter u.
/// \param[in] u parameter.
/// \param[out] ret pClothoids_linux_staticoint corresponding to parameter u.
void nclothoid::P(double u,npoint& ret) const
{
  ret[0]=Cl.X(u);
  ret[1]=Cl.Y(u);
  ret[2]=0;
  ret[3]=1;
}

/// \brief First derivative - generic numerical implementation.
/// \param[in] u_ parameter.
/// \param[out] ret first derivative at parameter u_.
void nclothoid::dPdu(double u_,npoint& ret) const
{
//  std::cout<< Cl.theta(u_) << std::endl;
  double th=Cl.theta(u_);
  ret[0]=cos(th);
  ret[1]=sin(th);
  ret[2]=0;
  ret[3]=0;
}

/// \brief Second derivative, generic numerical implementation.
/// \param[in] u_ parameter.
/// \param[out] ret second derivative at parameter u_.
void nclothoid::d2Pdu2(double u_,npoint& ret) const
{
  double th=Cl.theta(u_);
  double kap=Cl.kappa(u_);
  ret[0]=sin(th)*kap;
  ret[1]=-cos(th)*kap;
  ret[2]=0;
  ret[3]=0;
}

/// \brief Second derivative (one over radius of curvature).
/// \param[in] u_ parameter.
/// \return Second derivative.
double nclothoid::Kappa(double u_) const
{
  return Cl.kappa(u_);
}

#endif //ENABLE_CLOTHOID