/*
Copyright (c) 2004-2010, Dirk Krause
All rights reserved.

Redistribution and use in source and binary forms,
with or without modification, are permitted provided
that the following conditions are met:

* Redistributions of source code must retain the above
  copyright notice, this list of conditions and the
  following disclaimer.
* Redistributions in binary form must reproduce the above 
  opyright notice, this list of conditions and the following
  disclaimer in the documentation and/or other materials
  provided with the distribution.
* Neither the name of the Dirk Krause nor the names of
  contributors may be used to endorse or promote
  products derived from this software without specific
  prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED.
IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
DAMAGE.
*/



/**	@file	dkxsp.c	X-spline calculations module.
*/



/** Inside the dkxsp module. */
#define DK_XSP_C	1



#include "dkxsp.h"

#include <dk.h>

#if DK_HAVE_MATH_H
#include <math.h>
#endif

#include <dkma.h>



$(trace-include)



#ifndef DKFIG_EPSILON
/** Epsilon for calculation. */
#define DKFIG_EPSILON 0.0001
#endif



/**	Calculation f(u).
	@param	u	The u-value.
	@param	p	The p coefficient.
	@return	The function result.
*/
static
double
f DK_P2(double,u,double,p)
{
  double back;
  double uu, uuu, uuuu, uuuuu;
  $? "+ f %lf %lf", u, p
  uu = u * u; uuu = uu * u; uuuu = uu * uu; uuuuu = uuuu * u;
  /* 2004/04/08 bug fixed: uuuu was used instead of uuuuu */
  back = (6.0-p)*uuuuu+(2.0*p-15.0)*uuuu+(10.0-p)*uuu;
  $? "- f(%lg,%lg)=%lg", u, p, back
  return back;
}



/**	Calculation g(u).
	@param	u	The u-value.
	@param	p	The p coefficient.
	@param	q	The q coefficient.
	@return	The function result.
*/
static
double
g DK_P3(double,u,double,p,double,q)
{
  double back;
  double uu, uuu, uuuu, uuuuu;
  $? "+ g u=%lf   p=%lf   q=%lf", u, p, q
  uu = u * u; uuu = uu * u; uuuu = uuu * u; uuuuu = uuuu * u;
  back = q*u+2.0*q*uu+(10.0-12.0*q-p)*uuu
         + ( 2.0*p + 14.0*q - 15.0 )*uuuu + ( 6.0 - 5.0*q - p )*uuuuu;
  $? "- g %lf", back
  return back;
}



/**	Calculation h(u).
	@param	u	The u-value.
	@param	q	The q coefficient.
	@return	The function result.
*/
static
double
h DK_P2(double,u,double,q)
{
  double back;
  double uu, uuu, uuuu, uuuuu;
  $? "+ h   u=%lf   q=%lf", u, q
  uu = u * u; uuu = uu * u; uuuu = uuu * u; uuuuu = uuuu * u;
  back = q*u+2.0*q*uu-2.0*q*uuuu-q*uuuuu;
  $? "- h %lf", back
  return back;
}



/**	Calculation df/du.
	@param	u	The u-value.
	@param	p	The p coefficient.
	@return	The calculation result.
*/
static
double
dfdu DK_P2(double,u,double,p)
{
  double back;
  double uu, uuu, uuuu;
  uu = u * u; uuu = uu * u; uuuu = uu * uu;
  $? "+ dfdu   u=%lf   p=%lf", u, p
  back = 5.0*(6.0-p)*uuuu+4.0*(2.0*p-15.0)*uuu+3.0*(10.0-p)*uu;
  $? "- dfdu(%lg,%lg)=%lg", u,p,back
  return back;
}



/**	Calculation dg/du.
	@param	u	The u-value.
	@param	p	The p coefficient.
	@param	q	The q coefficient.
	@return	The calculation result.
*/
static
double
dgdu DK_P3(double,u,double,p,double,q)
{
  double back;
  double uu, uuu, uuuu;
  uu = u * u; uuu = uu * u; uuuu = uuu * u;
  $? "+ dgdu   u=%lf   p=%lf   q=%lf", u, p, q
  back = q+4.0*q*u+3.0*(10.0-12.0*q-p)*uu+4.0*(2.0*p+14.0*q-15.0)*uuu
         +5.0*(6.0-5.0*q-p)*uuuu;
  $? "- dgdu %lf", back
  return back;
}



