/*
Copyright (c) 2004-2005, 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
  its 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.
*/

#define DK_XSP_C	1
#include "dkxsp.h"

#include <dk.h>

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

#include <dkma.h>


#line 49 "dkxsp.ctr"


#ifndef DKFIG_EPSILON
#define DKFIG_EPSILON 0.0001
#endif

/* {{{

  Complete reset.

*/
void
dkxsp_reset DK_P1(dk_xspline_t *,s)
{
  
  if(s) {
    s->isclosed = 0; s->current = s->max = 0;
    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 = s->tB = s->tC = s->tD = 0.0;
    s->pA = s->pB = s->pC = s->pD = 0.0;
    s->qA = s->qB = s->qC = s->qD = 0.0;
    s->uA = s->uB = s->uC = s->uD = 0.0;
    s->fA = s->fB = s->fC = s->fD = 0.0;
    s->ddtA = s->ddtB = s->ddtC = s->ddtD = 0.0;
    s->xA = s->xB = s->xC = s->xD = 0.0;
    s->yA = s->yB = s->yC = s->yD = 0.0;
    s->t = 0.0;
    s->x = s->y = s->ddtx = s->ddty = 0.0;
  }
  
}
/* }}} */



/* {{{

  Set control point data.
  A: t = -1
  B: t =  0
  C: t =  1
  D: t =  2

*/
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;
  }
  
}

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;
  }
  
}

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;
  }
  
}

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_step1

  The first step does all the calculations where t
  is not involved.

*/
void
dkxsp_step1 DK_P1(dk_xspline_t *,s)
{
  double res;
  
  if(s) {
    /* check, where to interpolate or approximate */
    if(s->sA < 0.0) { s->ghA = 1; }
    if(s->sB < 0.0) { s->ghB = 1; }
    if(s->sC < 0.0) { s->ghC = 1; }
    if(s->sD < 0.0) { s->ghD = 1; }
    if(s->sB >= 0.0) { s->ghA = s->ghC = 0; }
    if(s->sC >= 0.0) { s->ghB = s->ghD = 0; }
    /* now calculate p and q values */
    if(s->ghA) {		
      s->pA = 2.0;
      s->qA = (0.0 - s->sA) / 2.0;
    } else {			
      if(s->sB >= 0.0) {
        s->tA = s->sB;
      } else {
        s->tA = 0.0;
      }
      res = -1.0 - (s->tA);
      res = res * res;
      s->pA = 2.0 * res;
      s->qA = 0.0;
    } 
    if(s->ghB) {		
      s->pB = 2.0;
      s->qB = (0.0 - s->sB) / 2.0;
    } else {			
      if(s->sC >= 0.0) {
        s->tB = 1.0 + s->sC;
      } else {
        s->tB = 1.0;
      }
      res = s->tB;
      res = res * res;
      s->pB = 2.0 * res;
      s->qB = 0.0;
    } 
    if(s->ghC) {		
      s->pC = 2.0;
      s->qC = (0.0 - s->sC) / 2.0;
    } else {			
      if(s->sB >= 0.0) {
        s->tC = 0.0 - s->sB;
      } else {
        s->tC = 0.0;
      }
      res = 1.0 - s->tC;
      res = res * res;
      s->pC = 2.0 * res;
      s->qC = 0.0;
    } 
    if(s->ghD) {		
      s->pD = 2.0;
      s->qD = (0.0 - s->sD) / 2.0;
    } else {			
      if(s->sC >= 0.0) {
        s->tD = 1.0 - s->sC;
      } else {
        s->tD = 1.0;
      }
      res = 2.0 - s->tD;
      res = res * res;
      s->pD = 2.0 * res;
      s->qD = 0.0;
    }
    /* some final corrections on spline ends */
    if(!(s->isclosed)) {		
      if((s->current) == 0) {		
        if((fabs(s->sB) < DKFIG_EPSILON) && (s->sC < 0.0)) {
	  s->ghC = 0; s->ghB = 1;	
          s->pB = 2.0;
          s->qB = (0.0 - s->sC) / 2.0;
          s->tC = 0.0;
          res = 1.0 - s->tC; res = res * res; s->pC = 2.0 * res;
          s->qC = 0.0;
	}
      } else {
        if((s->current) == 1) {		
	  if((fabs(s->sA) < DKFIG_EPSILON) && (s->sB < 0.0)) {
	    s->ghA = 1;			
	    s->pA  = 2.0;
	    s->qA  = (0.0 - s->sB) / 2.0;
	  }
	}
      }
      if((s->current) == ((s->max) - 3)) { 
        if((fabs(s->sD) < DKFIG_EPSILON) && (s->sC < 0.0)) {
	  s->ghD = 1;			
	  s->pD  = 2.0;
	  s->qD  = (0.0 - s->sC) / 2.0;
	}
      } else {
        if((s->current) == ((s->max) - 2)) { 
	  if((fabs(s->sC) < DKFIG_EPSILON) && (s->sB < 0.0)) {
	    s->ghB = 0; s->ghC = 1;	
	    s->pC = 2.0;
	    s->qC = (0.0 - s->sB) / 2.0;
	    s->tB = 1.0;
	    res = 1.0 - s->tB; res = res * res; s->pB = 2.0 * res;
	    s->qB = 0.0;
	  }
	}
      }
    }
    
    
  } 
}
/* }}} */



