/*
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 dkbsp.c	The Bezier splines module.
*/


/** Inside the dkbsp module. */

#define DKBSP_C 1
#include "dkbsp.h"



#if DK_HAVE_MATH_H
#include <math.h>
#endif
#if DK_HAVE_STDLIB_H
#include <stdlib.h>
#endif
#include <dkma.h>



$(trace-include)



/**	Calculate Bezier spline control points from
	x_0, dx/dt at x_0, dx/dt at x_1 and x_1.
	The result is stored in the dk_bspline_t
	structure.
	@param	s	Bezier spline structure.
	@param	x0	x at start of interval.
	@param	d0	dx/dt at x0.
	@param	x1	x at end of interval.
	@param	d1	dx/dt at x1.
	@return	1 on success, 0 on error.
*/
int
dkbsp_calculate DK_P5(dk_bspline_t *,s,double,x0,double,d0,double,x1,double,d1)
{
  int back = 0;	/* Function result. */
  int ok = 0;	/* Flag: Mathematical error. */
  $? "+ dkbsp_calculate %s %lg %lg %lg %lg", TR_PTR(s), x0, d0, x1, d1
  if(s) {
    back = 1;
    s->x0 = x0; s->x1 = x1;
    s->dxdt0 = d0; s->dxdt1 = d1;
    s->xp0 = dkma_add_double_ok(x0, (d0/3.0), &ok); $? ". xp0 = %lg", s->xp0
    s->xm1 = dkma_sub_double_ok(x1, (d1/3.0), &ok); $? ". xm1 = %lg", s->xm1
    if(ok) { back = 0; }
  } $? "- dkbsp_calculate %d", back
  return back;
}



/**	Apply variable, calculate value and derivative to find minimum
	and maximum.
	@param	s	Bezier spline structure.
	@param	t	The t value.
	@return	1 on success, 0 on error.
*/
static
int
apply_t_for_min_max DK_P2(dk_bspline_t *,s,double,t)
{
  int back = 1, ok = 0;
  double x, tt, ttt, a, aa, aaa;
  $? "+ apply_t_for_min_max %s %lg", TR_PTR(s), t
  tt = dkma_mul_double_ok(t,t,&ok);
  ttt = dkma_mul_double_ok(tt,t,&ok);
  a = dkma_sub_double_ok(1.0, t, &ok);
  aa = dkma_mul_double_ok(a,a,&ok);
  aaa = dkma_mul_double_ok(aa,a,&ok);
  x = dkma_add_double_ok(
    dkma_add_double_ok(
      dkma_mul_double_ok(s->x0, aaa, &ok),
      dkma_mul_double_ok(
        dkma_mul_double_ok(s->xp0, aa, &ok),
	dkma_mul_double_ok(3.0, t, &ok),
	&ok
      ),
      &ok
    ),
    dkma_add_double_ok(
      dkma_mul_double_ok(
        dkma_mul_double_ok(s->xm1, a, &ok),
	dkma_mul_double_ok(3.0, tt, &ok),
	&ok
      ),
      dkma_mul_double_ok(s->x1, ttt, &ok),
      &ok
    ),
    &ok
  ); $? ". x = %lg", x
  if(x > s->max) { s->max = x; }
  if(x < s->min) { s->min = x; }
  if(ok) back = 0;
  $? "- apply_t_for_min_max %d", back
  return back;
}