/**	Calculation dh/du.
	@param	u	The u-value.
	@param	q	The q coefficient.
	@return	The calculation result.
*/
static
double
dhdu DK_P2(double,u,double,q)
{
  double back, uu, uuu, uuuu;
  uu = u * u; uuu = uu * u; uuuu = uuu * u;
  $? "+ dhdu   u=%lf   q=%lf", u, q
  back = q+4.0*q*u-8.0*q*uuu-5.0*q*uuuu;
  $? "- dhdu %lf", back
  return back;
}



/**	Set values for point A.
	@param	s	X-spline calculation structure.
	@param	sk	s-value.
	@param	x	x-value.
	@param	y	y-value.
*/
void
dkxsp_setA DK_P4(dk_xspline_t *,s,double,sk,double,x,double,y)
{
  if(s) {
    s->usea = 1; s->sa = sk; s->xa = x; s->ya = y;
  }
  $? "= dkxsp_setA %s sk=%lf x=%lf y=%lf", TR_PTR(s),sk,x,y
}



/**	Set values for point B.
	@param	s	X-spline calculation structure.
	@param	sk	s-value.
	@param	x	x-value.
	@param	y	y-value.
*/
void
dkxsp_setB DK_P4(dk_xspline_t *,s,double,sk,double,x,double,y)
{
  if(s) {
    s->useb = 1; s->sb = sk; s->xb = x; s->yb = y;
  }
  $? "= dkxsp_setB %s sk=%lf x=%lf y=%lf", TR_PTR(s),sk,x,y
}



/**	Set values for point C.
	@param	s	X-spline calculation structure.
	@param	sk	s-value.
	@param	x	x-value.
	@param	y	y-value.
*/
void
dkxsp_setC DK_P4(dk_xspline_t *,s,double,sk,double,x,double,y)
{
  if(s) {
    s->usec = 1; s->sc = sk; s->xc = x; s->yc = y;
  }
  $? "= dkxsp_setC %s sk=%lf x=%lf y=%lf", TR_PTR(s),sk,x,y
}



/**	Set values for point D.
	@param	s	X-spline calculation structure.
	@param	sk	s-value.
	@param	x	x-value.
	@param	y	y-value.
*/
void
dkxsp_setD DK_P4(dk_xspline_t *,s,double,sk,double,x,double,y)
{
  if(s) {
    s->used = 1; s->sd = sk; s->xd = x; s->yd = y;
  }
  $? "= dkxsp_setD %s sk=%lf x=%lf y=%lf", TR_PTR(s),sk,x,y
}


/**	Reset all components.
	@param	s	X-spline calculation structure to reset.
*/
void
dkxsp_reset DK_P1(dk_xspline_t *,s)
{
  $? "+ dkxsp_reset"
  if(s) {
    s->usea = s->useb = s->usec = s->used = 0;
    s->gha = s->ghb = s->ghc = s->ghd = 0;
    s->sa = s->sb = s->sc = s->sd = 0.0;
    s->Ta = 0.0;
    s->Tb = 1.0;
    s->Tc = 0.0;
    s->Td = 1.0;
    s->pa = s->pb = s->pc = s->pd = 2.0;
    s->qa = s->qb = s->qc = s->qd = 0.0;
    s->xa = s->xb = s->xc = s->xd = 0.0;
    s->ya = s->yb = s->yc = s->yd = 0.0;
    s->dudta = s->dudtb = -1.0;
    s->dudtc = s->dudtd =  1.0;
    s->t = 0.0;
    s->x = s->y = s->ddtx = s->ddty = 0.0;
  }
  $? "- dkxsp_reset"
}