/* {{{ f

  Calculate f(u).

*/
static
double
f DK_P2(double,u,double,p)
{
  double back;
  double uu, uuu, uuuu, uuuuu;
  
  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;
  
  return back;
}
/* }}} */



/* {{{ g

  Calculate g(u).

*/
static
double
g DK_P3(double,u,double,p,double,q)
{
  double back;
  double uu, uuu, uuuu, uuuuu;
  
  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;
  
  return back;
}
/* }}} */



/* {{{ h

  Calulate h(u)
*/
static
double
h DK_P2(double,u,double,q)
{
  double back;
  double uu, uuu, uuuu, uuuuu;
  
  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;
  
  return back;
}
/* }}} */



/* {{{ dfdu

  Calculate df/du.
*/
static
double
dfdu DK_P2(double,u,double,p)
{
  double back;
  double uu, uuu, uuuu;
  uu = u * u; uuu = uu * u; uuuu = uu * uu;
  
  back = 5.0*(6.0-p)*uuuu+4.0*(2.0*p-15.0)*uuu+3.0*(10.0-p)*uu;
  
  return back;
}
/* }}} */



/* {{{ dgdu

  Calculate dg/du.

*/
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;
  
  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;
  
  return back;
}
/* }}} */



/* {{{ dhdu

  Calculate dh/du.

*/
static
double
dhdu DK_P2(double,u,double,q)
{
  double back, uu, uuu, uuuu;
  uu = u * u; uuu = uu * u; uuuu = uuu * u;
  
  back = q+4.0*q*u-8.0*q*uuu-5.0*q*uuuu;
  
  return back;
}
/* }}} */



