/****************************************************************************
 *               splines.cpp
 *
 * Contributed by ???
 *
 * This module implements splines.
 *
 * from Persistence of Vision(tm) Ray Tracer version 3.6.
 * Copyright 1991-2003 Persistence of Vision Team
 * Copyright 2003-2004 Persistence of Vision Raytracer Pty. Ltd.
 *---------------------------------------------------------------------------
 * NOTICE: This source code file is provided so that users may experiment
 * with enhancements to POV-Ray and to port the software to platforms other
 * than those supported by the POV-Ray developers. There are strict rules
 * regarding how you are permitted to use this file. These rules are contained
 * in the distribution and derivative versions licenses which should have been
 * provided with this file.
 *
 * These licences may be found online, linked from the end-user license
 * agreement that is located at http://www.povray.org/povlegal.html
 *---------------------------------------------------------------------------
 * This program is based on the popular DKB raytracer version 2.12.
 * DKBTrace was originally written by David K. Buck.
 * DKBTrace Ver 2.0-2.12 were written by David K. Buck & Aaron A. Collins.
 *---------------------------------------------------------------------------
 * $File: //depot/povray/3.6-release/source/splines.cpp $
 * $Revision: #3 $
 * $Change: 3032 $
 * $DateTime: 2004/08/02 18:43:41 $
 * $Author: chrisc $
 * $Log$
 *****************************************************************************/

#include "frame.h"
#include "userio.h"
#include "splines.h"

BEGIN_POV_NAMESPACE

/*****************************************************************************
* Global Variables
******************************************************************************/


/*****************************************************************************
* Local preprocessor defines
******************************************************************************/


/*****************************************************************************
* Local typedefs
******************************************************************************/


/*****************************************************************************
* Local variables
******************************************************************************/


/*****************************************************************************
* Local functions
******************************************************************************/

DBL linear_interpolate(SPLINE_ENTRY * se, int i, int k, DBL p);
DBL quadratic_interpolate(SPLINE_ENTRY * se, int i, int k, DBL p);
DBL natural_interpolate(SPLINE_ENTRY * se, int i, int k, DBL p);
DBL catmull_rom_interpolate(SPLINE_ENTRY * se, int i, int k, DBL p);
int findt(SPLINE * sp, DBL Time);
void mkfree(SPLINE * sp, int i);
void Precompute_Cubic_Coeffs(SPLINE *sp);

/*****************************************************************************
*
* FUNCTION
*
*       Precompute_Cubic_Coeffs
*
* INPUT
*
*       sp : a pointer to the spline to compute interpolation coefficients for
*
* OUTPUT
*
* RETURNS
*
* AUTHOR
*
*   Mark Wagner
*
* DESCRIPTION
*
*       Computes the coefficients used in cubic_interpolate.
*
* CHANGES
*
******************************************************************************/

void Precompute_Cubic_Coeffs(SPLINE *sp)
{
    int i, k;
    DBL *h;
    DBL *b;
    DBL *u;
    DBL *v;

    h = (DBL *)POV_MALLOC(sp->Number_Of_Entries*sizeof(DBL), "Spline coefficient storage");
    b = (DBL *)POV_MALLOC(sp->Number_Of_Entries*sizeof(DBL), "Spline coefficient storage");
    u = (DBL *)POV_MALLOC(sp->Number_Of_Entries*sizeof(DBL), "Spline coefficient storage");
    v = (DBL *)POV_MALLOC(sp->Number_Of_Entries*sizeof(DBL), "Spline coefficient storage");

    for(k = 0; k < 5; k++)
    {
        for(i = 0; i <= sp->Number_Of_Entries - 2; i++)
        {
            h[i] = sp->SplineEntries[i+1].par - sp->SplineEntries[i].par;
            b[i] = (sp->SplineEntries[i+1].vec[k] - sp->SplineEntries[i].vec[k])/h[i];
        }
        u[1] = 2*(h[0]+h[1]);
        v[1] = 6*(b[1]-b[0]);
        for(i = 2; i <= sp->Number_Of_Entries - 2; i++)
        {
            u[i] = 2*(h[i]+h[i-1]) - (h[i-1]*h[i-1])/u[i-1];
            v[i] = 6*(b[i]-b[i-1]) - (h[i-1]*v[i-1])/u[i-1];
        }
        sp->SplineEntries[sp->Number_Of_Entries-1].coeff[k] = 0;
        for(i = sp->Number_Of_Entries-2; i > 0; i--)
        {
            sp->SplineEntries[i].coeff[k] = (v[i] - h[i]*sp->SplineEntries[i+1].coeff[k])/u[i];
        }
        sp->SplineEntries[0].coeff[k] = 0;
    }
    sp->Coeffs_Computed = true;

    POV_FREE(h);
    POV_FREE(b);
    POV_FREE(u);
    POV_FREE(v);
}


