/******************************************************************************
* CagdOfst.c - computes offset approximation to curves and surfaces.          *
*******************************************************************************
* Written by Gershon Elber, March. 93.					      *
******************************************************************************/

#include "cagd_loc.h"

#define MAX_OFFSET_IMPROVE_ITERS	20
#define NORMAL_PERTURB			1e-3
#define OFFSET_TRIM_EPS			1.25

/******************************************************************************
* Given a curve and an offset amount Offset, returns an approximation to the  *
* offset curve by offseting the control polygon in the normal direction.      *
******************************************************************************/
CagdCrvStruct *CagdCrvOffset(CagdCrvStruct *Crv, CagdRType OffsetDist)
{
    CagdBType
	IsNotRational = !CAGD_IS_RATIONAL_CRV(Crv);
    int i, j,
	MaxCoord = CAGD_NUM_OF_PT_COORD(Crv -> PType),
	Order = Crv -> Order,
	Length = Crv -> Length;
    CagdBType
	HasNewKV = FALSE;
    CagdVecStruct *N;
    CagdCrvStruct
	*OffsetCrv = CagdCrvCopy(Crv);
    CagdRType *Nodes, *NodePtr,
	**Points = OffsetCrv -> Points,
	*KV = NULL;

    switch (Crv -> GType) {
	case CAGD_CBEZIER_TYPE:
	    HasNewKV = TRUE;
	    KV = BspKnotUniformOpen(Length, Order, NULL);
	    break;
	case CAGD_CBSPLINE_TYPE:
	    KV = Crv -> KnotVector;
	    break;
	case CAGD_CPOWER_TYPE:
	    FATAL_ERROR(CAGD_ERR_POWER_NO_SUPPORT);
	    return NULL;
	default:
	    FATAL_ERROR(CAGD_ERR_UNDEF_CRV);
	    return NULL;
    }

    Nodes = BspKnotNodes(KV, Length + Order, Order);
    NodePtr = Nodes;

    if (IsNotRational)
	for (j = 0; j < Length; j++, NodePtr++) {
	    if ((N = CagdCrvNormal(Crv, *NodePtr)) == NULL &&
		(N = CagdCrvNormal(Crv, NodePtr == Nodes ?
				        *NodePtr + NORMAL_PERTURB :
				        *NodePtr - NORMAL_PERTURB)) == NULL) {
		FATAL_ERROR(CAGD_ERR_CANNOT_COMP_NORMAL);
		return NULL;
	    }
	    for (i = 1; i <= MaxCoord; i++)
		Points[i][j] += N -> Vec[i - 1] * OffsetDist;
	}
    else
	for (j = 0; j < Length; j++, NodePtr++) {
	    if ((N = CagdCrvNormal(Crv, *NodePtr)) == NULL &&
		(N = CagdCrvNormal(Crv, NodePtr == Nodes ?
				        *NodePtr + NORMAL_PERTURB :
				        *NodePtr - NORMAL_PERTURB)) == NULL) {
		FATAL_ERROR(CAGD_ERR_CANNOT_COMP_NORMAL);
		return NULL;
	    }
	    for (i = 1; i <= MaxCoord; i++)
		Points[i][j] = Points[i][j] +
			       N -> Vec[i - 1] * OffsetDist * Points[W][j];
	}

    if (HasNewKV)
	IritFree((VoidPtr) KV);
    IritFree((VoidPtr) Nodes);

    return OffsetCrv;
}