/**	Step 1: Calculations with no t involved.
	- Check where to interpolate or to approximate.
	- Calculate coefficients for control functions.
	@param	s	X-spline calculation structure.
*/
void
dkxsp_step1 DK_P1(dk_xspline_t *,s)
{
  double res;
  $? "+ dkxsp_step1"
  if(s) {
    if(s->sb < 0.0) {
      if(s->usea) s->gha = 1;
      if(s->usec) s->ghc = 1;
    }
    if(s->sc < 0.0) {
      if(s->useb) s->ghb = 1;
      if(s->used) s->ghd = 1;
    }
    if(s->usea) {
      if(s->gha) {
        s->qa = -0.5 * s->sb;
      } else {
        s->Ta = s->sb;
	s->pa = 2.0 * (1.0 + s->Ta) * (1.0 + s->Ta);
	s->dudta = -1.0 / (1.0 + s->Ta);
      }
    }
    if(s->useb) {	$? ". use b"
      if(s->ghb) {	$? ". gh for b"
        s->qb = -0.5 * s->sc;
      } else {		$? ". f for b"
        s->Tb = 1.0 + s->sc;
	s->pb = 2.0 * (s->Tb) * (s->Tb);
	s->dudtb = -1.0 / (s->Tb);
      }
    }
    if(s->usec) {
      if(s->ghc) {
        s->qc = -0.5 * s->sb;
      } else {
        s->Tc = 0.0 - s->sb;
	s->pc = 2.0 * (1.0 - s->Tc) * (1.0 - s->Tc);
	s->dudtc = 1.0 / (1.0 - s->Tc);
      }
    }
    if(s->used) {
      if(s->ghd) {
        s->qd = -0.5 * s->sc;
      } else {
        s->Td = 1.0 - s->sc;
	s->pd = 2.0 * (2.0 - s->Td) * (2.0 - s->Td);
	s->dudtd = 1.0 / (2.0 - s->Td);
      }
    }
    $? ". x     A=%g   B=%g   C=%g   D=%g", s->xa, s->xb, s->xc, s->xd
    $? ". y     A=%g   B=%g   C=%g   D=%g", s->ya, s->yb, s->yc, s->yd
    $? ". s     A=%g   B=%g   C=%g   D=%g", s->sa, s->sb, s->sc, s->sd
    $? ". use   A=%d   B=%d   C=%d   D=%d", s->usea, s->useb, s->usec, s->used
    $? ". gh    A=%d   B=%d   C=%d   D=%d", s->gha, s->ghb, s->ghc, s->ghd
    $? ". p     A=%g   B=%g   C=%g   D=%g", s->pa, s->pb, s->pc, s->pd
    $? ". q     A=%g   B=%g   C=%g   D=%g", s->qa, s->qb, s->qc, s->qd
    $? ". T     A=%g   B=%g   C=%g   D=%g", s->Ta, s->Tb, s->Tc, s->Td
    $? ". dudt  A=%g   B=%g   C=%g   D=%g", s->dudta, s->dudtb, s->dudtc, s->dudtd
  } $? "- dkxsp_step1"
}