/*****************************************************************************
*
* FUNCTION
*
*       linear_interpolate
*
* INPUT
*
*       se : a pointer to the entries in the spline
*       i  : the first point to interpolate between
*       k  : which dimension of the 5D vector to interpolate in
*       p  : the parameter to interpolate the value for
*
* OUTPUT
*
* RETURNS
*
*       The value of the kth dimension of the vector linearly interpolated at p
*
* AUTHOR
*
*   Wolfgang Ortmann
*
* DESCRIPTION
*
* CHANGES
*
******************************************************************************/

DBL linear_interpolate(SPLINE_ENTRY * se, int i, int k, DBL p)
{
    DBL p1, p2, v1, v2;
    p1 = se[i].par;
    p2 = se[i+1].par;
    v1 = se[i].vec[k];
    v2 = se[i+1].vec[k];
    return (p-p1)*(v2-v1)/(p2-p1)+v1;
}


/*****************************************************************************
*
* FUNCTION
*
*       quadratic_interpolate
*
* INPUT
*
*       se : a pointer to the entries in the spline
*       i  : the second point of three to interpolate between
*       k  : which dimension of the 5D vector to interpolate in
*       p  : the parameter to interpolate the value for
*
* OUTPUT
*
* RETURNS
*
*       The value of the kth dimension of the quadratic interpolation of the
*        vector at p
*
* AUTHOR
*
*   Wolfgang Ortmann
*
* DESCRIPTION
*
* CHANGES
*
******************************************************************************/

DBL quadratic_interpolate(SPLINE_ENTRY * se, int i, int k, DBL p)
{
  /* fit quadratic function to three point*/
  DBL n;
  DBL p1, p2, p3,
      v1, v2, v3,
      a, b, c;
  /* assignments to make life easier */
  p1=se[i-1].par;    p2=se[i].par;    p3=se[i+1].par;
  v1=se[i-1].vec[k]; v2=se[i].vec[k]; v3=se[i+1].vec[k];

  n=(p2-p1)*(p3-p1)*(p3-p2);

  /* MWW NOTE: I'm assuming these are correct.  I didn't write them */
  a=(-p2*v1+p3*v1
      +p1*v2-p3*v2
      -p1*v3+p2*v3) /n;
  b=( p2*p2*v1 - p3*p3*v1
      -p1*p1*v2 + p3*p3*v2
      +p1*p1*v3 - p2*p2*v3) /n;
  c=(-p2*p2*p3*v1+p2*p3*p3*v1
      +p1*p1*p3*v2-p1*p3*p3*v2
      -p1*p1*p2*v3+p1*p2*p2*v3) /n;
  return (a*p+b)*p+c; /* Fast way of doing ap^2+bp+c */
}


/*****************************************************************************
*
* FUNCTION
*
*       natural_interpolate
*
* INPUT
*
*       se : a pointer to the entries in the spline
*       i  : the first point in the interpolation interval
*       k  : which dimension of the 5D vector to interpolate in
*       p  : the parameter to interpolate the value for
*
* OUTPUT
*
* RETURNS
*
*       The value of the kth dimension of the natural cubic interpolation
*        of the vector at p
*
* AUTHOR
*
*       Mark Wagner
*
* DESCRIPTION
*
* CHANGES
*
******************************************************************************/

