/******************************************************************************
* SymbZero.c - computes the zeros and extremes of a given object.	      *
*******************************************************************************
* (C) Gershon Elber, Technion, Israel Institute of Technology                 *
*******************************************************************************
* Written by Gershon Elber, March 93.					      *
******************************************************************************/

#include "symb_loc.h"
#include "geom_lib.h"

#define SET_DEFAULT_IRIT_EPS	1e-3
#define SET_DEFAULT_IRIT_EPS	1e-3
#define ZERO_APX_EQ(x, y)		(FABS((x) - (y)) < GlblSetEpsilon * 10)
#define MID_RATIO			0.5123456789
#define SYMB_ZERO_NR_MAX_ITER	10

STATIC_DATA CagdPtStruct
    *GlblPtList = NULL;
STATIC_DATA CagdRType CrvTMin, CrvTMax,
    GlblSetEpsilon = SET_DEFAULT_IRIT_EPS;

static void SymbScalarCrvZeroSet(CagdCrvStruct *Crv);
static CagdRType SymbScalarCrvNewtonRaphson(CagdCrvStruct *Crv,
					    CagdRType t,
					    CagdRType TMin,
					    CagdRType TMax);
static void SymbScalarCrvExtremSet(CagdCrvStruct *Crv);

/*****************************************************************************
* DESCRIPTION:                                                               M
* Computes the zero set of a given curve, in the given axis (1-3 for X-Z).   M
*   Returned is a list of the zero set points holding the parameter values   M
* att Pt[0] of each point.						     M
*                                                                            *
* PARAMETERS:                                                                M
*   Crv:        To compute its zero set.                                     M
*   Axis:       The axis of Crv to compute zero set for.                     M
*   Epsilon:    Tolerance control.                                           M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdPtStruct *:   List of parameter values form which Crv is zero in     M
*                     axis Axis.                                             M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbCrvZeroSet, zero set, symbolic computation                           M
*****************************************************************************/
CagdPtStruct *SymbCrvZeroSet(CagdCrvStruct *Crv, int Axis, CagdRType Epsilon)
{
    return SymbCrvConstSet(Crv, Axis, Epsilon, 0.0);
}