/**	Step 2: Calculate x, y, dx/dt and dy/dt for a
	given t.
	@param	s	X-spline calculation structure.
	@param	t	t-value (0<=t<=1).
	@return	1 on success, 0 on error (mathmematical error).
*/
int
dkxsp_step2 DK_P2(dk_xspline_t *,s,double,t)
{
  int back = 0, ok = 0;
  $? "+ dkxsp_step2   t=%lf", t
  double	fv;	/* Function value */
  double	dfdt;	/* First derivative */
  double	s_f;	/* Summary weights. */
  double	s_x_f;	/* Summary x * weight */
  double	s_y_f;	/* Summary y * weight */
  double	s_x_d_f_d_t;	/* Summary x * derivative */
  double	s_y_d_f_d_t;	/* Summary y * derivative */
  double	s_d_f_d_t;	/* Summary derivatives */
  double	u;		/* Substution for t */
  s_f = s_x_f = s_y_f = s_x_d_f_d_t = s_y_d_f_d_t = s_d_f_d_t = 0.0;
  if(s) {
    if(s->usea) {		$? ". use a"
      fv = dfdt = 0.0;
      if(s->gha) {		$? ". gh a"
        u = 0.0 - t;
	fv = h(u, s->qa);
	dfdt = -1.0 * dhdu(u, s->qa);
      } else {			$? ". f a"
        if(t < s->Ta) {
	  u = (s->Ta - t)/(1.0 + s->Ta);
	  fv = f(u, s->pa);
	  dfdt = dfdu(u, s->pa) * s->dudta;
	}
      } $? ". A:   fv=%g   dfdt=%g", fv, dfdt
      s_f += fv;
      s_x_f += s->xa * fv;
      s_y_f += s->ya * fv;
      s_x_d_f_d_t += s->xa * dfdt;
      s_y_d_f_d_t += s->ya * dfdt;
      s_d_f_d_t += dfdt;
    }
    if(s->useb) {		$? ". use b"
      fv = dfdt = 0.0;
      if(s->ghb) {		$? ". gh b"
        u = 1.0 - t;
	fv = g(u, s->pb, s->qb);
	dfdt = -1.0 * dgdu(u, s->pb, s->qb);
      } else {			$? ". f b"
        u = (s->Tb - t)/s->Tb;
	fv = f(u, s->pb);
	dfdt = dfdu(u, s->pb) * s->dudtb;
      } $? ". B:   fv=%g   dfdt=%g", fv, dfdt
      s_f += fv;
      s_x_f += s->xb * fv;
      s_y_f += s->yb * fv;
      s_x_d_f_d_t += s->xb * dfdt;
      s_y_d_f_d_t += s->yb * dfdt;
      s_d_f_d_t += dfdt;
    }
    if(s->usec) {		$? ". use c"
      fv = dfdt = 0.0;
      if(s->ghc) {		$? ". gh c"
        u = t;
	fv = g(u, s->pc, s->qc);
	dfdt = dgdu(u, s->pc, s->qc);
      } else {			$? ". f c"
        u = (t - s->Tc)/(1.0 - s->Tc);
	fv = f(u, s->pc);
	dfdt = dfdu(u, s->pc) * s->dudtc;
      } $? ". C:   fv=%g   dfdt=%g", fv, dfdt
      s_f += fv;
      s_x_f += s->xc * fv;
      s_y_f += s->yc * fv;
      s_x_d_f_d_t += s->xc * dfdt;
      s_y_d_f_d_t += s->yc * dfdt;
      s_d_f_d_t += dfdt;
    }
    if(s->used) {		$? ". use d"
      fv = dfdt = 0.0;
      if(s->ghd) {		$? ". gh d"
        u = t - 1.0;
	fv = h(u, s->qd);
	dfdt = dhdu(u, s->qd);
      } else {			$? ". f c"
        if(t > s->Td) {
	  u = (t - s->Td)/(2.0 - s->Td);
	  fv = f(u, s->pd);
	  dfdt = dfdu(u, s->pd) * s->dudtd;
	}
      } $? ". D:   fv=%g   dfdt=%g", fv, dfdt
      s_f += fv;
      s_x_f += s->xd * fv;
      s_y_f += s->yd * fv;
      s_x_d_f_d_t += s->xd * dfdt;
      s_y_d_f_d_t += s->yd * dfdt;
      s_d_f_d_t += dfdt;
    }
    s->x = dkma_div_double_ok(s_x_f, s_f, &ok);
    s->y = dkma_div_double_ok(s_y_f, s_f, &ok);
    s->ddtx = dkma_div_double_ok(
      (s_x_d_f_d_t * s_f - s_x_f * s_d_f_d_t),
      (s_f * s_f),
      &ok
    );
    s->ddty = dkma_div_double_ok(
      (s_y_d_f_d_t * s_f - s_y_f * s_d_f_d_t),
      (s_f * s_f),
      &ok
    );
    if(!ok) {
      back = 1;
      $? ". x     = %lf", s->x
      $? ". y     = %lf", s->y
      $? ". dx/dt = %lf", s->ddtx
      $? ". dy/dt = %lf", s->ddty
    }
  } $? "- dkxsp_step2 %d", back
  return back;
}



/**	Set spline position.
	@param	s	X-spline calculation structure.
	@param	c	Index of current segment (0 is first, m-2 is last).
	@param	m	Number of control points.
	@param	isc	Flag: Closed spline (1) or open spline (0).
*/
void
dkxsp_set_pos DK_P4(dk_xspline_t *,s,size_t,c,size_t,m,int,isc)
{
  if(s) {
    /* s->current = c; s->max = m; s->isclosed = isc; */
  }
}