DBL natural_interpolate(SPLINE_ENTRY * se, int i, int k, DBL p)
{
    DBL h, tmp;
    h = se[i+1].par - se[i].par;
    tmp = se[i].coeff[k]/2.0 + ((p - se[i].par)*(se[i+1].coeff[k] - se[i].coeff[k]))/(6.0*h);
    tmp = -(h/6.0)*(se[i+1].coeff[k] + 2.0*se[i].coeff[k]) + (se[i+1].vec[k] - se[i].vec[k])/h + (p - se[i].par)*tmp;
    return se[i].vec[k] + (p - se[i].par)*tmp;
}


/*****************************************************************************
*
* FUNCTION
*
*       catmull_rom_interpolate
*
* INPUT
*
*       se : a pointer to the entries in the spline
*       i  : the first point in the interpolation interval
*       k  : which dimension of the 5D vector to interpolate in
*       p  : the parameter to interpolate the value for
*
* OUTPUT
*
* RETURNS
*
*       The value of the kth dimension of the Catmull-Rom interpolation of the
*        vector at p
*
* AUTHOR
*
*       Mark Wagner
*
* DESCRIPTION
*
* CHANGES
*
******************************************************************************/

DBL catmull_rom_interpolate(SPLINE_ENTRY * se, int i, int k, DBL p)
{
    DBL dt = (se[i+1].par - se[i].par); /* Time between se[i] and se[i+1] */
    DBL u = (p - se[i].par)/dt;         /* Fractional time from se[i] to p */
    DBL dp0 = ((se[i].vec[k] - se[i-1].vec[k])/(se[i].par - se[i-1].par) + (se[i+1].vec[k] - se[i].vec[k])/(se[i+1].par - se[i].par))/2.0 * dt;
    DBL dp1 = ((se[i+2].vec[k] - se[i+1].vec[k])/(se[i+2].par - se[i+1].par) + (se[i+1].vec[k] - se[i].vec[k])/(se[i+1].par - se[i].par))/2.0 * dt;

    return (se[i].vec[k] * (2*u*u*u - 3*u*u + 1) +
            se[i+1].vec[k] * (3*u*u - 2*u*u*u) +
            dp0 * (u*u*u - 2*u*u + u) +
            dp1 * (u*u*u - u*u) );
}


/*****************************************************************************
*
* FUNCTION
*
*       findt
*
* INPUT
*
*       sp   : a pointer to a spline
*       Time : The parameter to search for
*
* OUTPUT
*
* RETURNS
*
*       The first spline entry with a parameter greater than Time
*
* AUTHOR
*
*   Wolfgang Ortmann
*
* DESCRIPTION
*
* CHANGES
*
*       Mark Wagner  6 Nov 2000 : Changed from linear search to binary search
*
******************************************************************************/

int findt(SPLINE * sp, DBL Time)
{
    int i, i2;
    SPLINE_ENTRY * se;
    se = sp->SplineEntries;
    if(sp->Number_Of_Entries == 0) return 0;
    
    if(Time <= se[0].par) return 0;

    if(Time >= se[sp->Number_Of_Entries-1].par) return sp->Number_Of_Entries;

    i = sp->Number_Of_Entries / 2;
    /* Bracket the proper entry */
    if( Time > se[i].par ) /* i is lower, i2 is upper */
    {
        i2 = sp->Number_Of_Entries-1;
        while(i2 - i > 1)
        {
            if(Time > se[i+(i2-i)/2].par)
                i = i+(i2-i)/2;
            else
                i2 = i+(i2-i)/2;
        }
        return i2;
    }
    else /* i is upper, i2 is lower */
    {
        i2 = 0;
        while(i - i2 > 1)
        {
            if(Time > se[i2+(i-i2)/2].par)
                i2 = i2+(i-i2)/2;
            else
                i = i2+(i-i2)/2;
        }
        return i;
    }
}