/*****************************************************************************
* DESCRIPTION:                                                               M
*   Computes the extremum set of a given curve, in given axis (0/1-3 for     M
* W/X-Z).								     M
*   Returned is a list of the extreme set points holding the parameter       M
* values at Pt[0] of each point.					     M
*   One could compute the derivative of the curve and find its zero set.     M
*   However, for rational curves, this will double the degree and slow down  M
* the computation considerably.						     M
*                                                                            *
* PARAMETERS:                                                                M
*   Crv:        To compute its extremum set.                                 M
*   Axis:       The axis of Crv to compute extremum set for.                 M
*   Epsilon:    Tolerance control.                                           M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdPtStruct *:   List of parameter values form which Crv has an         M
*                     extremum value in axis Axis.                           M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbCrvExtremSet, extremum set, symbolic computation                     M
*****************************************************************************/
CagdPtStruct *SymbCrvExtremSet(CagdCrvStruct *Crv, int Axis, CagdRType Epsilon)
{
    CagdCrvStruct *CrvW, *CrvX, *CrvY, *CrvZ, *TCrv,
	*ScalarCrv = NULL;

    GlblSetEpsilon = Epsilon;

    SymbCrvSplitScalar(Crv, &CrvW, &CrvX, &CrvY, &CrvZ);

    switch (Axis) {
	case 0:
	    if (CrvW)
		ScalarCrv = CrvW;
	    else
		SYMB_FATAL_ERROR(SYMB_ERR_OUT_OF_RANGE);
	    break;
	case 1:
	    if (CrvX)
		ScalarCrv = CrvX;
	    else
		SYMB_FATAL_ERROR(SYMB_ERR_OUT_OF_RANGE);
	    break;
	case 2:
	    if (CrvY)
		ScalarCrv = CrvY;
	    else
		SYMB_FATAL_ERROR(SYMB_ERR_OUT_OF_RANGE);
	    break;
	case 3:
	    if (CrvZ)
		ScalarCrv = CrvZ;
	    else
		SYMB_FATAL_ERROR(SYMB_ERR_OUT_OF_RANGE);
	    break;
	default:
	    SYMB_FATAL_ERROR(SYMB_ERR_OUT_OF_RANGE);
    }

    if (Axis > 0) {
        Crv = SymbCrvMergeScalar(CrvW, ScalarCrv, NULL, NULL);
	if (CrvW)
	    CagdCrvFree(CrvW);
    }
    else
        Crv = ScalarCrv;

    if (CrvX)
	CagdCrvFree(CrvX);
    if (CrvY)
	CagdCrvFree(CrvY);
    if (CrvZ)
	CagdCrvFree(CrvZ);
    
    GlblPtList = NULL;
    if (CAGD_IS_BEZIER_CRV(Crv)) {
	/* We need to save domains. */
	TCrv = CnvrtBezier2BsplineCrv(Crv);
	CagdCrvFree(Crv);
	Crv = TCrv;
    }

    CagdCrvDomain(Crv, &CrvTMin, &CrvTMax);
    SymbScalarCrvExtremSet(Crv);
    CagdCrvFree(Crv);

    return GlblPtList;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
*   Computes the constant set of a given curve, in the given axis (0/1-3 for M
* W/X-Z).							 	     M
*   Returned is a list of the constant set points holding the parameter	     M
* values at Pt[0] of each point.					     M
*                                                                            *
* PARAMETERS:                                                                M
*   Crv:        To compute its constant set.                                 M
*   Axis:       The axis of Crv to compute constant set for.                 M
*   Epsilon:    Tolerance control.                                           M
*   ConstVal:   The value at which to compute the constant set.		     M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdPtStruct *:   List of parameter values form which Crv has an         M
*                     value of ConstVal in axis Axis.                        M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbCrvConstSet, constant set, zero set, symbolic computation            M
*****************************************************************************/
CagdPtStruct *SymbCrvConstSet(CagdCrvStruct *Crv,
			      int Axis,
			      CagdRType Epsilon,
			      CagdRType ConstVal)
{
    CagdCrvStruct *CrvW, *CrvX, *CrvY, *CrvZ, *TCrv;

    GlblSetEpsilon = Epsilon;

    SymbCrvSplitScalar(Crv, &CrvW, &CrvX, &CrvY, &CrvZ);

    Crv = NULL;

    switch (Axis) {
	case 0:
	    if (CrvW)
		SWAP(CagdCrvStruct *, Crv, CrvW)
	    break;
	case 1:
	    if (CrvX)
		SWAP(CagdCrvStruct *, Crv, CrvX)
	    break;
	case 2:
	    if (CrvY)
		SWAP(CagdCrvStruct *, Crv, CrvY)
	    break;
	case 3:
	    if (CrvZ)
		SWAP(CagdCrvStruct *, Crv, CrvZ)
	    break;
	default:
	    break;
    }

    if (Crv == NULL)
        SYMB_FATAL_ERROR(SYMB_ERR_OUT_OF_RANGE);

    if (CrvW) {
        if (ConstVal != 0.0) {
	    int i;
            CagdRType
	        *SPts = Crv -> Points[1],
	        *WPts = CrvW -> Points[1];

	    for (i = 0; i < Crv -> Length; i++)
	        *SPts++ -= ConstVal * *WPts++;
	}
	CagdCrvFree(CrvW);
    }
    else {
        if (ConstVal != 0.0) {
	    int i;
	    CagdRType
	        *SPts = Crv -> Points[1];

	      for (i = 0; i < Crv -> Length; i++)
		  *SPts++ -= ConstVal;
	}
    }

    if (CrvX)
	CagdCrvFree(CrvX);
    if (CrvY)
	CagdCrvFree(CrvY);
    if (CrvZ)
	CagdCrvFree(CrvZ);

    GlblPtList = NULL;
    if (CAGD_IS_BEZIER_CRV(Crv)) {
	/* We need to save domains. */
	TCrv = CnvrtBezier2BsplineCrv(Crv);
	CagdCrvFree(Crv);
	Crv = TCrv;
    }

    CagdCrvDomain(Crv, &CrvTMin, &CrvTMax);

    SymbScalarCrvZeroSet(Crv);

#ifdef DEBUG
    {
        IRIT_SET_IF_DEBUG_ON_PARAMETER(_DebugSymbZeroPrintSol, FALSE) {
	    CagdPtStruct *Pt = GlblPtList;

	    for (Pt = GlblPtList; Pt != NULL; Pt = Pt -> Pnext)
	        fprintf(stderr, "Pt = %f\n", Pt -> Pt[0]);
	}
    }
#endif /* DEBUG */

    CagdCrvFree(Crv);

    return GlblPtList;
}

/*****************************************************************************
* DESCRIPTION:                                                               *
* Computes the zero set of a scalar curve. Curve must be polynomial.	     *
*   Assumes open end condition on the curve parametric domain.		     *
*   Constant set points are append to the GlblPtList point list.	     *
*                                                                            *
* PARAMETERS:                                                                *
*   Crv:        To compute its constant set.                                 *
*                                                                            *
* RETURN VALUE:                                                              *
*   void                                                                     *
*****************************************************************************/
static void SymbScalarCrvZeroSet(CagdCrvStruct *Crv)
{
    int i,
	Len = Crv -> Length,
        Sign = 0;
    CagdRType
	*ScalarPts = Crv -> Points[1];

    for (i = 0; i < Len; i++) {
        CagdRType
	    V = ScalarPts[i];
	int NewSign = SIGN(V);

	if (Sign * NewSign < 0)
	    break;
	else if (NewSign)
	    Sign = NewSign;
    }

    if (i < Len) {
	CagdRType TMin, TMax, TMid;

	/* Control polygon is both positive and negative. */
	CagdCrvDomain(Crv, &TMin, &TMax);
	if (Crv -> Length > Crv -> Order)
	    TMid = Crv -> KnotVector[(Crv -> Length + Crv -> Order) >> 1];
	else
	    TMid = TMin * MID_RATIO + TMax * (1.0 - MID_RATIO);

	if (TMax - TMin < GlblSetEpsilon * 0.2) {    /* Small enough - stop. */
	    SymbInsertNewParam((TMax + TMin) * 0.5);
	}
	else if (Crv -> Length == Crv -> Order && Crv -> Order < 6) {
	    /* A low degree polynomial Bezier curve - solve analytically. */
	    int j = 0;
	    CagdCrvStruct
		*BzrCrv = CAGD_IS_BEZIER_CRV(Crv) ? Crv
					         : CnvrtBspline2BezierCrv(Crv),
		*PwrCrv = CnvrtBezier2PowerCrv(BzrCrv);
	    CagdRType Sols[4],
		*Pts = PwrCrv -> Points[1];

	    if (BzrCrv != Crv)
	        CagdCrvFree(BzrCrv);

	    switch (Crv -> Order) {
		case 5:
		    if (!APX_EQ(Pts[4], 0.0)) {
			j = GMSolveQuarticEqn(Pts[3] / Pts[4],
					      Pts[2] / Pts[4],
					      Pts[1] / Pts[4],
					      Pts[0] / Pts[4],
					      Sols);
			break;
		    }
		case 4:
		    if (!APX_EQ(Pts[3], 0.0)) {
			j = GMSolveCubicEqn(Pts[2] / Pts[3],
					    Pts[1] / Pts[3],
					    Pts[0] / Pts[3],
					    Sols);
			break;
		    }
		case 3:
		    if (!APX_EQ(Pts[2], 0.0)) {
			j = GMSolveQuadraticEqn(Pts[1] / Pts[2],
						Pts[0] / Pts[2],
						Sols);
			break;
		    }
		case 2:
		    if (!APX_EQ(Pts[1], 0.0)) {
			j = 1;
			Sols[0] = -Pts[0] / Pts[1];
		    }
		    break;
	    }
	    CagdCrvFree(PwrCrv);

	    while (--j >= 0) {
		if (Sols[j] > 0.0 && Sols[j] <= 1.0)
		    SymbInsertNewParam(TMin + (TMax - TMin) * Sols[j]);
	    }
	}
	else {
	    CagdRType PrevV;
	    CagdCrvStruct *Crv1, *Crv2;

	    /* Check if control polygon is monotone - if so one solution. */
	    ScalarPts = Crv -> Points[1];
	    PrevV = ScalarPts[0];

	    for (i = Sign = 0; i < Len; i++) {
	        CagdRType
		    V = ScalarPts[i],
		    DV = V - PrevV;
		int NewSign = SIGN(DV);

		if (Sign * NewSign < 0)
		    break;
		else if (NewSign)
		    Sign = NewSign;

		PrevV = V;
	    }

	    if (i == Len) {
	        /* Try to find this single solution using numeric marching. */
	        CagdRType
		    t = SymbScalarCrvNewtonRaphson(Crv, TMid, TMin, TMax);

		if (t >= TMin && t < TMax)
		    SymbInsertNewParam(t);
		else
		    i = 0;
	    }

	    /* Control poly is both increasing and decreasing and/or numeric */
	    /* marching has failed - subdivide.			 	     */
	    if (i < Len) {
	        Crv1 = CagdCrvSubdivAtParam(Crv, TMid);
		Crv2 = Crv1 -> Pnext;

		SymbScalarCrvZeroSet(Crv1);
		SymbScalarCrvZeroSet(Crv2);

		CagdCrvFree(Crv1);
		CagdCrvFree(Crv2);
	    }
	}
    }
}

/*****************************************************************************
* DESCRIPTION:                                                               *
*   Assuming Crv = 0 has a single solution in t in [TMin, TMax], seek it     *
* using Newton Raphson numeric iterations.				     *
*                                                                            *
* PARAMETERS:                                                                *
*   Crv:         Curve to seek its intersectiion with Y = ConstVal.          *
*   t:           Starting parameter.					     *
*   TMin, TMax:  Domain of Crv.					 	     *
*                                                                            *
* RETURN VALUE:                                                              *
*   CagdRType:   Solution if found, or t outside [TMin, TMax] if failed.     *
*****************************************************************************/
static CagdRType SymbScalarCrvNewtonRaphson(CagdCrvStruct *Crv,
					    CagdRType t,
					    CagdRType TMin,
					    CagdRType TMax)
{
    int i;
    CagdBType
	IsRational = CAGD_IS_RATIONAL_CRV(Crv);
    CagdCrvStruct
	*DCrv = CagdCrvDeriveScalar(Crv);

    for (i = 0; i < SYMB_ZERO_NR_MAX_ITER; i++) {
        CagdRType *R, v, Dv;

#	ifdef SYMBZERO_SUPPORT_NR_RATIONAL
        if (IsRational) {
	    CagdRType Pos[2];

	    R = CagdCrvEval(Crv, t);
	    v = R[1] / R[0];
	    PT2D_COPY(Pos, R);

	    R = CagdCrvEval(DCrv, t);
	    Dv = (R[1] * Pos[0] - Pos[1] * R[0]) / SQR(Pos[0]);
	}
	else
#	endif /* SYMBZERO_SUPPORT_NR_RATIONAL */
	{
	    R = CagdCrvEval(Crv, t);
	    v = R[1];

	    R = CagdCrvEval(DCrv, t);
	    Dv = R[1];
	}

	/* Are we accurate enough!? */
	if (FABS(v) < GlblSetEpsilon)
	    break;

	t = t - v / Dv;
	t = BOUND(t, TMin, TMax);
    }

    CagdCrvFree(DCrv);

    if (i >= SYMB_ZERO_NR_MAX_ITER)
        return TMin - 1.0; /* Failed. */
    else
        return t;
}

/*****************************************************************************
* DESCRIPTION:                                                               *
* Computes the extrem set of a scalar curve. Curve might be rational.	     *
*   Assumes open end condition on the curve parametric domain.		     *
*   Extreme set points are append to the GlblPtList point list.		     *
*                                                                            *
* PARAMETERS:                                                                *
*   Crv:        To compute its extremum set.                                 *
*                                                                            *
* RETURN VALUE:                                                              *
*   void                                                                     *
*****************************************************************************/
static void SymbScalarCrvExtremSet(CagdCrvStruct *Crv)
{
    int i,
	Len = Crv -> Length,
        Sign = 0;
    CagdRType
	*ScalarPts = Crv -> Points[1],
	*ScalarWPts = Crv -> Points[0];

    if (SymbCrvPosNegWeights(Crv)) {
	i = 0; 				 /* Force subdivision of the curve. */
    }
    else {
	CagdRType
	    PrevV = ScalarWPts ? ScalarPts[0] / ScalarWPts[0] : ScalarPts[0];

	for (i = 0; i < Len; i++) {
	    CagdRType
		V = (ScalarWPts ? ScalarPts[i] / ScalarWPts[i] : ScalarPts[i]),
		DV = V - PrevV;
	    int NewSign = SIGN(DV);

	    if (Sign * NewSign < 0)
		break;
	    else if (NewSign)
		Sign = NewSign;

	    PrevV = V;
	}
    }

    if (i < Len) {
	CagdRType TMin, TMax, TMid;

	/* Control polygon is both increasing and decreasing. */
	CagdCrvDomain(Crv, &TMin, &TMax);
	TMid = TMin * MID_RATIO + TMax * (1.0 - MID_RATIO);

	if (TMax - TMin < GlblSetEpsilon * 0.2) {    /* Small enough - stop. */
	    SymbInsertNewParam((TMax + TMin) * 0.5);
	}
	else { 						 /* Needs to subdiv. */
	    CagdCrvStruct
	        *Crv1 = CagdCrvSubdivAtParam(Crv, TMid),
		*Crv2 = Crv1 -> Pnext;

	    SymbScalarCrvExtremSet(Crv1);
	    SymbScalarCrvExtremSet(Crv2);

	    CagdCrvFree(Crv1);
	    CagdCrvFree(Crv2);
	}
    }
    else {
	CagdRType V1, V2, TMin, TMax;

	CagdCrvDomain(Crv, &TMin, &TMax);

	/* This segment is monotone. Test if end points are extremes. */
	V1 = ScalarWPts ? ScalarPts[0] / ScalarWPts[0] : ScalarPts[0];
	V2 = ScalarWPts ? ScalarPts[1] / ScalarWPts[1] : ScalarPts[1];
	if (APX_EQ(V1, V2))
	    SymbInsertNewParam(TMin);

	V1 = ScalarWPts ? ScalarPts[Len - 2] / ScalarWPts[Len - 2]
			: ScalarPts[Len - 2];
	V2 = ScalarWPts ? ScalarPts[Len - 1] / ScalarWPts[Len - 1]
			: ScalarPts[Len - 1];
	if (APX_EQ(V1, V2))
	    SymbInsertNewParam(TMax);
    }
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Returns TRUE iff the Crv is not rational or rational with weights that are M
* entirely positive or entirely negative.				     M
*                                                                            *
* PARAMETERS:                                                                M
*   Crv:       To examine for same sign weights, if any.                     M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdBType:   TRUE if no weights or all of same sign.                     M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbCrvPosNegWeights, symbolic computation                               M
*****************************************************************************/
CagdBType SymbCrvPosNegWeights(CagdCrvStruct *Crv)
{
    int i;
    CagdBType HasNeg, HasPos;
    CagdRType
	*Weights = Crv -> Points[0];

    if (Weights == NULL)
	return FALSE;				   /* Curve is not rational. */

    for (HasNeg = HasPos = FALSE, i = Crv -> Length - 1; i >= 0; i--) {
	HasNeg |= *Weights < 0.0;
	HasPos |= *Weights++ > 0.0;
    }

    return HasNeg && HasPos;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Clear the global sorted list of points and return what was on that list    M
* before.  This sorted list is built via SymbInsertNewParam		     M
*                                                                            *
* PARAMETERS:                                                                M
*   TMin, TMax:	  Domain to insert points in between.                        M
*                                                                            *
* RETURN VALUE:                                                              M
*   CagdPtStruct *:	The old point list.                                  M
*                                                                            *
* SEE ALSO:                                                                  M
*   SymbInsertNewParam                                                       M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbGetParamListAndReset		                                     M
*****************************************************************************/
CagdPtStruct *SymbGetParamListAndReset(CagdRType TMin, CagdRType TMax)
{
    CagdPtStruct
	*PtList = GlblPtList;

    CrvTMin = TMin;
    CrvTMax = TMax;
    GlblPtList = NULL;

    return PtList;
}

/*****************************************************************************
* DESCRIPTION:                                                               M
* Insert a single t value into existing GlblPtList, provided no equal t      M
* value exists already in the list. List is ordered incrementally.	     M
*                                                                            *
* PARAMETERS:                                                                M
*   t:         New value to insert to global GlblPtList list.		     M
*                                                                            *
* RETURN VALUE:                                                              M
*   void                                                                     M
*                                                                            *
* SEE ALSO:                                                                  M
*   SymbGetParamListAndReset                                                 M
*                                                                            *
* KEYWORDS:                                                                  M
*   SymbInsertNewParam    		                                     M
*****************************************************************************/
void SymbInsertNewParam(CagdRType t)
{
    CagdPtStruct *PtTmp, *PtLast, *Pt;

    if (APX_EQ_EPS(t, CrvTMin, IRIT_UEPS) || APX_EQ_EPS(t, CrvTMax, IRIT_UEPS))
	return;

    Pt = CagdPtNew();
    Pt -> Pt[0] = t;

    if (GlblPtList) {
	for (PtTmp = GlblPtList, PtLast = NULL;
	     PtTmp != NULL;
	     PtLast = PtTmp, PtTmp = PtTmp -> Pnext) {
	    if (ZERO_APX_EQ(PtTmp -> Pt[0], t)) {
	        IritFree(Pt);
		return;
	    }
	    if (PtTmp -> Pt[0] > t)
	        break;
	}
	if (PtTmp) {
	    /* Insert the new point in the middle of the list. */
	    Pt -> Pnext = PtTmp;
	    if (PtLast)
		PtLast -> Pnext = Pt;
	    else
		GlblPtList = Pt;
	}
	else {
	    /* Insert the new point as the last point in the list. */
	    PtLast -> Pnext = Pt;
	}
    }
    else
        GlblPtList = Pt;
}