/******************************************************************************
* Bsp_knot.c - Various bspline routines to handle knot vectors.		      *
*******************************************************************************
* Written by Gershon Elber, Aug. 90.					      *
******************************************************************************/

#ifdef __MSDOS__
#include <stdlib.h>
#endif /* __MSDOS__ */

#include <ctype.h>
#include <stdio.h>
#include <string.h>
#include "cagd_loc.h"

/******************************************************************************
* Returns TRUE iff The given knot vector has open end conditions.	      *
******************************************************************************/
CagdBType BspKnotHasOpenEC(CagdRType *KnotVector, int Len, int Order)
{
    int i,
        LastIndex = Len + Order - 1;

    for (i = 1; i < Order; i++)
       if (!APX_EQ(KnotVector[0], KnotVector[i])) return FALSE;

    for (i = LastIndex - 1; i >= Len; i--)
       if (!APX_EQ(KnotVector[LastIndex], KnotVector[i])) return FALSE;

    return TRUE;
}

/******************************************************************************
* Returns TRUE iff t is in the parametric domain as define by the knot vector *
* KnotVector its length Len and the order Order.			      *
******************************************************************************/
CagdBType BspKnotParamInDomain(CagdRType *KnotVector, int Len, int Order,
								   CagdRType t)
{
    CagdRType
	r1 = KnotVector[Order - 1],
	r2 = KnotVector[Len];

    return (r1 < t || APX_EQ(r1, t)) && (r2 > t || APX_EQ(r2, t));
}

/******************************************************************************
* Return the index of the last knot which is less than or equal to t in the   *
* given knot vector KnotVector of length Len. APX_EQ is used for equality.    *
* Parameter t is assumed to be in the parametric domain for the knot vector.  *
******************************************************************************/
int BspKnotLastIndexLE(CagdRType *KnotVector, int Len, CagdRType t)
{
    int i;

    for (i = 0;
	 i < Len && (KnotVector[i] <= t || APX_EQ(KnotVector[i], t));
	 i++);

    return i - 1;
}

/******************************************************************************
* Return the index of the last knot which is less than t in the given knot    *
* vector KnotVector of length Len.					      *
* Parameter t is assumed to be in the parametric domain for the knot vector.  *
******************************************************************************/
int BspKnotLastIndexL(CagdRType *KnotVector, int Len, CagdRType t)
{
    int i;

    for (i = 0;
	 i < Len && KnotVector[i] < t && !APX_EQ(KnotVector[i], t);
	 i++);

    return i - 1;
}

/******************************************************************************
* Return the index of the first knot which is greater than t in given knot    *
* vector KnotVector of length Len.					      *
* Parameter t is assumed to be in the parametric domain for the knot vector.  *
******************************************************************************/
int BspKnotFirstIndexG(CagdRType *KnotVector, int Len, CagdRType t)
{
    int i;

    for (i = Len - 1;
	 i >= 0 && KnotVector[i] > t && !APX_EQ(KnotVector[i], t);
	 i--);

    return i + 1;
}

/******************************************************************************
* Return a uniform float knot vector for Len Control points and order Order.  *
* If KnotVector is NULL it is being allocated dynamically.		      *
******************************************************************************/
CagdRType *BspKnotUniformFloat(int Len, int Order, CagdRType *KnotVector)
{
    int i;
    CagdRType *KV;

    if (KnotVector == NULL)
	KnotVector = KV = (CagdRType *) CagdMalloc(sizeof(CagdRType) *
								(Len + Order));
    else
	KV = KnotVector;

    for (i = 0; i < Len + Order; i++) *KV++ = (CagdRType) i;

    return KnotVector;
}

/******************************************************************************
* Return a uniform open knot vector for Len Control points and order Order.   *
* If KnotVector is NULL it is being allocated dynamically.		      *
******************************************************************************/
CagdRType *BspKnotUniformOpen(int Len, int Order, CagdRType *KnotVector)
{
    int i, j;
    CagdRType *KV;

    if (KnotVector == NULL)
	KnotVector = KV = (CagdRType *) CagdMalloc(sizeof(CagdRType) *
								(Len + Order));
    else
	KV = KnotVector;

    for (i = 0; i < Order; i++) *KV++ = 0.0;
    for (i = 1, j = Len - Order; i <= j; ) *KV++ = (CagdRType) i++;
    for (j = 0; j < Order; j++) *KV++ = (CagdRType) i;

    return KnotVector;
}