/*****************************************************************************
*
* FUNCTION
*
*       mkfree
*
* INPUT
*
*       sp : a pointer to the entries in the spline
*       i  : the index of the entry to make available
*
* OUTPUT
*
* RETURNS
*
* AUTHOR
*
*   Wolfgang Ortmann
*
* DESCRIPTION
*
*       Makes the spline entry at index i available for inserting a new entry
*       by moving all entries starting with that index down by one slot in the
*       array.
*
* CHANGES
*
******************************************************************************/

void mkfree(SPLINE * sp, int i)
{
    int j;
    SPLINE_ENTRY * se;
    se = sp->SplineEntries;
    
    for (j=sp->Number_Of_Entries; j>i; j--)
        se[j] = se[j-1];
}


/*****************************************************************************
*
* FUNCTION
*
*       Create_Spline
*
* INPUT
*
*       Type : the type of the new spline
*
* OUTPUT
*
* RETURNS
*
*       A pointer to the newly-created spline
*
* AUTHOR
*
*   Wolfgang Ortmann
*
* DESCRIPTION
*
* CHANGES
*
*       Mark Wagner  6 Nov 2000 : Added support for dynamic resizing of the
*               SplineEntry array.
*       Mark Wagner 25 Aug 2001 : Added support for pre-computing coefficients
*               of cubic splines
*
******************************************************************************/

SPLINE * Create_Spline(int Type)
{
    SPLINE * New;
    New = (SPLINE *)POV_MALLOC(sizeof(SPLINE), "spline");
    New->SplineEntries = (SPLINE_ENTRY *)POV_MALLOC(INIT_SPLINE_SIZE*sizeof(SPLINE_ENTRY), "spline entry");
    New->Max_Entries = INIT_SPLINE_SIZE;
    New->Number_Of_Entries = 0;
    New->Type = Type;
    New->Coeffs_Computed = false;
    New->Terms = 2;
    New->Cache_Valid = false;
    int i;
    for (i=0; i< New->Max_Entries; i++)
    {
      New->SplineEntries[i].par=-1e6;   //this should be a negative large number
    }

    return New;
}


/*****************************************************************************
*
* FUNCTION
*
*       Copy_Spline
*
* INPUT
*
*       Old : A pointer to the old spline
*
* OUTPUT
*
* RETURNS
*
*       A pointer to the new spline
*
* AUTHOR
*
*   Wolfgang Ortmann
*
* DESCRIPTION
*
* CHANGES
*
*       Mark Wagner  6 Nov 2000 : Added support for dynamic resizing of the
*               SplineEntry array
*       Mark Wagner 25 Aug 2001 : Added support for pre-computing coefficients
*               of cubic splines
*
******************************************************************************/

SPLINE * Copy_Spline(SPLINE * Old)
{
    SPLINE * New;
    New = (SPLINE *)POV_MALLOC(sizeof(SPLINE), "spline");
    
    New->SplineEntries = (SPLINE_ENTRY *)POV_MALLOC(Old->Number_Of_Entries*sizeof(SPLINE_ENTRY), "spline entry");
    POV_MEMCPY(New->SplineEntries, Old->SplineEntries, Old->Number_Of_Entries*sizeof(SPLINE_ENTRY));

    New->Max_Entries = Old->Number_Of_Entries;
    New->Number_Of_Entries = Old->Number_Of_Entries;
    New->Type = Old->Type;
    New->Coeffs_Computed = Old->Coeffs_Computed;
    New->Terms = Old->Terms;
    New->Cache_Valid = false; // we don't copy the cache so mark it as invalid

    return New;
}


/*****************************************************************************
*
* FUNCTION
*
*       Destroy_Spline
*
* INPUT
*
*       Spline : A pointer to the spline to delete
*
* OUTPUT
*
* RETURNS
*
* AUTHOR
*
*   Wolfgang Ortmann
*
* DESCRIPTION
*
*       Deletes the SplineEntry array
*
* CHANGES
*
******************************************************************************/

