/******************************************************************************
* CBsp-Int.c - Bspline curve interpolation/approximation schemes.	      *
*******************************************************************************
* Written by Gershon Elber, Feb. 94.					      *
******************************************************************************/

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

/*****************************************************************************
* DESCRIPTION:                                                               M
* Given a set of points, PtList, computes a Bspline curve of order Order     M
* that interpolates or least square approximates the set of points.	     M
*   The size of the control polygon of the resulting Bspline curve defaults  M
* to the number of points in PtList (if CrvSize = 0).			     M
*   However, this number is can smaller to yield a least square              M
* approximation.							     M
*   The created curve can be parametrized as specified by ParamType.	     M
*                                                                            M
*                                                                            *
* PARAMETERS:                                                                M
*   PtList:       List of points to interpolate/least square approximate.    M
*   Order:        Of interpolating/approximating curve.                      M
*   CrvSize:      Number of degrees of freedom (control points) of the       M
*                 interpolating/approximating curve.                         M
*   ParamType:    Type of parametrization.                                   M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdCrvStruct *:   Constructed interpolating/approximating curve.        M
*                                                                            *
* KEYWORDS:                                                                  M
*   BspCrvInterpPts, interpolation, least square approximation               M
*****************************************************************************/
CagdCrvStruct *BspCrvInterpPts(CagdPtStruct *PtList,
			       int Order,
			       int CrvSize,
			       CagdParametrizationType ParamType)
{
    int i, NumPts;
    CagdPtStruct *Pt;
    CagdRType *KV, *PtKnots, *AveKV, *R, *R2, t;
    CagdCrvStruct *Crv;
    CagdCtlPtStruct
	*CtlPt = NULL,
	*CtlPtList = NULL;

    for (NumPts = 0, Pt = PtList; Pt != NULL; NumPts++, Pt = Pt -> Pnext);
    if (CrvSize == 0)
	CrvSize = NumPts;
    if (NumPts < 3 || NumPts < Order || NumPts < CrvSize || CrvSize < Order)
	return NULL;

    R = PtKnots = (CagdRType *) IritMalloc(sizeof(CagdRType) * NumPts);

    /* Compute parameter values at which the curve will interpolate PtList. */
    switch (ParamType) {
	case CAGD_CHORD_LEN_PARAM:
	    *R++ = 0.0;
	    for (Pt = PtList; Pt -> Pnext != NULL; Pt = Pt -> Pnext, R++)
		*R = *(R - 1) + PT_PT_DIST(Pt -> Pt, Pt -> Pnext -> Pt);
	    for (t = R[-1]; R != PtKnots; *--R /= t);
	    break;
	case CAGD_CENTRIPETAL_PARAM:
	    *R++ = 0.0;
	    for (Pt = PtList; Pt -> Pnext != NULL; Pt = Pt -> Pnext, R++)
		*R = *(R - 1) + sqrt(PT_PT_DIST(Pt -> Pt, Pt -> Pnext -> Pt));
	    for (t = R[-1]; R != PtKnots; *--R /= t);
	    break;
	case CAGD_UNIFORM_PARAM:
	default:
	    for (i = 0; i < NumPts; i++)
		*R++ = ((RealType) i) / (NumPts - 1);
	    break;
    }

    /* Construct the knot vector of the Bspline curve. */
    KV = (CagdRType *) IritMalloc(sizeof(CagdRType) * (CrvSize + Order));

    AveKV = BspKnotAverage(PtKnots, NumPts, NumPts + Order - CrvSize - 1);
    BspKnotAffineTrans(AveKV, CrvSize - Order + 2, PtKnots[0] - AveKV[0],
		       (PtKnots[NumPts - 1] - PtKnots[0]) /
			   (AveKV[CrvSize - Order + 1] - AveKV[0]));

    for (i = 0, R = KV, R2 = AveKV; i < Order; i++)
	*R++ = *R2;
    for (i = 0; i < CrvSize - Order; i++)
	*R++ = *++R2;
    for (i = 0, R2++; i < Order; i++)
	*R++ = *R2;

    IritFree((VoidPtr) AveKV);

    /* Convert to control points in a linear list */
    for (Pt = PtList; Pt != NULL; Pt = Pt -> Pnext) {
	if (CtlPtList == NULL)
	    CtlPtList = CtlPt = CagdCtlPtNew(CAGD_PT_E3_TYPE);
	else {
	    CtlPt ->Pnext = CagdCtlPtNew(CAGD_PT_E3_TYPE);
	    CtlPt = CtlPt -> Pnext;
	}
	for (i = 0; i < 3; i++)
	    CtlPt -> Coords[i + 1] = Pt -> Pt[i];
    }
    Crv = BspCrvInterpolate(CtlPtList, NumPts, PtKnots, KV, CrvSize, Order,
			    FALSE);
    CagdCtlPtFreeList(CtlPtList);

    IritFree((VoidPtr *) PtKnots);
    IritFree((VoidPtr *) KV);

    return Crv;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Given set of points, PtList, parameter values the curve should interpolate M
* or approximate these points, Params, and the expected knot vector, KV,     M
* length Length and order Order of the Bspline curve, computes the Bspline   M
* curve's coefficients.							     M
*   All points in PtList are assumed of the same type.			     M
*   If Periodic, Order - 1 more constraints (and DOF's) are added so that    M
* the first Order - 1 points are the same as the last Order - 1 points.	     M
*                                                                            *
* PARAMETERS:                                                                M
*   PtList:       List of points to interpolate/least square approximate.    M
*   NumPts:       Size of PtList.
*   Params:       At which to interpolate the points in PtList.              M
*   KV:           Computed knot vector for the constructed curve.            M
*   Length:       Number of degrees of freedom (control points) of the       M
*                 interpolating/approximating curve.                         M
*   Order:        Of interpolating/approximating curve.                      M
*   Periodic:     If dat a and hence constructed curve should be Periodic.   M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdCrvStruct *:   Constructed interpolating/approximating curve.        M
*                                                                            *
* KEYWORDS:                                                                  M
*   BspCrvInterpolate, interpolation, least square approximation             M
*****************************************************************************/
CagdCrvStruct *BspCrvInterpolate(CagdCtlPtStruct *PtList,
				 int NumPts,
				 CagdRType *Params,
				 CagdRType *KV,
				 int Length,
				 int Order,
				 CagdBType Periodic)
{
    int i, j, k,
	PerNumPts = NumPts + (Periodic ? Order - 1 : 0);
    CagdCtlPtStruct *Pt;
    CagdRType *Mat, *InterpPts, *M, *R;
    CagdCrvStruct *Crv;
    CagdPointType
	PtType = PtList -> PtType;

    /* Construct the Bspline curve and its knot vector. */
    Crv = BspPeriodicCrvNew(Length, Order, Periodic, PtType);
    CAGD_GEN_COPY(Crv -> KnotVector, KV,
		  (CAGD_CRV_PT_LST_LEN(Crv) + Order) * sizeof(CagdRType));

    /* Construct the system of equations' matrix. */
    M = Mat = (CagdRType *) IritMalloc(sizeof(CagdRType) * Length * PerNumPts);
    for (i = 0, R = Params; i < PerNumPts; i++, R++, M += Length) {
	int Index;
	CagdRType
	    *Line = BspCrvCoxDeBoorBasis(KV, Order,
					 Length + (Periodic ? Order - 1 : 0),
					 *R, &Index);

	for (k = 0; k < Length; k++)
	    M[k] = 0.0;
	for (j = Order, k = Index; j > 0; j--, k++)
	    M[k % Length] = *Line++;
    }

    /* Compute SVD decompostion for Mat. */
    SvdLeastSqr(Mat, NULL, NULL, PerNumPts, Length);
    IritFree((VoidPtr *) Mat);

    /* Solve for the coefficients of all the coordinates of the curve. */
    InterpPts = (CagdRType *) IritMalloc(sizeof(CagdRType) * PerNumPts);
    for (i = !CAGD_IS_RATIONAL_PT(PtType);
	 i <= CAGD_NUM_OF_PT_COORD(PtType);
	 i++) {
	for (Pt = PtList, R = InterpPts, j = PerNumPts;
	     j > 0;
	     Pt = Pt -> Pnext, j--)
	    *R++ = Pt -> Coords[i];

	SvdLeastSqr(NULL, Crv -> Points[i], InterpPts, NumPts, Length);
    }
    IritFree((VoidPtr *) InterpPts);

    return Crv;
}


/*****************************************************************************
* DESCRIPTION:                                                               M
* Given a set of points, and a curve least square fitting them using the     M
* BspCrvInterpPts function, computes an error measure as a the maximal	     M
* distance between the curve and points (L1 norm).			     M
*                                                                            *
* PARAMETERS:                                                                M
*   Crv:        Curve that was fitted to the data set.                       M
*   PtList:     The data set.                                                M
*   ParamType:  Parameter values at with curve should interpolate PtList.    M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdRType:   Error measured in the L1 norm.                              M
*                                                                            *
* KEYWORDS:                                                                  M
*   BspCrvInterpPtsError, error estimation, interpolation, least square      M
*   approximation                                                            M
*****************************************************************************/
CagdRType BspCrvInterpPtsError(CagdCrvStruct *Crv,
			       CagdPtStruct *PtList,
			       CagdParametrizationType ParamType)
{
    int i, NumPts;
    CagdPtStruct *Pt;
    CagdRType *PtKnots, *R, t,
	MaxDist = 0.0;

    for (NumPts = 0, Pt = PtList; Pt != NULL; NumPts++, Pt = Pt -> Pnext);

    R = PtKnots = (CagdRType *) IritMalloc(sizeof(CagdRType) * NumPts);

    /* Compute parameter values at which the curve will interpolate PtList. */
    switch (ParamType) {
	case CAGD_CHORD_LEN_PARAM:
	    *R++ = 0.0;
	    for (Pt = PtList; Pt -> Pnext != NULL; Pt = Pt -> Pnext, R++)
		*R = *(R - 1) + PT_PT_DIST(Pt -> Pt, Pt -> Pnext -> Pt);
	    for (t = R[-1]; R != PtKnots; *--R /= t);
	    break;
	case CAGD_CENTRIPETAL_PARAM:
	    *R++ = 0.0;
	    for (Pt = PtList; Pt -> Pnext != NULL; Pt = Pt -> Pnext, R++)
		*R = *(R - 1) + sqrt(PT_PT_DIST(Pt -> Pt, Pt -> Pnext -> Pt));
	    for (t = R[-1]; R != PtKnots; *--R /= t);
	    break;
	case CAGD_UNIFORM_PARAM:
	default:
	    for (i = 0; i < NumPts; i++)
		*R++ = ((RealType) i) / (NumPts - 1);
	    break;
    }

    for (i = 0, Pt = PtList; i < NumPts; i++, Pt = Pt -> Pnext) {
	CagdRType Dist;

	R = CagdCrvEval(Crv, PtKnots[i]);
	R = &R[1];

	Dist = PT_PT_DIST(R, Pt->Pt);
	if (Dist > MaxDist)
	    MaxDist = Dist;
    }

    return MaxDist;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
*   Computes zero and first moments of a curve.                              M
*                                                                            *
* PARAMETERS:                                                                M
*   Crv:     To compute zero and first moment.                               M
*   n:       Number of samples the curve should be sampled at.               M
*   Loc:     Center of curve as zero moment.                                 M
*   Dir:     Main direction of curve as first moment.                        M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   CagdCrvFirstMoments                                                      M
*****************************************************************************/
void CagdCrvFirstMoments(CagdCrvStruct *Crv,
			 int n,
			 CagdPType Loc,
			 CagdVType Dir)
{
    int i;
    CagdRType t, TMin, TMax, Dt, **Points;
    CagdPtStruct *Pt,
	*PtList = NULL;
    CagdCrvStruct *InterpCrv;

    if (n < 2)
	n = 2;

    CagdCrvDomain(Crv, &TMin, &TMax);
    t = TMin;
    Dt = (TMax - TMin) / (n - 1);
    for (i = 0; i < n; i++, t += Dt) {
	RealType
	    *R = CagdCrvEval(Crv, t);

	Pt = CagdPtNew();
	CagdCoerceToE3(Pt -> Pt, &R, -1, Crv -> PType);
	LIST_PUSH(Pt, PtList);
    }

    InterpCrv = BspCrvInterpPts(PtList, 2, 2, CAGD_UNIFORM_PARAM);
    CagdPtFreeList(PtList);
    Points = InterpCrv -> Points;

    Loc[0] = (Points[1][0] + Points[1][1]) / 2.0;
    Loc[1] = (Points[2][0] + Points[2][1]) / 2.0;
    Loc[2] = (Points[3][0] + Points[3][1]) / 2.0;

    Dir[0] = (Points[1][1] - Points[1][0]);
    Dir[1] = (Points[2][1] - Points[2][0]);
    Dir[2] = (Points[3][1] - Points[3][0]);
    PT_NORMALIZE(Dir);

    CagdCrvFree(InterpCrv);
}