/******************************************************************************
* Apply an affine transformation to the given knot vector. Note affine        *
* transformation for the knot vector does not change the spline.	      *
* Knot vector is translated by Translate amount and scaled by Scale.
******************************************************************************/
void BspKnotAffineTrans(CagdRType *KnotVector, int Len,
					CagdRType Translate, CagdRType Scale)
{
    int i;

    for (i = 0; i < Len; i++)
	KnotVector[i] = (KnotVector[i] + Translate) * Scale;
}

/******************************************************************************
* Creates an identical copy of a given knot vector KnotVector of length Len.  *
******************************************************************************/
CagdRType *BspKnotCopy(CagdRType *KnotVector, int Len)
{
    CagdRType
	*NewKnotVector = (CagdRType *) CagdMalloc(sizeof(CagdRType) * Len);

    GEN_COPY(NewKnotVector, KnotVector, sizeof(CagdRType) * Len);

    return NewKnotVector;
}

/******************************************************************************
* Reverse a knot vector of length Len.					      *
******************************************************************************/
CagdRType *BspKnotReverse(CagdRType *KnotVector, int Len)
{
    int i;
    CagdRType
	*NewKnotVector = (CagdRType *) CagdMalloc(sizeof(CagdRType) * Len),
	t = KnotVector[Len - 1] + KnotVector[0];

    for (i = 0; i < Len; i++)
	NewKnotVector[i] = t - KnotVector[Len - i - 1];

    return NewKnotVector;
}

/******************************************************************************
* Returns all the knots in KnotVector1 not in KnotVector2.		      *
* NewLen is set to new KnotVector length.				      *
******************************************************************************/
CagdRType *BspKnotSubtrTwo(CagdRType *KnotVector1, int Len1,
			   CagdRType *KnotVector2, int Len2,
			   int *NewLen)
{
    int i = 0,
	j = 0;
    CagdRType
	*NewKnotVector = (CagdRType *) CagdMalloc(sizeof(CagdRType) * Len1),
	*t = NewKnotVector;

    *NewLen = 0;
    while (i < Len1 && j < Len2) {
	if (APX_EQ(KnotVector1[i], KnotVector2[j])) {
	    i++;
	    j++;
	}
	else if (KnotVector1[i] > KnotVector2[j]) {
	    j++;
	}
	else {
	    /* Current KnotVector1 value is less than current KnotVector2: */
	    *t++ = KnotVector1[i++];
	    (*NewLen)++;
	}
    }

    return NewKnotVector;
}

/******************************************************************************
* Merge two knot vector KnotVector1 and KnotVector2 of length Len1 and Len2   *
* respectively into one. If Mult is not zero then knot multiplicity is tested *
* not to be bigger than Mult value. NewLen is set to new KnotVector length.   *
******************************************************************************/
CagdRType *BspKnotMergeTwo(CagdRType *KnotVector1, int Len1,
			   CagdRType *KnotVector2, int Len2,
			   int Mult, int *NewLen)
{
    int i = 0,
	j = 0,
	k = 0,
        m = 0,
	Len = Len1 + Len2;
    CagdRType t,
	*NewKnotVector = (CagdRType *) CagdMalloc(sizeof(CagdRType) * Len);

    if (Mult == 0) Mult = Len + 1;

    while (i < Len1 && j < Len2) {
	if (KnotVector1[i] < KnotVector2[j]) {
	    /* Use value from KnotVector1: */
	    t = KnotVector1[i++];
	}
	else {
	    /* Use value from KnotVector2: */
	    t = KnotVector2[j++];
	}

	if (k == 0 || k > 0 && !APX_EQ(NewKnotVector[k - 1], t)) {
	    NewKnotVector[k++] = t;
	    m = 1;
	}
	else if (m < Mult) {
	    NewKnotVector[k++] = t;
	    m++;
	}
    }

    /* It should be noted that k <= Len so there is a chance some of the new */
    /* KnotVector space will not be used (it is not memory leak!). If you    */
    /* really care that much - copy it to a new allocated vector of size k   */
    /* and return it while freeing the original of size Len.		     */
    *NewLen = k;

    return NewKnotVector;
}