/* {{{ dkxsp_step2

  Calculation of x, y, dx/dt and dy/dt for t.

*/
int
dkxsp_step2 DK_P2(dk_xspline_t *,s,double,t)
{
  int back = 0, ok = 0;
  double a, b, c, d;
  
  if(s) {
    s->t = t;
    /* calculate u */
    s->uA = s->uB = s->uC = s->uD = 0.0;
    s->dudtA = s->dudtB = s->dudtC = s->dudtD = 0.0;
    if(s->useA) {
    if(s->ghA) {
      s->uA = 0.0 - t;
      s->dudtA = -1.0;
    } else {
      s->uA = dkma_div_double_ok(
        (t - s->tA), (-1.0 - s->tA), &ok
      );
      s->dudtA = 0.0 - dkma_div_double_ok(
        1.0,
	(1.0 + s->tA),
        &ok
      );
    }
    }
    if(s->useB) {
    if(s->ghB) {
      /* s->uB = -1.0 - t; */
      s->uB = 1.0 - t;
      s->dudtB = -1.0;
    } else {
      s->uB = dkma_div_double_ok(
        (t - s->tB), (0.0 - s->tB), &ok
      );
      s->dudtB = 0.0 - dkma_div_double_ok(
        1.0,
	s->tB,
        &ok
      );
    }
    }
    if(s->useC) {
    if(s->ghC) {
      s->uC = t;
      s->dudtC = 1.0;
    } else {
      s->uC = dkma_div_double_ok(
        (t - s->tC), (1.0 - s->tC), &ok
      );
      s->dudtC = dkma_div_double_ok(
        1.0,
	(1.0 - s->tC),
	&ok
      );
    }
    }
    if(s->useD) {
    if(s->ghD) {
      s->uD = t - 1.0;
      s->dudtD = 1.0;
    } else {
      s->uD = dkma_div_double_ok(
        (t - s->tD), (2.0 - s->tD), &ok
      );
      s->dudtD = dkma_div_double_ok(
        1.0,
	(2.0 - s->tD),
	&ok
      );
    }
    }
    
    
    /* now the blending function values */
    s->fA = s->fB = s->fC = s->fD = 0.0;
    s->ddtA = s->ddtB = s->ddtC = s->ddtD = 0.0;
    s->dduA = s->dduB = s->dduC = s->dduD = 0.0;
    a = b = c = d = 0.0;
    if(s->useA) {
      if(s->ghA) {
        s->fA = h(s->uA, s->qA);
	s->dduA = dhdu(s->uA, s->qA);
	s->ddtA = s->dduA * s->dudtA;
	a += s->fA; d += s->ddtA;
      } else {
        if(t < s->tA) {
	  s->fA = f(s->uA, s->pA);
	  s->dduA = dfdu(s->uA, s->pA);
	  s->ddtA = s->dduA * s->dudtA;
	  a += s->fA; d += s->ddtA;
	}
      }
    }
    if(s->useB) {
      if(s->ghB) {
        s->fB = g(s->uB, s->pB, s->qB);
	s->dduB = dgdu(s->uB, s->pB, s->qB);
	s->ddtB = s->dduB * s->dudtB;
	a += s->fB; d += s->ddtB;
      } else {
        if(t < s->tB) {
	  s->fB = f(s->uB, s->pB);
	  s->dduB = dfdu(s->uB, s->pB);
	  s->ddtB = s->dduB * s->dudtB;
	  a += s->fB; d += s->ddtB;
	}
      }
    }
    if(s->useC) {
      if(s->ghC) {
        s->fC = g(s->uC, s->pC, s->qC);
	s->dduC = dgdu(s->uC, s->pC, s->qC);
	s->ddtC = s->dduC * s->dudtC;
	a += s->fC; d += s->ddtC;
      } else {
        if(t > s->tC) {
	  s->fC = f(s->uC, s->pC);
	  s->dduC = dfdu(s->uC, s->pC);
	  s->ddtC = s->dduC * s->dudtC;
	  a += s->fC; d += s->ddtC;
	}
      }
    }
    if(s->useD) {
      if(s->ghD) {
        s->fD = h(s->uD, s->qD);
	s->dduD = dhdu(s->uD, s->qD);
	s->ddtD = s->dduD * s->dudtD;
	a += s->fD; d += s->ddtD;
      } else {
        if(t > s->tD) {
	  s->fD = f(s->uD, s->pD);
	  s->dduD = dfdu(s->uD, s->pD);
	  s->ddtD = s->dduD * s->dudtD;
	  a += s->fD; d += s->ddtD;
	}
      }
    }
    
    
    
    
    /* and now x and y */
    c = b = 0.0;
    if(s->useA) { c+= ((s->xA) * (s->fA)); b += ((s->xA) * (s->ddtA)); }
    if(s->useB) { c+= ((s->xB) * (s->fB)); b += ((s->xB) * (s->ddtB)); }
    if(s->useC) { c+= ((s->xC) * (s->fC)); b += ((s->xC) * (s->ddtC)); }
    if(s->useD) { c+= ((s->xD) * (s->fD)); b += ((s->xD) * (s->ddtD)); }
    s->x = dkma_div_double_ok(c, a, &ok);
    s->ddtx = dkma_div_double_ok((a*b-c*d), (a*a), &ok);
    c = b = 0.0;
    if(s->useA) { c+= ((s->yA) * (s->fA)); b += ((s->yA) * (s->ddtA)); }
    if(s->useB) { c+= ((s->yB) * (s->fB)); b += ((s->yB) * (s->ddtB)); }
    if(s->useC) { c+= ((s->yC) * (s->fC)); b += ((s->yC) * (s->ddtC)); }
    if(s->useD) { c+= ((s->yD) * (s->fD)); b += ((s->yD) * (s->ddtD)); }
    s->y = dkma_div_double_ok(c, a, &ok);
    s->ddty = dkma_div_double_ok((a*b-c*d), (a*a), &ok);
    
    if(!ok) {
      back = 1;
    }
  } 
  return back;
}
/* }}} */



/* {{{ dkxsp_set_pos

  Set the current position in an X-spline.

*/
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;
  }
}
/* }}} */



/* {{{ SCCS ID */
#ifndef LINT
static char sccs_id[] = {
"@(#)dkxsp.ctr 1.98 02/19/08\t(krause) - fig2vect"
};
#endif
/* }}} */