/******************************************************************************
* Arc_len.c - arc length functions and unit length normalization.	      *
*******************************************************************************
* Written by Gershon Elber, Apr. 93.					      *
******************************************************************************/

#include "symb_loc.h"

#define MAX_ALEN_IMPROVE_ITERS 5
#define ARCLEN_CONST_SET_EPSILON 0.001

/*****************************************************************************
* DESCRIPTION:                                                               M
* Subdivides the given curves to curves, each with size of control polygon   M
* less than or equal to MaxLen.	Returned is a list of curves.		     M
*                                                                            *
* PARAMETERS:                                                                M
*   Crv:        To subdivide into curves, each with control polygon length   M
*               less than MaxLen.					     M
*   MaxLen:     Maximum length of control polygon to allow.                  M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdCrvStruct *:   List of subdivided curves from Crv, each with control M
*                      polygon size of less than MaxLen.                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbLimitCrvArcLen, arc length                                           M
*****************************************************************************/
CagdCrvStruct *SymbLimitCrvArcLen(CagdCrvStruct *Crv, CagdRType MaxLen)
{
    if (SymbCrvArcLenPoly(Crv) > MaxLen) {
	CagdRType TMin, TMax;
	CagdCrvStruct *Crv1, *Crv2, *Crv1MaxLen, *Crv2MaxLen;

	CagdCrvDomain(Crv, &TMin, &TMax);

	Crv1 = CagdCrvSubdivAtParam(Crv, (TMin + TMax) / 2.0);
	Crv2 = Crv1 -> Pnext;

	Crv1MaxLen = SymbLimitCrvArcLen(Crv1, MaxLen);
	Crv2MaxLen = SymbLimitCrvArcLen(Crv2, MaxLen);

	CagdCrvFree(Crv1);
	CagdCrvFree(Crv2);

	for (Crv1 = Crv1MaxLen; Crv1 -> Pnext != NULL; Crv1 = Crv1 -> Pnext);
	Crv1 -> Pnext = Crv2MaxLen;
	return Crv1MaxLen;
    }
    else
        return CagdCrvCopy(Crv);
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Computes a bound on the arc length of a curve by computing the length of   M
* its control polygon.							     M
*                                                                            *
* PARAMETERS:                                                                M
*   Crv:       To bound its length.                                          M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdRType:   An upper bound on the curve Crv length as the length of     M
*                Crv's control polygon.                                      M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbCrvArcLenPoly, arc length                                            M
*****************************************************************************/
CagdRType SymbCrvArcLenPoly(CagdCrvStruct *Crv)
{
    int i;
    CagdCrvStruct
	*CrvE3 = CagdCoerceCrvTo(Crv, CAGD_PT_E3_TYPE);
    CagdRType
	Len = 0.0,
	**Points = CrvE3 -> Points;    

    for (i = 1; i < CrvE3 -> Length; i++)
        Len += sqrt(SQR(Points[1][i] - Points[1][i - 1]) +
		    SQR(Points[2][i] - Points[2][i - 1]) +
		    SQR(Points[3][i] - Points[3][i - 1]));

    CagdCrvFree(CrvE3);

    return Len;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Normalizes the given vector field curve to be a unit length curve, by      M
* computing a scalar curve to multiply with this vector field curve.         M
*   Returns the multiplied curve if Mult, or otherwise just the scalar       M
* curve.								     M
*                                                                            *
* PARAMETERS:                                                                M
*   OrigCrv:     Curve to approximate a unit size for.			     M
*   Mult:        Do we want to multiply the computed scalar curve with Crv?  M
*   Epsilon:     Accuracy required of this approximation.                    M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdCrvStruct *:  A scalar curve to multiply OrigCrv so a unit size      M
*                     curve will return if Mult is FALSE, or the actual      M
*                     unit size vector field curve, if Mult.		     M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbCrvUnitLenScalar, unit vector field                                  M
*****************************************************************************/
CagdCrvStruct *SymbCrvUnitLenScalar(CagdCrvStruct *OrigCrv,
				    CagdBType Mult,
				    CagdRType Epsilon)
{
    int i, j;
    CagdCrvStruct
	*ScalarCrv = NULL,
	*Crv = CAGD_IS_BEZIER_CRV(OrigCrv) ? CnvrtBezier2BsplineCrv(OrigCrv)
					   : CagdCrvCopy(OrigCrv);
    CagdBType
        IsRational = CAGD_IS_RATIONAL_CRV(Crv);

    for (i = 0; i < MAX_ALEN_IMPROVE_ITERS; i++) {
	CagdCrvStruct *UnitCrvAprx, *ScalarCrvSqr,
	    *DotProdCrv = SymbCrvDotProd(Crv, Crv);
	CagdRType Min, Max, *SCPoints,
	    *DPPoints = DotProdCrv -> Points[1];

	if (ScalarCrv)
	    CagdCrvFree(ScalarCrv);
	ScalarCrv = CagdCrvCopy(DotProdCrv);
	SCPoints = ScalarCrv -> Points[1];

	/* Compute an approximation to the inverse function. */
	for (j = 0; j < ScalarCrv -> Length; j++, DPPoints++) {
	    *SCPoints++ = *DPPoints > 0.0 ? 1.0 / sqrt(*DPPoints) : 1.0;
	}
	ScalarCrvSqr = SymbCrvMult(ScalarCrv, ScalarCrv);

	/* Multiply the two to test the error. */
	UnitCrvAprx = SymbCrvMult(ScalarCrvSqr, DotProdCrv);
	CagdCrvFree(ScalarCrvSqr);

	CagdCrvMinMax(UnitCrvAprx, 1, &Min, &Max);

	if (1.0 - Min < Epsilon && Max - 1.0 < Epsilon) {
	    CagdCrvFree(UnitCrvAprx);
	    CagdCrvFree(DotProdCrv);
	    break;
	}
	else {
	    /* Refine in regions where the error is too large. */
	    int k,
	        Length = UnitCrvAprx -> Length,
	        Order = UnitCrvAprx -> Order,
	        KVLen = Length + Order;
	    CagdRType
		*KV = UnitCrvAprx -> KnotVector,
		*RefKV = IritMalloc(sizeof(CagdRType) * 2 * Length),
	        *Nodes = BspKnotNodes(KV, KVLen, Order),
		**Points = UnitCrvAprx -> Points;

	    for (j = k = 0; j < Length; j++) {
		CagdRType
		    V = 1.0 - (IsRational ? Points[1][j] / Points[0][j]
					  : Points[1][j]);

		if (ABS(V) > Epsilon) {
		    int Index = BspKnotLastIndexLE(KV, KVLen, Nodes[j]);

		    if (APX_EQ(KV[Index], Nodes[j])) {
			if (j > 0)
			    RefKV[k++] = (Nodes[j] + Nodes[j - 1]) / 2.0;
			if (j < Length - 1)
			    RefKV[k++] = (Nodes[j] + Nodes[j + 1]) / 2.0;
		    }
		    else
			RefKV[k++] = Nodes[j];
		}
	    }
	    CagdCrvFree(UnitCrvAprx);
	    CagdCrvFree(DotProdCrv);

	    IritFree((VoidPtr) Nodes);

	    if (k == 0) {
		/* No refinement was found needed - return current curve. */
		IritFree((VoidPtr) RefKV);
		break;
	    }
	    else {
		CagdCrvStruct
		    *CTmp = CagdCrvRefineAtParams(Crv, FALSE, RefKV, k);

		IritFree((VoidPtr) RefKV);
		CagdCrvFree(Crv);
		Crv = CTmp;
	    }

	}
    }

    CagdCrvFree(Crv);

    /* Error is probably within bounds - returns this unit length approx. */
    if (Mult) {
	CagdCrvStruct *CrvX, *CrvY, *CrvZ, *CrvW, *CTmp;
	int MaxCoord = CAGD_NUM_OF_PT_COORD(OrigCrv -> PType);

	SymbCrvSplitScalar(ScalarCrv, &CrvW, &CrvX, &CrvY, &CrvZ);
	CagdCrvFree(ScalarCrv);
	ScalarCrv = SymbCrvMergeScalar(CrvW,
				       CrvX,
				       MaxCoord > 1 ? CrvX : NULL,
				       MaxCoord > 2 ? CrvX : NULL);
	CagdCrvFree(CrvX);
	if (CrvW)
	    CagdCrvFree(CrvW);

	CTmp = SymbCrvMult(ScalarCrv, OrigCrv);
	CagdCrvFree(ScalarCrv);

	return CTmp;
    }
    else {
	return ScalarCrv;
    }
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Computes the curve which is a square root approximation to a given scalar  M
* curve, to within epsilon.						     M
*                                                                            *
* PARAMETERS:                                                                M
*   OrigCrv:    Scalar curve to approximate its square root function.        M
*   Epsilon:    Accuracy of approximation.                                   M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdCrvStruct *:   A curve approximating the square root of OrigCrv.     M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbCrvSqrtScalar, square root                                           M
*****************************************************************************/
CagdCrvStruct *SymbCrvSqrtScalar(CagdCrvStruct *OrigCrv, CagdRType Epsilon)
{
    int i, j;
    CagdCrvStruct
	*ScalarCrv = NULL,
	*Crv = CAGD_IS_BEZIER_CRV(OrigCrv) ? CnvrtBezier2BsplineCrv(OrigCrv)
					   : CagdCrvCopy(OrigCrv);
    CagdBType
        IsRational = CAGD_IS_RATIONAL_CRV(Crv);

    for (i = 0; i < MAX_ALEN_IMPROVE_ITERS; i++) {
	CagdCrvStruct *ScalarCrvSqr, *ErrorCrv;
	CagdRType Min, Max, *SCPoints,
	    *Points = Crv -> Points[1],
	    *WPoints = IsRational ? Crv -> Points[0] : NULL;

	if (ScalarCrv)
	    CagdCrvFree(ScalarCrv);
	ScalarCrv = CagdCrvCopy(Crv);
	SCPoints = ScalarCrv -> Points[1];

	/* Compute an approximation to the inverse function. */
	for (j = 0; j < ScalarCrv -> Length; j++) {
	    CagdRType
		V = IsRational ? *Points++ / *WPoints++ : *Points++;

	    *SCPoints++ = V >= 0.0 ? sqrt(V) : 0.0;
	}
	ScalarCrvSqr = SymbCrvMult(ScalarCrv, ScalarCrv);

	/* Compare the two to test the error. */
	ErrorCrv = SymbCrvSub(ScalarCrvSqr, Crv);
	CagdCrvFree(ScalarCrvSqr);

	CagdCrvMinMax(ErrorCrv, 1, &Min, &Max);

	if (Min > -Epsilon && Max < Epsilon) {
	    CagdCrvFree(ErrorCrv);
	    break;
	}
	else {
	    /* Refine in regions where the error is too large. */
	    int k,
	        Length = ErrorCrv -> Length,
	        Order = ErrorCrv -> Order,
	        KVLen = Length + Order;
	    CagdRType
		*KV = ErrorCrv -> KnotVector,
		*RefKV = IritMalloc(sizeof(CagdRType) * 2 * Length),
	        *Nodes = BspKnotNodes(KV, KVLen, Order),
		**ErrPoints = ErrorCrv -> Points;

	    for (j = k = 0; j < Length; j++) {
		CagdRType
		    V = 1.0 - (IsRational ? ErrPoints[1][j] / ErrPoints[0][j]
					  : ErrPoints[1][j]);

		if (ABS(V) > Epsilon) {
		    int Index = BspKnotLastIndexLE(KV, KVLen, Nodes[j]);

		    if (APX_EQ(KV[Index], Nodes[j])) {
			if (j > 0)
			    RefKV[k++] = (Nodes[j] + Nodes[j - 1]) / 2.0;
			if (j < Length - 1)
			    RefKV[k++] = (Nodes[j] + Nodes[j + 1]) / 2.0;
		    }
		    else
			RefKV[k++] = Nodes[j];
		}
	    }
	    CagdCrvFree(ErrorCrv);

	    IritFree((VoidPtr) Nodes);

	    if (k == 0) {
		/* No refinement was found needed - return current curve. */
		IritFree((VoidPtr) RefKV);
		break;
	    }
	    else {
		CagdCrvStruct
		    *CTmp = CagdCrvRefineAtParams(Crv, FALSE, RefKV, k);

		IritFree((VoidPtr) RefKV);
		CagdCrvFree(Crv);
		Crv = CTmp;
	    }

	}
    }

    CagdCrvFree(Crv);

    return ScalarCrv;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Computes a scalar curve approximating the arc length of given curve Crv.   M
* Arc length is astimated by computing the square of Crv first derivative    M
* approximating its square root and integrating symbolically.		     M
*                                                                            *
* PARAMETERS:                                                                M
*   Crv:       To approximate its arc length scalar field.                   M
*   Epsilon:   Accuracy of approximating.                                    M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdCrvStruct *:   A scalar field approximating Crv arc length.          M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbCrvArcLenCrv, arc length                                             M
*****************************************************************************/
CagdCrvStruct *SymbCrvArcLenCrv(CagdCrvStruct *Crv, CagdRType Epsilon)
{
    CagdCrvStruct
	*DerivCrv = CagdCrvDerive(Crv),
        *DerivMagSqrCrv = SymbCrvDotProd(DerivCrv, DerivCrv),
	*DerivMagCrv = SymbCrvSqrtScalar(DerivMagSqrCrv, Epsilon),
        *ArcLenCrv = CagdCrvIntegrate(DerivMagCrv);

    CagdCrvFree(DerivCrv);
    CagdCrvFree(DerivMagSqrCrv);
    CagdCrvFree(DerivMagCrv);

    return ArcLenCrv;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Computes a tight approximation to the arc length of a curve.		     M
*   Estimates the arc length scalar field of Crv using SymbCrvArcLenCrv and  M
* evaluate the estimate on the curve's domain boundary.                      M
*                                                                            *
* PARAMETERS:                                                                M
*   Crv:        Curve to compute a tight approximation on arc length.        M
*   Epsilon:    Accuracy control.                                            M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdRType:   The approximated arc length of the given curve Crv.         M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbCrvArcLen, arc length                                                M
*****************************************************************************/
CagdRType SymbCrvArcLen(CagdCrvStruct *Crv, CagdRType Epsilon)
{
    CagdRType TMin, TMax, *Pt;
    CagdCrvStruct
	*ArcLenCrv = SymbCrvArcLenCrv(Crv, Epsilon);

    CagdCrvDomain(ArcLenCrv, &TMin, &TMax);
    Pt = CagdCrvEval(ArcLenCrv, TMax);
    CagdCrvFree(ArcLenCrv);

    return CAGD_IS_RATIONAL_CRV(ArcLenCrv) ? Pt[1] / Pt[0] : Pt[1];
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Computes parameter values to move steps of Length at a time on curve Crv.  M
*    Returned is a list of parameter values to move along.		     M
*                                                                            *
* PARAMETERS:                                                                M
*   Crv:        Curve to compute constant arc Length steps.                  M
*   Length:     The step size.                                               M
*   Epsilon:    Accuracy control.                                            M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdPtStruct *:  List of parameter values to march along Crv with arc    M
*                    Length between them.                                    M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbCrvArcLenSteps, arc length                                           M
*****************************************************************************/
CagdPtStruct *SymbCrvArcLenSteps(CagdCrvStruct *Crv,
				 CagdRType Length,
				 CagdRType Epsilon)
{
    CagdRType TMin, TMax, *Pt, CrvLength, Len;
    CagdPtStruct *Param,
	*ParamList = NULL;
    CagdCrvStruct
	*ArcLenCrv = SymbCrvArcLenCrv(Crv, Epsilon);

    CagdCrvDomain(ArcLenCrv, &TMin, &TMax);
    Pt = CagdCrvEval(ArcLenCrv, TMax);

    CrvLength = CAGD_IS_RATIONAL_CRV(ArcLenCrv) ? Pt[1] / Pt[0] : Pt[1];

    for (Len = CrvLength - Length; Len > 0.0; Len -= Length) {
	Param = SymbCrvConstSet(ArcLenCrv, 1, ARCLEN_CONST_SET_EPSILON, Len);
	if (Param == NULL || Param -> Pnext != NULL)
	    SYMB_FATAL_ERROR(SYMB_ERR_REPARAM_NOT_MONOTONE);

	Param -> Pnext = ParamList;
	ParamList = Param;
    }

    CagdCrvFree(ArcLenCrv);

    return ParamList;
}