/**	Calculate minimum and maximum x value for a Bezier spline segment.
	@param	s	Bezier spline structure.
	@param	x0	X-value of "left" control point.
	@param	d0	X-value of second control point.
	@param	x1	X-value of "right" control point.
	@param	d1	X-value of third control point.
	@return	1 on success, 0 on error.
*/
int
dkbsp_minmax DK_P5(dk_bspline_t *,s,double,x0,double,d0,double,x1,double,d1)
{
  int back = 0, ok = 0, test = 0, tneeded = 0;
  double a, b, c, p, q, wurzel, t, t0;
  $? "+ dkbsp_minmax %s %lg %lg %lg %lg", TR_PTR(s), x0, d0, x1, d1
  if(s) {
    back = dkbsp_calculate(s,x0,d0,x1,d1);
    if(s->x0 > s->x1) {
      s->max = s->x0; s->min = s->x1;
    } else {
      s->max = s->x1; s->min = s->x0;
    }
    if(back) {
      a = dkma_mul_double_ok(
        3.0,
	dkma_add_double_ok(
	  dkma_sub_double_ok(s->x1, s->xm1, &ok),
	  dkma_sub_double_ok(s->xp0, s->x0, &ok),
	  &ok
	),
	&ok
      );
      b = dkma_mul_double_ok(
        2.0,
	dkma_add_double_ok(
	  dkma_sub_double_ok(
	    s->xm1,
	    dkma_mul_double_ok(2.0, s->xp0, &ok),
	    &ok
	  ),
	  dkma_mul_double_ok(3.0, s->x0, &ok),
	  &ok
	),
	&ok
      );
      c = dkma_sub_double_ok(
        s->xp0,
	dkma_mul_double_ok(3.0, s->x0, &ok),
        &ok
      ); $? ". a = %lg   b = %lg   c = %lg", a, b, c
      t = 0.0; tneeded = 1;
      p = dkma_div_double_ok(b,a,&test);
      if(test) {
        t = 0.0 - dkma_div_double_ok(c,b,&ok);
	if((ok == 0) && (t >= 0.0) && (t <= 1.0)) {
	  if(!apply_t_for_min_max(s, t)) {
	    back = 0;
	  }
	}
      } else {
        q = dkma_div_double_ok(c,a,&ok);
	$? ". p = %lg   q = %lg", p, q
        t0 = 0.0 - dkma_div_double_ok(p, 2.0, &ok);
	wurzel = dkma_sub_double_ok(
	  dkma_mul_double_ok(t0, t0, &ok),
	  q,
	  &ok
	);
	if(wurzel >= 0.0) {
	  wurzel = sqrt(wurzel); $? ". t0 = %lg   wurzel = %lg", t0, wurzel
          t = dkma_add_double_ok(t0, wurzel, &ok);
          if((ok == 0) && (t >= 0.0) && (t <= 1.0)) {
            if(!apply_t_for_min_max(s, t)) {
	      back = 0;
	    }
          }
          t = dkma_sub_double_ok(t0, wurzel, &ok);
          if((ok == 0) && (t >= 0.0) && (t <= 1.0)) {
            if(!apply_t_for_min_max(s, t)) {
	      back = 0;
	    }
          }
	} else {
	  tneeded = 0;
	}
      }
    }
  } $? "- dkbsp_minmax %d", back
  return back;
}



/**	Calculate value and derivative for a given t
	(0<=t<=1) in a Bezier spline segment.
	@param	s	Bezier spline structure.
	@param	x0	"Left" control point.
	@param	xp	Second control point.
	@param	x1	"Right" control point.
	@param	xm	Third control point.
	@param	t	The t value.
	@return	1 on success, 0 on error.
*/
int dkbsp_for_t DK_P6(dk_bspline_t *,s,double,x0,double,xp,double,x1,double,xm,double,t)
{
  int back = 0;
  int me = 0;
  double tt, ttt, omt, omtomt, omtomtomt;
  $? "+ dkbsp_for_t x0=%lg  xp=%lg  x1=%lg  xm=%lg  t=%lg",x0,xp,x1,xm,t
  if(s) {
    back = 1;
    s->x0 = x0; s->x1 = x1; s->xp0 = xp; s->xm1 = xm;
    tt  = dkma_mul_double_ok(t, t, &me);
    ttt = dkma_mul_double_ok(tt, t, &me);
    omt = dkma_sub_double_ok(1.0, t, &me);
    omtomt = dkma_mul_double_ok(omt, omt, &me);
    omtomtomt = dkma_mul_double_ok(omtomt, omt, &me);
    /* value */
    s->xvalue = dkma_add_double_ok(
      dkma_add_double_ok(
        dkma_mul_double_ok(omtomtomt, x0, &me),
	dkma_mul_double_ok(
	  dkma_mul_double_ok(3.0, t, &me),
	  dkma_mul_double_ok(omtomt, xp, &me),
	  &me
	),
	&me
      ),
      dkma_add_double_ok(
        dkma_mul_double_ok(
	  dkma_mul_double_ok(3.0, tt, &me),
	  dkma_mul_double_ok(omt, xm, &me),
	  &me
	),
	dkma_mul_double_ok(ttt, x1, &me),
	&me
      ),
      &me
    );
    /* derivative */
    s->dxdt = dkma_add_double_ok(
      dkma_mul_double_ok(
        dkma_mul_double_ok(3.0, tt, &me),
	dkma_add_double_ok(
	  dkma_mul_double_ok(
	    3.0,
	    dkma_sub_double_ok(xp, xm, &me),
	    &me
	  ),
	  dkma_sub_double_ok(x1, x0, &me),
	  &me
	),
	&me
      ),
      dkma_add_double_ok(
        dkma_mul_double_ok(
	  dkma_mul_double_ok(6.0, t, &me),
	  dkma_sub_double_ok(
	    dkma_add_double_ok(xm, x0, &me),
	    dkma_mul_double_ok(2.0, xp, &me),
	    &me
	  ),
	  &me
	),
	dkma_mul_double_ok(
	  3.0,
	  dkma_sub_double_ok(xp, x0, &me),
	  &me
	),
	&me
      ),
      &me
    );
    if(me) { back = 0; }
    $? ". xv=%lg  dxdt=%lg", s->xvalue, s->dxdt
  }
  $? "- dkbsp_for_t %d", back
  return back;
}