/******************************************************************************
* Creates a new vector with the given KnotVector Node values.		      *
* The nodes are the approximated parametric value associated with the each    *
* control point. Therefore for a knot vector of length Len and order Order    *
* there are Len - Order control points and therefore nodes.		      *
* Nodes are defined as (k = Order - 1 or degree):			      *
*	    								      *
*	  i+k								      *
*	   -			 	First Node N(i = 0)		      *
*	   \								      *
*          / KnotVector(j)		Last Node  N(i = Len - k - 2)	      *
*	   -								      *
*        j=i+1								      *
* N(i) = -----------------						      *
*	        k							      *
******************************************************************************/
CagdRType *BspKnotNodes(CagdRType *KnotVector, int Len, int Order)
{
    int i, j,
	k = Order - 1,
	NodeLen = Len - Order;
    CagdRType
	*NodeVector = (CagdRType *) CagdMalloc(sizeof(CagdRType) * NodeLen);

    for (i = 0; i < NodeLen; i++) {
	NodeVector[i] = 0.0;
	for (j = i + 1; j <= i + k; j++) NodeVector[i] += KnotVector[j];
	NodeVector[i] /= k;
    }

    return NodeVector;
}

/******************************************************************************
* Creates a new vector with t inserted as a new knot. Attempt is made to make *
* sure t is in the knot vector domain.					      *
* No test is made for the current multiplicity for knot t in KnotVector.      *
******************************************************************************/
CagdRType *BspKnotInsertOne(CagdRType *KnotVector, int Order, int Len,
								CagdRType t)
{
    int Mult = BspKnotFindMult(KnotVector, Order, Len, t) + 1;

    return BspKnotInsertMult(KnotVector, Order, &Len, t, Mult);
}

/******************************************************************************
* Inserts Mult knots with value t into the knot vector KnotVector.	      *
* Attempt is made to make sure t in knot vector domain.			      *
* If a knot equal to t (up to APX_EQ) already exists with multiplicity i only *
* Mult - i knot are been inserted into the new knot vector.		      *
* Len is updated to the resulting knot vector.				      *
* Note it is possible to DELETE a knot using this routine by specifying       *
* multiplicity less then current multiplicity!				      *
******************************************************************************/
CagdRType *BspKnotInsertMult(CagdRType *KnotVector, int Order, int *Len,
						        CagdRType t, int Mult)
{
    int i, CurrentMult, NewLen, FirstIndex;
    CagdRType *NewKnotVector;

    if (!BspKnotParamInDomain(KnotVector, *Len, Order, t))
	FATAL_ERROR(CAGD_ERR_T_NOT_IN_CRV);

    CurrentMult = BspKnotFindMult(KnotVector, Order, *Len, t);
    NewLen = *Len + Mult - CurrentMult;
    NewKnotVector = (CagdRType *) CagdMalloc(sizeof(CagdRType) *
    							(NewLen + Order));
    FirstIndex = BspKnotLastIndexL(KnotVector, *Len + Order, t) + 1;

    /* Copy all the knot before the knot t. */
    GEN_COPY(NewKnotVector, KnotVector, sizeof(CagdRType) * FirstIndex);

    /* Insert Mult knot of value t. */
    for (i = FirstIndex; i < FirstIndex + Mult; i++)
	NewKnotVector[i] = t;

    /* And copy the second part. */
    GEN_COPY(&NewKnotVector[FirstIndex + Mult],
	     &KnotVector[FirstIndex + CurrentMult],
	     sizeof(CagdRType) * (*Len + Order - FirstIndex - Mult + 1));

    *Len = NewLen;
    return NewKnotVector;
}

/******************************************************************************
* Returns the multiplicity of knot t in knot vector KnotVector, zero if none. *
******************************************************************************/
int BspKnotFindMult(CagdRType *KnotVector, int Order, int Len, CagdRType t)
{
    int LastIndex, FirstIndex;

    if (!BspKnotParamInDomain(KnotVector, Len, Order, t))
	FATAL_ERROR(CAGD_ERR_T_NOT_IN_CRV);

    FirstIndex = BspKnotLastIndexL(KnotVector, Len, t) + 1;

    for (LastIndex = FirstIndex;
	 LastIndex < Len && APX_EQ(KnotVector[LastIndex], t);
	 LastIndex++);

    return LastIndex - FirstIndex;
}

