/*****************************************************************************
*   A decomposition of a Bspline curve into multi resolution hierarchy.      *
******************************************************************************
* (C) Gershon Elber, Technion, Israel Institute of Technology                *
******************************************************************************
* Written by:  Gershon Elber				Ver 0.1, June 1993.  *
*****************************************************************************/

#include "irit_sm.h"
#include "cagd_lib.h"
#include "symb_loc.h"

#define MAX_AFFECTED_DIST 4.0

/*****************************************************************************
* DESCRIPTION:                                                               M
* Given a Bspline curve, computes a hierarch of Bspline curves, each being   M
* represented using a subspace of the previous, upto a curve with no         M
* interior knots (i.e. a polynomial Bezier).				     M
*   However, if Discont == TRUE, then C1 discontinuities are preserved	     M
* through out the hierarchy decomposition.				     M
*   Each level in hierarchy has approximately half the number of control     M
* points of the previous one.						     M
*   Least square curve fitting is used to build the hierarchy.		     M
*                                                                            *
* PARAMETERS:                                                                M
*   Crv:       To compute a least square multi resolution decomposition for. M
*   Discont:   Do we want to preserve discontinuities?                       M
*                                                                            *
* RETURN VALUE:                                                              M
*   SymbMultiResCrvStruct *:   A multi resolution curve structure hold the   M
*                              multi resolution decomposition of Crv.        M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbCrvMultiResDecomp, multi resolution, least square decomposition      M
*****************************************************************************/
SymbMultiResCrvStruct *SymbCrvMultiResDecomp(CagdCrvStruct *Crv, int Discont)
{
    CagdBType
	Periodic = CAGD_IS_PERIODIC_CRV(Crv);
    int KVListSize, *KVListSizes, i, j,
	Length = Crv -> Length,
	PrevLen = CAGD_CRV_PT_LST_LEN(Crv),
	Order = Crv -> Order,
	KVMaxSize = CAGD_CRV_PT_LST_LEN(Crv) + Order;
    CagdRType **KVList, *KVPtr, *Params,
	*KVPrev = Crv -> KnotVector;
    SymbMultiResCrvStruct *MRCrv;
    CagdCrvStruct *OCrv;

    if (!CAGD_IS_BSPLINE_CRV(Crv)) {
	SYMB_FATAL_ERROR(SYMB_ERR_BSP_CRV_EXPECT);
	return NULL;
    }

    /* Compute the hierarchy size and allocate its KnotVectors. */
    for (KVListSize = 0; (1 << KVListSize) < PrevLen - Order; KVListSize++);
    KVList = (CagdRType **) IritMalloc(sizeof(CagdRType *) * ++KVListSize);
    KVListSizes = (int *) IritMalloc(sizeof(int) * KVListSize);

    KVList[0] = (CagdRType *) IritMalloc(sizeof(CagdRType) * KVMaxSize);
    KVListSizes[0] = KVMaxSize;
    SYMB_GEN_COPY(KVList[0], KVPrev, sizeof(CagdRType) * KVMaxSize);
    
    for (i = 1; i < KVListSize; i++) {
	KVPtr = KVList[i] = (CagdRType *)
				IritMalloc(sizeof(CagdRType) * KVMaxSize);

	/* Copy first Order knots verbatim. */
	KVListSizes[i] = 2 * Order; /* End conditions are copied verbatim. */
	for (j = 0; j < Order; j++)
	    *KVPtr++ = *KVPrev++;

	/* Skip every second interior knot. */
	for (; j < PrevLen; j++, KVPrev++) {
	    if (Discont) {
		if ((j & 0x01) == 0 ||
		    APX_EQ(KVPrev[-1], KVPrev[0]) ||
		    APX_EQ(KVPrev[0], KVPrev[1])) {
		    *KVPtr++ = *KVPrev;
		    KVListSizes[i]++;
		}
	    }
	    else {
		if ((j & 0x01) == 0) {
		    *KVPtr++ = *KVPrev;
		    KVListSizes[i]++;
		}
	    }
	}

	/* Copy last Order knots verbatim. */
	for (j = 0; j < Order; j++)
	    *KVPtr++ = *KVPrev++;

	KVPrev = KVList[i];
	PrevLen = KVListSizes[i] - Order;

	/* Make sure we did not exhaust all the interior knots already, or  */
	/* alternatively all interior knots maintains discontonuities.      */
	if (Periodic ? PrevLen <= Order + Order - 1 : PrevLen <= Order) {
	    KVListSize -= (KVListSize - i) - 1;
	    if (Periodic ? PrevLen < Order + Order - 1 : PrevLen < Order) {
		IritFree(KVList[i]);
		KVListSize--;
	    }
	    break;
	}
	else if (KVListSizes[i] == KVListSizes[i-1]) {
	    KVListSize -= KVListSize - i;
	    IritFree(KVList[i]);
	    break;
	}
    }

    if (Periodic) {
	/* Make sure spaces are consistent at the ends. */
	for (i = 0; i < KVListSize; i++) {
	    int Len = KVListSizes[i] - Order;
	    CagdRType
		*KV = KVList[i];

	    for (j = 0; j < Order - 1; j++)
		KV[j] = KV[Order - 1] + KV[Len + j - (Order - 1)]
				      - KV[Len];
	    for (j = Len + 1; j < Len + Order; j++)
		KV[j] = KV[Len] + KV[j - Len + Order - 1] - KV[Order - 1];
	}
    }

#ifdef DEBUG_PRINT_KVS
    for (i = 0; i < KVListSize; i++) {
	fprintf(stderr, "KV - LEVEL %d (len = %d) ***********************\n",
		i, KVListSizes[i]);

	for (j = 0; j < KVListSizes[i]; j++)
	    fprintf(stderr, "%0.7lf ", KVList[i][j]);
	fprintf(stderr, "\n\n");
    }
#endif /* DEBUG_PRINT_KVS */

    /* Now compute the curves in reverse, from the smallest subspace to the */
    /* largest and accumulate the representation upto the original space.   */
    Params = CagdCrvNodes(Crv);
    MRCrv = SymbCrvMultiResNew(KVListSize, Periodic);

    if (!BspCrvHasOpenEC(Crv)) { /* We need an open end version of the curve. */
	if (Periodic) {
	    CagdCrvStruct
		*TCrv = CnvrtPeriodic2FloatCrv(Crv);

	    OCrv = CnvrtFloat2OpenCrv(TCrv);
	    CagdCrvFree(TCrv);
	}
	else {
	    OCrv = CnvrtFloat2OpenCrv(Crv);
	}
    }
    else
	OCrv = CagdCrvCopy(Crv);

    for (i = KVListSize - 1; i >= 0; i--) {
	CagdCrvStruct *InterpCrv, *DiffCrv;
	CagdCtlPtStruct
	    *PtList = NULL;

	/* Sample the current curve at its node values to guarantee a       */
	/* unique interpolatory result - identical to the original curve.   */
	for (j = CAGD_CRV_PT_LST_LEN(OCrv) - 1; j >= 0; j--) {
	    CagdCtlPtStruct
		*Pt = CagdCtlPtNew(OCrv -> PType);
	    CagdRType
		*R = BspCrvEvalAtParam(OCrv, Params[j]);

	    SYMB_GEN_COPY(Pt -> Coords, R,
			  sizeof(CagdRType) * CAGD_MAX_PT_SIZE);

	    Pt -> Pnext = PtList;
	    PtList = Pt;
	}

	InterpCrv = BspCrvInterpolate(PtList, Length, Params, KVList[i],
				      KVListSizes[i] - Order -
				          (Periodic ? Order - 1 : 0),
				      Order, Periodic);
	CagdCtlPtFreeList(PtList);

	if (!BspCrvHasOpenEC(InterpCrv)) {
	    CagdCrvStruct *OInterpCrv;

	    if (Periodic) {
		CagdCrvStruct
		    *TCrv = CnvrtPeriodic2FloatCrv(InterpCrv);

		OInterpCrv = CnvrtFloat2OpenCrv(TCrv);
		CagdCrvFree(TCrv);
	    }
	    else {
		OInterpCrv = CnvrtFloat2OpenCrv(InterpCrv);
	    }

	    DiffCrv = SymbCrvSub(OCrv, OInterpCrv);
	    MRCrv -> HieCrv[KVListSize - 1 - i] = OInterpCrv;
	    CagdCrvFree(InterpCrv);
	}
	else {
	    DiffCrv = SymbCrvSub(OCrv, InterpCrv);
	    MRCrv -> HieCrv[KVListSize - 1 - i] = InterpCrv;
	}

	CagdCrvFree(OCrv);
	OCrv = DiffCrv;
    }

    CagdCrvFree(OCrv);
    IritFree((VoidPtr) Params);

    return MRCrv;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Given a multi resolution decomposition of a Bspline curve, computes the    M
* regular Bspline curve out of it.					     *
*                                                                            M
*                                                                            *
* PARAMETERS:                                                                M
*   MRCrv:    A multi resolution decomposition of a curve.                   M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdCrvStruct *:  A curve that adds up all components of the multi       M
*                     resolution decomposition MRCrv.                        M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbCrvMultiResCompos, multi resolution, least square decomposition      M
*****************************************************************************/
CagdCrvStruct *SymbCrvMultiResCompos(SymbMultiResCrvStruct *MRCrv)
{
    return SymbCrvMultiResComposAtT(MRCrv,
				    MRCrv -> Levels - 1 +
					(MRCrv -> RefineLevel != FALSE));
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Given a multi resolution decomposition of a Bspline curve, computes a      M
* regular Bspline curve out of it representing the decomposed curve at       M
* the multi resolution hierarchy level of T.				     M
*   Although decomposition is discrete, T can be any real number between     M
* these discrete levels and a linear interpolation of adjacent levels is     M
* exploited.								     M
*                                                                            *
* PARAMETERS:                                                                M
*   MRCrv:     A multi resolution decomposition of a curve.                  M
*   T:         A mult resolution hierarcy level to compute curve for.        M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdCrvStruct *:  A curve that adds up all components of the multi       M
*                     resolution decomposition MRCrv up to and including     M
*                     level T.                                               M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbCrvMultiResComposAtT, multi resolution, least square decomposition   M
*****************************************************************************/
CagdCrvStruct *SymbCrvMultiResComposAtT(SymbMultiResCrvStruct *MRCrv,
					CagdRType T)
{
    int i,
	It = (int) T;
    CagdCrvStruct
	*SumCrv = CagdCrvCopy(MRCrv -> HieCrv[0]);

    for (i = 1;
	 i <= It && i < MRCrv -> Levels + (MRCrv -> RefineLevel != FALSE);
	 i++) {
	CagdCrvStruct
	    *TCrv = SymbCrvAdd(SumCrv, MRCrv -> HieCrv[i]);

	CagdCrvFree(SumCrv);
	SumCrv = TCrv;
    }

    if (It != T) {
	CagdCrvStruct
	    *TCrv1 = SymbCrvScalarScale(MRCrv -> HieCrv[It + 1], T - It),
	    *TCrv2 = SymbCrvAdd(SumCrv, TCrv1);

	CagdCrvFree(TCrv1);
	CagdCrvFree(SumCrv);
	SumCrv = TCrv2;
    }

    return SumCrv;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Given a multi resolution decomposition of a Bspline curve, edit it by      M
* modifying its Level'th Level according to the TransDir of Position at      M
* parametr t. 								     M
*   Level can be a fraction number between the discrete levels of the        M
* decomposition denoting a linear blend of two neighboring discrete levels.  M
*   Editing is performed in place.					     M
*                                                                            *
* PARAMETERS:                                                                M
*   MRCrv:       A multi resolution decomposition of a curve to edit it in   M
*                place.							     M
*   t:           Parameter value at which to modify MRCrv.                   M
*   TransDir:    Directional tranlation transformation to apply.             M
*   Level:       Of multi resolution hierarchy to edit.                      M
*   FracLevel:   The fraction level to edit - will blend two neighboring     M
*		 levels.						     M
*   SpanDiscont: Are we allowed to cross over discontinuities?		     M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbCrvMultiResEdit, multi resolution, least square decomposition        M
*****************************************************************************/
void SymbCrvMultiResEdit(SymbMultiResCrvStruct *MRCrv,
			 CagdRType t,
			 CagdVType TransDir,
			 CagdRType Level,
			 CagdRType FracLevel)
{
    int ILevel = (int) Level;

    if (Level == ILevel) {
	CagdCrvStruct *Crv, *OrigCrv, *DiffCrv;
	CagdRType *BasisFuncs, **Points;
	int i, Length, Order, IndexFirst;

	if (ILevel < 0 ||
	    ILevel >= MRCrv -> Levels + (MRCrv -> RefineLevel != FALSE)) {
	    SYMB_FATAL_ERROR(SYMB_ERR_OUT_OF_RANGE);
	    return;
	}

	/* Prepare the Level'th curve in the hierarchy. */
	OrigCrv = CagdCrvCopy(MRCrv -> HieCrv[0]);
	for (i = 1; i <= ILevel; i++) {
	    CagdCrvStruct
		*TCrv = SymbCrvAdd(OrigCrv, MRCrv -> HieCrv[i]);

	    CagdCrvFree(OrigCrv);
	    OrigCrv = TCrv;
	}
	Crv = CagdCrvCopy(OrigCrv);

	Points = Crv -> Points;
	Length = Crv -> Length;
	Order = Crv -> Order;

	BasisFuncs = BspCrvCoxDeBoorBasis(Crv -> KnotVector, Order,
					  Length +
					      (Crv -> Periodic ? Order - 1 : 0),
					  t, &IndexFirst);

	for (i = IndexFirst; i < IndexFirst + Order; i++) {
	    CagdRType
		MultFactor = BasisFuncs[i - IndexFirst];

	    switch (Crv -> PType) {
	        case CAGD_PT_E3_TYPE:
	            Points[3][i] += TransDir[2] * MultFactor;
		case CAGD_PT_E2_TYPE:
	            Points[2][i] += TransDir[1] * MultFactor;
	            Points[1][i] += TransDir[0] * MultFactor;
		    break;
		case CAGD_PT_P3_TYPE:
		case CAGD_PT_P2_TYPE:
		    fprintf(stderr, "RATIONALS NOT SUPPORTED\n");
		    exit(1);
		    break;
		default:
		    SYMB_FATAL_ERROR(SYMB_ERR_UNSUPPORT_PT);
		    break;
	    }
	}

	/* Construct the new hierarchy. */
	DiffCrv = SymbCrvSub(Crv, OrigCrv);
	CagdCrvFree(OrigCrv);
	CagdCrvFree(Crv);

	/* Apply the fraction ratio, if necessary. */
	if (!APX_EQ(FracLevel, 1.0)) {
	    Crv = SymbCrvScalarScale(DiffCrv, FracLevel);
	    CagdCrvFree(DiffCrv),
	    DiffCrv = Crv;
	}

	Crv = SymbCrvAdd(MRCrv -> HieCrv[ILevel], DiffCrv);
	CagdCrvFree(MRCrv -> HieCrv[ILevel]);
	MRCrv -> HieCrv[ILevel] = Crv;

	CagdCrvFree(DiffCrv);
    }
    else {
	int Level1 = ILevel,
	    Level2 = Level1 + 1;

	FracLevel = Level - Level1;

	SymbCrvMultiResEdit(MRCrv, t, TransDir, Level1,
			    1.0 - FracLevel),
	SymbCrvMultiResEdit(MRCrv, t, TransDir, Level2, FracLevel);
    }
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Given a multi resolution decomposition of a Bspline curve, refine it at    M
* neighborhood of parameter value t, in place.				     M
*                                                                            *
* PARAMETERS:                                                                M
*   MRCrv:       A multi resolution decomposition of a curve, to refine in   M
*                place.							     M
*   T:           Parameter value at which to refine MRCrv.                   M
*   SpanDiscont: Do we want to refine beyond discontinuities?                M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdRType *:  A pointer to an array of two real numbers holding the      M
*                 domain in MRCrv that was refined.			     M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbCrvMultiResRefineLevel, multi resolution, least square decomposition M
*****************************************************************************/
CagdRType *SymbCrvMultiResRefineLevel(SymbMultiResCrvStruct *MRCrv,
				      CagdRType T,
				      int SpanDiscont)
{
    static CagdRType Domain[2];
    CagdCrvStruct *Crv, *RefCrv;
    CagdRType TMin, TMax, **Points, *KV, *NewKV;
    int i, j, Order, Length, LastLIndex, FirstGIndex, StartIndex,
	NewKVIndex = 0;

    if (!MRCrv -> RefineLevel) {
	/* Do not have a refining level yet - add a zero valued curve now. */
	Crv = MRCrv -> HieCrv[MRCrv -> Levels] =
	    CagdCrvCopy(MRCrv -> HieCrv[MRCrv -> Levels - 1]);
	Points = Crv -> Points;
	for (j = 0; j < Crv -> Length; j++)
	    for (i = 1; i <= CAGD_NUM_OF_PT_COORD(Crv -> PType); i++)
		Points[i][j] = 0.0;
	MRCrv -> RefineLevel = TRUE;
    }
    else {
	Crv = MRCrv -> HieCrv[MRCrv -> Levels];
    }
    Length = Crv -> Length;
    Order = Crv -> Order;

    /* Figure out the knots to insert to the refining curve. */
    KV = Crv -> KnotVector;
    NewKV = (CagdRType *) IritMalloc(sizeof(CagdRType) * (Order + 1) * 2);
    CagdCrvDomain(Crv, &TMin, &TMax);
    LastLIndex = BspKnotLastIndexL(KV, Length + Order, T);
    FirstGIndex = BspKnotFirstIndexG(KV, Length + Order, T);

    /* Compute the new knots to the left. */
    for (StartIndex = 0, i = MAX(0, LastLIndex - Order);
	 i <= LastLIndex;
	 i++) {
	if (!APX_EQ(KV[i], KV[i + 1]))
	    NewKV[NewKVIndex++] = (KV[i] + KV[i + 1]) / 2.0;
	else if (SpanDiscont)
	    StartIndex = NewKVIndex;
    }
    /* Compute the new knots to the right. */
    for (i = FirstGIndex; i < MIN(FirstGIndex + Order, Length + Order); i++) {
	if (!APX_EQ(KV[i], KV[i + 1]))
	    NewKV[NewKVIndex++] = (KV[i] + KV[i + 1]) / 2.0;
	else if (SpanDiscont)
	    break;
    }

#ifdef DEBUG_REFINE_KNOTS
    fprintf(stderr, "\nOld knot vector ********************\n");
    for (i = 0; i < Length + Order; i++)
	fprintf(stderr, "%8lg  ", KV[i]);
    fprintf(stderr, "\nNew knots **************************\n");
    for (i = StartIndex; i < NewKVIndex; i++)
	fprintf(stderr, "%8lg  ", NewKV[i]);
#endif /* DEBUG_REFINE_KNOTS */

    Domain[0] = NewKV[StartIndex];
    Domain[1] = NewKV[NewKVIndex - 1];
    RefCrv = CagdCrvRefineAtParams(Crv, FALSE, &NewKV[StartIndex],
				   NewKVIndex - StartIndex);
    IritFree((VoidPtr) NewKV);

    CagdCrvFree(MRCrv -> HieCrv[MRCrv -> Levels]);
    MRCrv -> HieCrv[MRCrv -> Levels] = RefCrv;

    return Domain;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Given a multi resolution decomposition of a Bspline curve, free it.        M
*                                                                            *
* PARAMETERS:                                                                M
*   MRCrv:       A multi resolution decomposition of a curve to free.        M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbCrvMultiResFree, multi resolution, least square decomposition	     M
*****************************************************************************/
void SymbCrvMultiResFree(SymbMultiResCrvStruct *MRCrv)
{
    int i;

    if (MRCrv == NULL)
	return;

    for (i = 0; i < MRCrv -> Levels + (MRCrv -> RefineLevel != FALSE); i++)
	CagdCrvFree(MRCrv -> HieCrv[i]);

    IritFree((VoidPtr) (MRCrv -> HieCrv));
    IritFree((VoidPtr) MRCrv);
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Allocates a data structure for multi resolution decomposition of a Bspline M
* curve of Levels levels and possiblt periodic.				     M
*                                                                            *
* PARAMETERS:                                                                M
*   Levels:      Number of levels to expect in the decomposition.            M
*   Periodic:    Is the curve periodic?                                      M
*                                                                            *
* RETURN VALUE:                                                              M
*   SymbMultiResCrvStruct *:   A structure to hold a multi resolution        M
*                              decomposition of a curve of Levels levels.    M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbCrvMultiResNew, multi resolution, least square decomposition	     M
*****************************************************************************/
SymbMultiResCrvStruct *SymbCrvMultiResNew(int Levels, CagdBType Periodic)
{
    SymbMultiResCrvStruct
	*MRCrv = (SymbMultiResCrvStruct *)
				     IritMalloc(sizeof(SymbMultiResCrvStruct));

    MRCrv -> Levels = Levels;

    /* Keep one more level for the refining level to come. */
    MRCrv -> HieCrv = (CagdCrvStruct **)
	IritMalloc(sizeof(CagdCrvStruct *) * (Levels + 1));

    MRCrv -> RefineLevel = FALSE;
    MRCrv -> Periodic = Periodic;
    MRCrv -> Pnext = NULL;

    return MRCrv;
}
/*****************************************************************************
* DESCRIPTION:                                                               M
* Given a multi resolution decomposition of a Bspline curve, copy it.        M
*                                                                            *
* PARAMETERS:                                                                M
*   MRCrv:       A multi resolution decomposition of a curve to copy.        M
*                                                                            *
* RETURN VALUE:                                                              M
*   SymbMultiResCrvStruct *:   A duplicated structure of MRCrv.		     M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbCrvMultiResCopy, multi resolution, least square decomposition	     M
*****************************************************************************/
SymbMultiResCrvStruct *SymbCrvMultiResCopy(SymbMultiResCrvStruct *MRCrvOrig)
{
    int i;
    SymbMultiResCrvStruct
	*MRCrv = (SymbMultiResCrvStruct *)
				     IritMalloc(sizeof(SymbMultiResCrvStruct));

    MRCrv -> Levels = MRCrvOrig -> Levels;
    MRCrv -> RefineLevel = MRCrvOrig -> RefineLevel;
    MRCrv -> Pnext = NULL;
    MRCrv -> HieCrv = (CagdCrvStruct **) IritMalloc(sizeof(CagdCrvStruct *) *
						    (MRCrvOrig -> Levels + 1));

    for (i = 0; i < MRCrv -> Levels + (MRCrv -> RefineLevel != FALSE); i++)
	MRCrv -> HieCrv[i] = CagdCrvCopy(MRCrvOrig -> HieCrv[i]);


    return MRCrv;
}