/******************************************************************************
* Given a surface and an offset amount Offset, returns an approximation to    *
* the offset surface by offseting the control mesh in the normal direction.   *
******************************************************************************/
CagdSrfStruct *CagdSrfOffset(CagdSrfStruct *Srf, CagdRType OffsetDist)
{
    CagdBType
	IsNotRational = !CAGD_IS_RATIONAL_SRF(Srf);
    int	i, Row, Col,
	MaxCoord = CAGD_NUM_OF_PT_COORD(Srf -> PType),
	UOrder = Srf -> UOrder,
	VOrder = Srf -> VOrder,
	ULength = Srf -> ULength,
	VLength = Srf -> VLength;
    CagdBType
	HasNewKV = FALSE;
    CagdVecStruct *N;
    CagdSrfStruct
	*OffsetSrf = CagdSrfCopy(Srf);
    CagdRType *UNodes, *VNodes, *UNodePtr, *VNodePtr,
	**Points = OffsetSrf -> Points,
	*UKV = NULL,
	*VKV = NULL;

    switch (Srf -> GType) {
	case CAGD_SBEZIER_TYPE:
	    HasNewKV = TRUE;
	    UKV = BspKnotUniformOpen(ULength, UOrder, NULL);
	    VKV = BspKnotUniformOpen(VLength, VOrder, NULL);
	    break;
	case CAGD_SBSPLINE_TYPE:
	    UKV = Srf -> UKnotVector;
	    VKV = Srf -> VKnotVector;
	    break;
	case CAGD_SPOWER_TYPE:
	    FATAL_ERROR(CAGD_ERR_POWER_NO_SUPPORT);
	    return NULL;
	default:
	    FATAL_ERROR(CAGD_ERR_UNDEF_CRV);
	    return NULL;
    }

    UNodes = BspKnotNodes(UKV, ULength + UOrder, UOrder);
    VNodes = BspKnotNodes(VKV, VLength + VOrder, VOrder);

    if (IsNotRational)
	for (Row = 0, VNodePtr = VNodes; Row < VLength; Row++, VNodePtr++)
	    for (Col = 0, UNodePtr = UNodes; Col < ULength; Col++, UNodePtr++) {
	    	N = CagdSrfNormal(Srf, *UNodePtr, *VNodePtr);
	    	for (i = 1; i <= MaxCoord; i++)
		    Points[i][CAGD_MESH_UV(OffsetSrf, Col, Row)] +=
			N -> Vec[i - 1] * OffsetDist;
	    }
    else
	for (Row = 0, VNodePtr = VNodes; Row < VLength; Row++, VNodePtr++)
	    for (Col = 0, UNodePtr = UNodes; Col < ULength; Col++, UNodePtr++) {
	    	N = CagdSrfNormal(Srf, *UNodePtr, *VNodePtr);
	    	for (i = 1; i <= MaxCoord; i++)
	    	    Points[i][CAGD_MESH_UV(OffsetSrf, Col, Row)] +=
			N -> Vec[i - 1] * OffsetDist *
			    Points[W][CAGD_MESH_UV(OffsetSrf, Col, Row)];
	    }

    if (HasNewKV) {
	IritFree((VoidPtr) UKV);
	IritFree((VoidPtr) VKV);
    }

    IritFree((VoidPtr) UNodes);
    IritFree((VoidPtr) VNodes);

    return OffsetSrf;
}