/******************************************************************************
* Scans the given knot vector to a potential C1 discontinuity.		      *
*   Returns TRUE if found one and set t to its parameter value.		      *
*   Assumes knot vector has open end condition.				      *
******************************************************************************/
CagdBType BspKnotC1Discont(CagdRType *KnotVector, int Order, int Length,
								CagdRType *t)
{
    int i, Count;
    CagdRType
	LastT = KnotVector[0] - 1.0;

    for (i = Order, Count = 0; i < Length; i++) {
	if (APX_EQ(LastT, KnotVector[i]))
	    Count++;
	else {
	    Count = 1;
	    LastT = KnotVector[i];
	}

	if (Count >= Order - 1) {
	    *t = LastT;
	    return TRUE;
	}
    }

    return FALSE;
}

/******************************************************************************
* Scans the given knot vector to aall potential C1 discontinuity. Returns     *
* a vector holding the parameter values of C1 discontinuities, NULL of non    *
* found.								      *
*   Sets n to length of returned vector.				      *
*   Assumes knot vector has open end condition.				      *
******************************************************************************/
CagdRType *BspKnotAllC1Discont(CagdRType *KnotVector, int Order, int Length,
									int *n)
{
    int i, Count,
	C1DiscontCount = 0;
    CagdRType
	LastT = KnotVector[0] - 1.0,
	*C1Disconts = (CagdRType *) CagdMalloc(sizeof(CagdRType) * Length);

    for (i = Order, Count = 0; i < Length; i++) {
	if (APX_EQ(LastT, KnotVector[i]))
	    Count++;
	else {
	    Count = 1;
	    LastT = KnotVector[i];
	}

	if (Count >= Order - 1) {
	    C1Disconts[C1DiscontCount++] = LastT;
	    Count = 0;
	}
    }

    if ((*n = C1DiscontCount) > 0) {
	return C1Disconts;
    }
    else {
	CagdFree((VoidPtr) C1Disconts);
	return NULL;
    }
}

/*****************************************************************************
* Routine to determine where to sample along the provided paramtric domain,  *
* given the C1 discontinuities along it.				     *
*   Returns a vector of length NumSamples.				     *
*   If C1Disconts != NULL (NumC1Disconts > 0) this vector is being freed.    *
*****************************************************************************/
CagdRType *BspKnotParamValues(CagdRType PMin, CagdRType PMax, int NumSamples,
			      CagdRType *C1Disconts, int NumC1Disconts)
{
    int i, CrntIndex, NextIndex, CrntDiscont;
    CagdRType Step,
	*Samples = (CagdRType *) CagdMalloc(sizeof(CagdRType) * NumSamples);

    Samples[0] = PMin;
    if (NumSamples <= 1) return Samples;
    Samples[NumSamples - 1] = PMax;
    if (NumSamples <= 2) return Samples;

    if (NumC1Disconts > NumSamples - 2) {
    	/* More C1 discontinuities than we are sampling. Grab a subset of    */
	/* The discontinuities as the sampling set.			     */
    	Step = ((CagdRType) (NumC1Disconts - 1)) / (NumSamples - 2) - EPSILON;
	for (i = 0; i < NumSamples - 2; i++)
    	    Samples[i + 1] = C1Disconts[(int) (i * Step)];
    }
    else {
	/* More samples than C1 discontinuites. Uniformly distribute the C1  */
	/* discontinuites between the samples and linearly interpolate the   */
	/* sample values in between.					     */
    	Step = ((CagdRType) (NumC1Disconts + 1)) / (NumSamples - 2);
	CrntIndex = CrntDiscont = 0;

	while (CrntDiscont < NumC1Disconts) {
	    NextIndex = (int) ((CrntDiscont + 1) / Step);
	    Samples[NextIndex] = C1Disconts[CrntDiscont++];

	    for (i = CrntIndex + 1; i < NextIndex; i++) {
		CagdRType
		    t = (i - CrntIndex) / ((CagdRType) (NextIndex - CrntIndex)),
		    t1 = 1.0 - t;

		Samples[i] = Samples[CrntIndex] * t1 + Samples[NextIndex] * t;
	    }

	    CrntIndex = NextIndex;
	}

	/* Interpolate the last interval: */
    	for (i = CrntIndex + 1; i < NumSamples - 1; i++) {
    	    CagdRType
    		t = (i - CrntIndex) / ((CagdRType) (NumSamples - 1 - CrntIndex)),
    		t1 = 1.0 - t;

    	    Samples[i] = Samples[CrntIndex] * t1 + Samples[NumSamples - 1] * t;
    	}
    }

    if (C1Disconts != NULL)
	CagdFree((VoidPtr) C1Disconts);

    return Samples;
}