void Destroy_Spline(SPLINE * Spline)
{
    POV_FREE(Spline->SplineEntries);
    POV_FREE(Spline);
}


/*****************************************************************************
*
* FUNCTION
*
*       Insert_Spline_Entry
*
* INPUT
*
*       sp : a pointer to the spline to insert the new value in
*       p  : the value of the parameter at that point
*       v  : a 5D vector that is the value of the spline at that parameter
*
* OUTPUT
*
* RETURNS
*
* AUTHOR
*
*   Wolfgang Ortmann
*
* DESCRIPTION
*
*       Inserts a value into the given spline, sorting the entries in order by
*               increasing p
*       If a value of the parameter already exists, that value is overwritten
*
* CHANGES
*
*       Mark Wagner  8 Nov 2000 : Added dynamic sizing of the SplineEntry array
*          If a value of the parameter already exists, that value is overwritten
*       Mark Wagner 25 Aug 2001 : Modified for compatibility with new method of
*          computing cubic splines.
*       Mark Wagner 30 Aug 2001 : Fixed a bug with over-writing of parameter 
*          values.
*
******************************************************************************/

void Insert_Spline_Entry(SPLINE * sp, DBL p, EXPRESS v)
{
    int i, k;

    /* Reset the Coeffs_Computed flag.  Inserting a new point invalidates 
     *  pre-computed coefficients */
    sp->Coeffs_Computed = false;
    sp->Cache_Valid = false;
    /* If all space is used, reallocate */
    if(sp->Number_Of_Entries >= sp->Max_Entries)
    {
        sp->Max_Entries += INIT_SPLINE_SIZE;
        sp->SplineEntries = (SPLINE_ENTRY *)POV_REALLOC(sp->SplineEntries, sp->Max_Entries * sizeof(SPLINE_ENTRY), "Temporary Spline Entries");
        for (i = sp->Number_Of_Entries; i < sp->Max_Entries; i++)
        {
          sp->SplineEntries[i].par=-1e6;
        }
    }
    i = findt(sp, p);
    /* If p is already in spline, replace */
    /* The clause after the || is needed because findt returns sp->Number_Of_Entries
     * if p is greater than OR EQUAL TO the highest par in the spline */
    if(sp->Number_Of_Entries != 0 && ((sp->SplineEntries[i].par == p) || (i == sp->Number_Of_Entries && sp->SplineEntries[i-1].par == p)))
    {
        for(k=0; k<5; k++)
            sp->SplineEntries[i].vec[k] = v[k];
    }
    else
    {
        mkfree(sp, i);
        sp->SplineEntries[i].par = p;

        for(k=0; k<5; k++)
            sp->SplineEntries[i].vec[k] = v[k];

        sp->Number_Of_Entries += 1;
    }
}


/*****************************************************************************
*
* FUNCTION
*
*       Get_Spline_Value
*
* INPUT
*
*       sp : a pointer to the spline to interpolate
*       p  : the parameter to interpolate the value for
*       v  : a pointer to a 5D vector to store the interpolated values in
*
* OUTPUT
*
*       v  : a pointer to a 5D vector to store the interpolated values in
*
* RETURNS
*
*       The value of the first element of the interpolated vector
*
* AUTHOR
*
*   Wolfgang Ortmann
*
* DESCRIPTION
*
*       Interpolates the spline at the given point.  If the point is outside the
*               range of parameters of the spline, returns the value at the
*               appropriate endpoint (ie. no extrapolation).  If there are not
*               enough points in the spline to do the desired type of
*               interpolation, does the next best type (cubic->quadratic->linear).
*
* CHANGES
*
*       Mark Wagner  8 Nov 2000 : Complete overhaul.  I am apalled at the
*               problems I found in the original implementation
*       Mark Wagner  25 Aug 2001 : Changed interpolation method to pre-compute
*               coefficients for cubic splines.
*       Mark Wagner  24 Feb 2002 : Added support for Catmull-Rom interpolation
*               Re-arranged the code to make future additions cleaner by moving
*                more of the code into the "switch" statement
*
******************************************************************************/