/******************************************************************************
* Given a curve and an offset amount OffsetDist, returns an approx. to the    *
* offset curve by offseting the control polygon in the normal direction.      *
*   This function computes an approximation to the offset using		      *
* OffsetAprxFunc, measure the error and use it to refine and decrease the     *
* error. Bezier curves are promoted to Bsplines. See paper below for more.    *
*									      *
* + Gershon Elber and Elaine Cohen. Error Bounded Variable Distance Offset    *
*   Operator for Free Form Curves and Surfaces. International Journal of      *
*   Computational Geometry & Applications, Vol. 1, Num. 1, March 1991,        *
*   pp 67-78.								      *
******************************************************************************/
CagdCrvStruct *CagdCrvAdapOffset(CagdCrvStruct *OrigCrv,
				 CagdRType OffsetDist,
				 CagdRType OffsetError,
				 CagdCrvFuncType OffsetAprxFunc)
{
    int i;
    CagdBType
	IsRational = CAGD_IS_RATIONAL_CRV(OrigCrv);
    CagdRType Min, Max, TMin, TMax,
	OffsetDistSqr = SQR(OffsetDist);
    CagdCrvStruct *OffsetCrv, *Crv;

    switch (OrigCrv -> GType) {
	case CAGD_CBEZIER_TYPE:
	    Crv = CnvrtBezier2BsplineCrv(OrigCrv);
	    break;
	case CAGD_CBSPLINE_TYPE:
	    Crv = CagdCrvCopy(OrigCrv);
	    break;
	case CAGD_CPOWER_TYPE:
	default:
	    FATAL_ERROR(CAGD_ERR_UNDEF_CRV);
	    Crv = NULL;
	    break;
    }

    if (OffsetAprxFunc == NULL)
	OffsetAprxFunc = CagdCrvOffset;

    CagdCrvDomain(Crv, &TMin, &TMax);

    for (i = 0; i < MAX_OFFSET_IMPROVE_ITERS; i++) {
	CagdCrvStruct *DiffCrv, *DistSqrCrv;

	OffsetCrv = OffsetAprxFunc(Crv, OffsetDist);
	DiffCrv = CagdCrvSub(OffsetCrv, Crv);
	DistSqrCrv = CagdCrvDotProd(DiffCrv, DiffCrv);
	CagdCrvFree(DiffCrv);

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

	if (OffsetDistSqr - Min < OffsetError &&
	    Max - OffsetDistSqr < OffsetError) {
	    /* Error is within bounds - returns this offset approximation. */
	    CagdCrvFree(DistSqrCrv);
	    break;
	}
	else {
	    /* Refine in regions where the error is too large. */
	    int j, k,
	        Length = DistSqrCrv -> Length,
	        Order = DistSqrCrv -> Order,
	        KVLen = Length + Order;
	    CagdRType
		*KV = DistSqrCrv -> KnotVector,
		*Nodes = BspKnotNodes(KV, KVLen, Order),
		*RefKV = (CagdRType *) IritMalloc(sizeof(CagdRType) * 2 * Length);

	    for (j = k = 0; j < Length; j++) {
		CagdRType
		    *Pt = CagdCrvEval(DistSqrCrv, Nodes[j]),
		    V = OffsetDistSqr - (IsRational ? Pt[1] / Pt[0] : Pt[1]);

		if (ABS(V) > OffsetError) {
		    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(DistSqrCrv);
	    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 OffsetCrv;
}

/******************************************************************************
* Same as CagdCrvAdapOffset, but trims the self intersection loops.	      *
* See paper below for more.						      *
*									      *
* + Gershon Elber and Elaine Cohen. Error Bounded Variable Distance Offset    *
*   Operator for Free Form Curves and Surfaces. International Journal of      *
*   Computational Geometry & Applications, Vol. 1, Num. 1, March 1991,        *
*   pp 67-78.								      *
******************************************************************************/
CagdCrvStruct *CagdCrvAdapOffsetTrim(CagdCrvStruct *OrigCrv,
				     CagdRType OffsetDist,
				     CagdRType OffsetError,
				     CagdCrvFuncType OffsetAprxFunc)
{
    CagdBType
        IsRational = CAGD_IS_RATIONAL_CRV(OrigCrv);
    CagdCrvStruct *CrvtrSqr, *CrvtrSign, *Crv, *Crvs, *LastCrv,
	*CrvOffsetList = NULL;
    CagdPtStruct *Pts, *PtsHead, *LastPts;
    CagdRType
        OffsetDistSqr1 = 1.0 / SQR(OffsetDist);

    switch (OrigCrv -> GType) {
	case CAGD_CBEZIER_TYPE:
	    Crv = CnvrtBezier2BsplineCrv(OrigCrv);
	    break;
	case CAGD_CBSPLINE_TYPE:
	    Crv = CagdCrvCopy(OrigCrv);
	    break;
	default:
	    FATAL_ERROR(CAGD_ERR_UNDEF_CRV);
	    break;
    }

    if (OffsetDist == 0.0)
	return Crv;

    CrvtrSqr = CagdCrvCurvatureSqr(Crv);
    CrvtrSign = CagdCrvCurvatureSign(Crv);

    PtsHead = CagdCrvConstSet(CrvtrSqr, 1, SQR(OffsetError),
			      OffsetDistSqr1 / SQR(OFFSET_TRIM_EPS));

    for (Pts = PtsHead, LastPts = NULL; Pts != NULL;) {
	CagdRType *CrvtrSignPt, CrvtrSignVal;

	CrvtrSignPt = CagdCrvEval(CrvtrSign, Pts -> Pt[0]);
	CrvtrSignVal = IsRational ? CrvtrSignPt[1] / CrvtrSignPt[0]
				  : CrvtrSignPt[1];

	if (CrvtrSignVal * OffsetDist > 0.0) {
	    /* Remove point from list. */
	    if (LastPts != NULL) {
		LastPts -> Pnext = Pts -> Pnext;
		IritFree((VoidPtr) Pts);
		Pts = LastPts -> Pnext;
	    }
	    else {
		PtsHead = PtsHead -> Pnext;
		IritFree((VoidPtr) Pts);
		Pts = PtsHead;
	    }
	}
	else {
	    LastPts = Pts;
	    Pts = Pts -> Pnext;
	}
    }

    for (Pts = PtsHead;
	 Pts != NULL || Crv != NULL;
	 Pts = (Pts != NULL ? Pts -> Pnext : NULL)) {
	CagdCrvStruct
	    *Crv1 = Pts ? CagdCrvSubdivAtParam(Crv, Pts -> Pt[0])
			: CagdCrvCopy(Crv),
	    *Crv2 = Pts ? Crv1 -> Pnext : NULL;
	CagdRType Min, Max, *CrvtrSqrPt, CrvtrSqrVal,
						*CrvtrSignPt, CrvtrSignVal;

	CagdCrvDomain(Crv1, &Min, &Max);

	CrvtrSqrPt = CagdCrvEval(CrvtrSqr, (Min + Max) / 2);
	CrvtrSqrVal = CrvtrSqrPt[1] / CrvtrSqrPt[0];
	CrvtrSignPt = CagdCrvEval(CrvtrSign, (Min + Max) / 2);
	CrvtrSignVal = IsRational ? CrvtrSignPt[1] / CrvtrSignPt[0]
				  : CrvtrSignPt[1];

	if (CrvtrSqrVal < OffsetDistSqr1 / SQR(OFFSET_TRIM_EPS) ||
	    CrvtrSignVal * OffsetDist > 0.0) {
	    CagdCrvStruct
		*Crv1Off = CagdCrvAdapOffset(Crv1, OffsetDist, OffsetError,
					     OffsetAprxFunc);

	    CAGD_LIST_PUSH(Crv1Off, CrvOffsetList);
	}

	CagdCrvFree(Crv1);
	CagdCrvFree(Crv);
	Crv = Crv2;
    }

    Crvs = CagdListReverse(CrvOffsetList);
    CrvOffsetList = NULL;
    LastCrv = Crvs;
    Crvs = Crvs -> Pnext;
    for (Crv = Crvs; Crv != NULL; Crv = Crv -> Pnext) {
	CagdCrvStruct *CTmp, *CTmp2;
	CagdSrfStruct
	    *DistSrf = CagdSrfDistCrvCrv(LastCrv, Crv);
	CagdPtStruct
	    *IPts = CagdSrfDistFindPoints(DistSrf, OffsetError, FALSE);

	CagdSrfFree(DistSrf);

	if (IPts != NULL) {
	    if (IPts -> Pnext) {
		FATAL_ERROR(CAGD_ERR_TOO_COMPLEX);
		return NULL;
	    }
	    else {
		CagdRType TMin, TMax;

		CagdCrvDomain(LastCrv, &TMin, &TMax);
		CTmp = CagdCrvRegionFromCrv(LastCrv, TMin, IPts -> Pt[0]);
		CagdCrvFree(LastCrv);
		LastCrv = CTmp;

		CagdCrvDomain(Crv, &TMin, &TMax);
		CTmp = CagdCrvRegionFromCrv(Crv, IPts -> Pt[1], TMax);

		CTmp2 = CagdMergeCrvCrv(LastCrv, CTmp);
		CagdCrvFree(CTmp);
		CagdCrvFree(LastCrv);
		LastCrv = CTmp2;
	    }
	}
	else {
	    /* Simply chain the pieces together. */
	    CTmp = CagdMergeCrvCrv(LastCrv, Crv);
	    CagdCrvFree(LastCrv);
	    LastCrv = CTmp;
	}
    }

    CagdPtFreeList(PtsHead);
    CagdCrvFree(CrvtrSqr);
    CagdCrvFree(CrvtrSign);

    return LastCrv;
}