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

#include "cagd_loc.h"

#define MAX_ALEN_IMPROVE_ITERS 5
#define ARCLEN_CONST_SET_EPSILON 0.001

/******************************************************************************
* Subdivides the given curves to curves, each with less of control polygon    *
* less than or equal to MaxLen.	Returned is a list of curves.		      *
******************************************************************************/
CagdCrvStruct *CagdLimitCrvArcLen(CagdCrvStruct *Crv, CagdRType MaxLen)
{
    if (CagdCrvArcLenPoly(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 = CagdLimitCrvArcLen(Crv1, MaxLen);
	Crv2MaxLen = CagdLimitCrvArcLen(Crv2, MaxLen);

	CagdCrvFree(Crv1);
	CagdCrvFree(Crv2);

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

/******************************************************************************
* Computes a bound on the arc length of a curve by computing the length of    *
* its control polygon.							      *
******************************************************************************/
CagdRType CagdCrvArcLenPoly(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;
}

/******************************************************************************
* Normalizes the given curve to be a unit length curve, by computing a scalar *
* curve to multiply with this curve. Returns the multiplied curve if Mult, or *
* just the scalar curve, otherwise.					      *
******************************************************************************/
CagdCrvStruct *CagdCrvUnitLenScalar(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 = CagdCrvDotProd(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 = CagdCrvMult(ScalarCrv, ScalarCrv);

	/* Multiply the two to test the error. */
	UnitCrvAprx = CagdCrvMult(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);

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

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

	return CTmp;
    }
    else {
	return ScalarCrv;
    }
}

/******************************************************************************
* Computes the curve which is a square root approximation to a given scalar   *
* curve, to within epsilon.						      *
******************************************************************************/
CagdCrvStruct *CagdCrvSqrtScalar(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 = CagdCrvMult(ScalarCrv, ScalarCrv);

	/* Compare the two to test the error. */
	ErrorCrv = CagdCrvSub(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;
}

/******************************************************************************
* Normalizes the given curve to be a unit length curve, by computing a scalar *
* curve to multiply with this curve. Returns the composed curve if Compose,   *
* or just the scalar curve, otherwise.					      *
* Note that the returned curves is approximately constant speed curve and     *
* not arc-length, since a Bezier curve always has a domain 0 to 1 nomatter    *
* the size of the curve itself.						      *
******************************************************************************/
CagdCrvStruct *CagdCrvArcLenCrv(CagdCrvStruct *Crv, CagdRType Epsilon)
{
    CagdCrvStruct
	*DerivCrv = CagdCrvDerive(Crv),
        *DerivMagSqrCrv = CagdCrvDotProd(DerivCrv, DerivCrv),
	*DerivMagCrv = CagdCrvSqrtScalar(DerivMagSqrCrv, Epsilon),
        *ArcLenCrv = CagdCrvIntegrate(DerivMagCrv);

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

    return ArcLenCrv;
}

/******************************************************************************
* Computes a tight approximation to the arc length of a curve.		      *
******************************************************************************/
CagdRType CagdCrvArcLen(CagdCrvStruct *Crv, CagdRType Epsilon)
{
    CagdRType TMin, TMax, *Pt;
    CagdCrvStruct
	*ArcLenCrv = CagdCrvArcLenCrv(Crv, Epsilon);

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

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

/******************************************************************************
* Computes parameter values to move steps of Length at a time. Returned is a  *
* list of parameter values to step at.					      *
******************************************************************************/
CagdPtStruct *CagdCrvArcLenSteps(CagdCrvStruct *Crv, CagdRType Length,
							    CagdRType Epsilon)
{
    CagdRType TMin, TMax, *Pt, CrvLength, Len;
    CagdPtStruct *Param,
	*ParamList = NULL;
    CagdCrvStruct
	*ArcLenCrv = CagdCrvArcLenCrv(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 = CagdCrvConstSet(ArcLenCrv, 1, ARCLEN_CONST_SET_EPSILON, Len);
	if (Param == NULL || Param -> Pnext != NULL)
	    FATAL_ERROR(CAGD_ERR_REPARAM_NOT_MONOTONE);

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

    CagdCrvFree(ArcLenCrv);

    return ParamList;
}