DBL Get_Spline_Val(SPLINE *sp, DBL p, EXPRESS v, int *Terms)
{
    int i, k;
    int last;
    SPLINE_ENTRY * se;

    *Terms = sp->Terms;

    if(!sp->Coeffs_Computed)
    {
        switch(sp->Type)
        {
            case NATURAL_SPLINE:
                Precompute_Cubic_Coeffs(sp);
                break;
            default:
                break;
        }
    }

	// check if the value is in the cache
	if((sp->Cache_Point == p) && (sp->Cache_Type == sp->Type))
	{
		if(sp->Cache_Valid == true) // doing this here is more efficient as it is rarely false [trf]
		{
			Assign_Express(v, sp->Cache_Data);
			return sp->Cache_Data[0];
		}
	}

	// init some cache data
	sp->Cache_Valid = false;
	sp->Cache_Type = sp->Type;
	sp->Cache_Point = p;

    last = sp->Number_Of_Entries-1;
    se = sp->SplineEntries;
    
    if(last == 0)
    {/* if only one entry then return this */
        for(k=0; k<5; k++)
            v[k] = se[0].vec[k];
        return se[0].vec[0];
    }

    /* Find which spline segment we're in.  i is the control point at the end of the segment */
    i = findt(sp, p);

    switch(sp->Type)
    {
        case LINEAR_SPLINE:
            for(k=0; k<5; k++)
            {
                /* If outside spline range, return first or last point */
                if(i == 0)
                    v[k] = se[0].vec[k];
                else if(i > last)
                    v[k] = se[last].vec[k];
                /* Else, normal case */
                else
                    v[k] = linear_interpolate(se, i-1, k, p);
            }
            break;
        case QUADRATIC_SPLINE:
            for(k=0; k<5; k++)
            {
                /* If outside the spline range, return the first or last point */
                if(i == 0)
                    v[k] = se[0].vec[k];
                else if(i > last)
                    v[k] = se[last].vec[k];
                /* If not enough points, reduce order */
                else if(last == 1)
                    v[k] = linear_interpolate(se, i-1, k, p);
                /* Normal case: between the second and last points */
                else if(i > 1)
                {
                    v[k] = quadratic_interpolate(se, i-1, k, p);
                }
                else /* Special case: between first and second points */
                {
                    v[k] = quadratic_interpolate(se, i, k, p);
                }
            }
            break;
        case NATURAL_SPLINE:
            for(k=0; k<5; k++)
            {
                /* If outside the spline range, return the first or last point */
                if(i == 0)
                    v[k] = se[0].vec[k];
                else if(i > last)
                    v[k] = se[last].vec[k];
                /* Else, normal case.  cubic_interpolate can handle the case of not enough points */
                else
                    v[k] = natural_interpolate(se, i-1, k, p);
            }
            break;
        case CATMULL_ROM_SPLINE:
            for(k=0; k<5; k++)
            {
                /* If only two points, return their average */
                if(last == 1)
                    v[k] = (se[0].vec[k] + se[1].vec[k])/2.0;
                /* Catmull-Rom: If only three points, return the second one */
                /* Catmull-Rom: Can't interpolate before second point or after next-to-last */
                else if(i < 2)
                    v[k] = se[1].vec[k];
                else if(i >= last)
                    v[k] = se[last-1].vec[k];
                /* Else, normal case */
                else
                    v[k] = catmull_rom_interpolate(se, i-1, k, p);
            }
            break;
        default:
            Error("Unknown spline type %d found.\n", sp->Type);

    }

	// put data in cache
	Assign_Express(sp->Cache_Data, v);
	sp->Cache_Valid = true;

    return v[0];
}

END_POV_NAMESPACE