/******************************************************************************
* CagdCMrg.c - Curve/Point merging routines.				      *
*******************************************************************************
* Written by Gershon Elber, May 91.					      *
******************************************************************************/

#include "string.h"
#include "cagd_loc.h"

static void CopyCrvOnCrv(CagdCrvStruct *DestCrv, int Index,
			 CagdCrvStruct *SrcCrv);
static void InterpolateLinearSeg(CagdCrvStruct *Crv, int Index1, int Index2);

/******************************************************************************
* Merge two curves.							      *
******************************************************************************/
CagdCrvStruct *CagdMergeCrvCrv(CagdCrvStruct *Crv1, CagdCrvStruct *Crv2)
{
    CagdBType CrvsSharePt,
	Crv1New = FALSE,
	Crv2New = FALSE,
	IsRational1 = CAGD_IS_RATIONAL_CRV(Crv1),
	IsRational2 = CAGD_IS_RATIONAL_CRV(Crv2);
    int Length, Order,
	Order1 = Crv1 -> Order,
	Order2 = Crv2 -> Order,
	Len1 = Crv1 -> Length,
	Len2 = Crv2 -> Length,
	MaxCoord1 = CAGD_NUM_OF_PT_COORD(Crv1 -> PType),
	MaxCoord2 = CAGD_NUM_OF_PT_COORD(Crv2 -> PType);
    CagdRType E3Pt1[3], E3Pt2[3];
    CagdPointType CrvPType = CAGD_PT_E2_TYPE;
    CagdCrvStruct *Crv;

    /* Make orders compatible: */
    if (Order1 < Order2) {
	for (; Order1 < Order2; Order1++) {
	    Crv = CagdCrvDegreeRaise(Crv1);
	    if (Crv1New)
		CagdCrvFree(Crv1);
	    Crv1 = Crv;
	    Len1 = Crv1 -> Length;
	    Crv1New = TRUE;
	}
    }
    else if (Order2 < Order1) {
	for (; Order2 < Order1; Order2++) {
	    Crv = CagdCrvDegreeRaise(Crv2);
	    if (Crv2New)
		CagdCrvFree(Crv2);
	    Crv2 = Crv;
	    Len2 = Crv2 -> Length;
	    Crv2New = TRUE;
	}
    }
    Order = Order1;

    /* Verify curve geometric types. */
    switch (Crv1 -> GType) {
	case CAGD_CBEZIER_TYPE:
	    Crv = CnvrtBezier2BsplineCrv(Crv1);
	    if (Crv1New)
		CagdCrvFree(Crv1);
	    Crv1 = Crv;
	    Crv1New = TRUE;
	    break;
	case CAGD_CBSPLINE_TYPE:
	    break;
	case CAGD_CPOWER_TYPE:
	    FATAL_ERROR(CAGD_ERR_POWER_NO_SUPPORT);
	    break;
	default:
	    FATAL_ERROR(CAGD_ERR_UNDEF_CRV);
	    break;
    }

    switch (Crv2 -> GType) {
	case CAGD_CBEZIER_TYPE:
	    Crv = CnvrtBezier2BsplineCrv(Crv2);
	    if (Crv2New)
		CagdCrvFree(Crv2);
	    Crv2 = Crv;
	    Crv2New = TRUE;
	    break;
	case CAGD_CBSPLINE_TYPE:
	    break;
	case CAGD_CPOWER_TYPE:
	    FATAL_ERROR(CAGD_ERR_POWER_NO_SUPPORT);
	    break;
	default:
	    FATAL_ERROR(CAGD_ERR_UNDEF_CRV);
	    break;
    }

    /* Compute curve point types. */
    CrvPType = CAGD_MAKE_PT_TYPE(IsRational1 || IsRational2,
				 MAX(MaxCoord1, MaxCoord2));


    /* Figure out if end point of Crv1 is equal to start point of Crv2 and   */
    /* Compute the length of the resulting curve accordingly.		     */
    /* If the same point, then the result can omit one copy and therefore    */
    /* has Len1 + Len2 - 1 Ctl points. If however a linear segment should be */
    /* introduced between the two curves, it should hold Order colinear pts  */
    /* including 2 end points shared with curves or Order - 2 new pts.       */
    CagdCoerceToE3(E3Pt1, Crv1 -> Points, Len1 - 1, Crv1 -> PType);
    CagdCoerceToE3(E3Pt2, Crv2 -> Points, 0, Crv2 -> PType);
    CrvsSharePt = APX_EQ(E3Pt1[0], E3Pt2[0]) &&
		  APX_EQ(E3Pt1[1], E3Pt2[1]) &&
		  APX_EQ(E3Pt1[2], E3Pt2[2]);
    Length = CrvsSharePt ? Len1 + Len2 - 1 : Len1 + Len2 + Order - 2;

    Crv = BspCrvNew(Length, Order, CrvPType);
    CopyCrvOnCrv(Crv, 0, Crv1);
    CopyCrvOnCrv(Crv, Length - Len2, Crv2);
    InterpolateLinearSeg(Crv, Len1 - 1, Length - Len2);

    /* Update the knot vector. We assume open end condition here... */
    GEN_COPY(Crv -> KnotVector, Crv1 -> KnotVector,
	     (Len1 + Order1 - 1) * sizeof(CagdRType));
    if (CrvsSharePt) {
	GEN_COPY(&Crv -> KnotVector[Len1 + Order1 - 1],
		 &Crv2 -> KnotVector[Order2], Len2 * sizeof(CagdRType));
	BspKnotAffineTrans(&Crv -> KnotVector[Len1 + Order1 - 1],
			   Len2,
			   Crv -> KnotVector[Len1 + Order1 - 2] -
			   Crv2 -> KnotVector[0],
			   1.0);
    }
    else {
	GEN_COPY(&Crv -> KnotVector[Len1 + Order1 - 1],
		 &Crv2 -> KnotVector[1],
		 (Len2 + Order2 - 1) * sizeof(CagdRType));
	BspKnotAffineTrans(&Crv -> KnotVector[Len1 + Order1 - 1],
			   Len2 + Order2 - 1,
			   Crv -> KnotVector[Len1 + Order1 - 2] -
			   Crv -> KnotVector[Len1 + Order1 - 1] + 1.0,
			   1.0);
    }

    if (Crv1New)
	CagdCrvFree(Crv1);
    if (Crv2New)
	CagdCrvFree(Crv2);

    return Crv;
}

/******************************************************************************
* Merge a curve and a point.						      *
******************************************************************************/
CagdCrvStruct *CagdMergeCrvPt(CagdCrvStruct *Crv, CagdPtStruct *Pt)
{
    CagdBType
	CrvNew = FALSE,
	IsRational = CAGD_IS_RATIONAL_CRV(Crv);
    int i, NewLen, NewMaxCoord,
	Order = Crv -> Order,
	Len = Crv -> Length,
	PtMaxCoord = APX_EQ(Pt -> Pt[2], 0.0) ? 2 : 3,
	MaxCoord = CAGD_NUM_OF_PT_COORD(Crv -> PType);
    CagdPointType
	CrvPType = CAGD_PT_E2_TYPE;
    CagdRType t, **Points;
    CagdCrvStruct *NewCrv;

    switch (Crv -> GType) {
	case CAGD_CBEZIER_TYPE:
	    Crv = CnvrtBezier2BsplineCrv(Crv);
	    CrvNew = TRUE;
	case CAGD_CBSPLINE_TYPE:
	    break;
	case CAGD_CPOWER_TYPE:
	    FATAL_ERROR(CAGD_ERR_POWER_NO_SUPPORT);
	    break;
	default:
	    FATAL_ERROR(CAGD_ERR_UNDEF_CRV);
	    break;
    }

    /* Compute curve point types. */
    NewMaxCoord = MAX(PtMaxCoord, MaxCoord);
    CrvPType = CAGD_MAKE_PT_TYPE(IsRational, NewMaxCoord);

    /* A linear segment is added at the end with Order colinear pts.        */
    /* However since first point is curve last point, only Order - 1 new.   */
    NewLen = Len + Order - 1;

    NewCrv = BspCrvNew(NewLen, Order, CrvPType);
    Points = NewCrv -> Points;

    CopyCrvOnCrv(NewCrv, 0, Crv);
    for (i = 1; i <= NewMaxCoord; i++) Points[i][NewLen - 1] = Pt -> Pt[i - 1];
    if (IsRational) Points[W][NewLen - 1] = 1.0;
    InterpolateLinearSeg(NewCrv, Len - 1, NewLen - 1);

    /* Update the knot vector. We assume open end condition here... */
    GEN_COPY(NewCrv -> KnotVector, Crv -> KnotVector,
	     (Len + Order - 1) * sizeof(CagdRType));
    t = Crv -> KnotVector[Len + Order - 1] + 1.0;
    for (i = Len + Order - 1; i < NewLen + Order; i++)
	NewCrv -> KnotVector[i] = t;

    if (CrvNew)
	CagdCrvFree(Crv);

    return NewCrv;
}

/******************************************************************************
* Merge a point and a curve.						      *
******************************************************************************/
CagdCrvStruct *CagdMergePtCrv(CagdPtStruct *Pt, CagdCrvStruct *Crv)
{
    CagdBType
	CrvNew = FALSE,
	IsRational = CAGD_IS_RATIONAL_CRV(Crv);
    int i, NewLen, NewMaxCoord,
	Order = Crv -> Order,
	Len = Crv -> Length,
	PtMaxCoord = APX_EQ(Pt -> Pt[2], 0.0) ? 2 : 3,
	MaxCoord = CAGD_NUM_OF_PT_COORD(Crv -> PType);
    CagdPointType
	CrvPType = CAGD_PT_E2_TYPE;
    CagdRType t, **Points;
    CagdCrvStruct *NewCrv;

    switch (Crv -> GType) {
	case CAGD_CBEZIER_TYPE:
	    Crv = CnvrtBezier2BsplineCrv(Crv);
	    CrvNew = TRUE;
	    break;
	case CAGD_CBSPLINE_TYPE:
	    break;
	case CAGD_CPOWER_TYPE:
	    FATAL_ERROR(CAGD_ERR_POWER_NO_SUPPORT);
	    break;
	default:
	    FATAL_ERROR(CAGD_ERR_UNDEF_CRV);
	    break;
    }

    /* Compute curve point types. */
    NewMaxCoord = MAX(PtMaxCoord, MaxCoord);
    CrvPType = CAGD_MAKE_PT_TYPE(IsRational, NewMaxCoord);

    /* A linear segment is added at the end with Order colinear pts.        */
    /* However since first point is curve last point, only Order - 1 new.   */
    NewLen = Len + Order - 1;

    NewCrv = BspCrvNew(NewLen, Order, CrvPType);
    Points = NewCrv -> Points;

    CopyCrvOnCrv(NewCrv, Order - 1, Crv);
    for (i = 1; i <= NewMaxCoord; i++) Points[i][0] = Pt -> Pt[i - 1];
    if (IsRational) Points[W][0] = 1.0;
    InterpolateLinearSeg(NewCrv, 0, Order - 1);

    /* Update the knot vector. We assume open end condition here... */
    GEN_COPY(&NewCrv -> KnotVector[Order], &Crv -> KnotVector[1],
	     (Len + Order - 1) * sizeof(CagdRType));
    t = Crv -> KnotVector[0] - 1.0;
    for (i = 0; i < Order; i++)
	NewCrv -> KnotVector[i] = t;

    if (CrvNew)
	CagdCrvFree(Crv);

    return NewCrv;
}

/******************************************************************************
* Merge two points into a linear Bezier curve.				      *
******************************************************************************/
CagdCrvStruct *CagdMergePtPt(CagdPtStruct *Pt1, CagdPtStruct *Pt2)
{
    CagdPointType
	CrvPType = APX_EQ(Pt1 -> Pt[2], 0.0) && APX_EQ(Pt2 -> Pt[2], 0.0) ?
					CAGD_PT_E2_TYPE : CAGD_PT_E3_TYPE;
    CagdCrvStruct
	*Crv = BzrCrvNew(2, CrvPType);
    CagdRType
	**Points = Crv -> Points;

    Points[X][0] = Pt1 -> Pt[0];
    Points[X][1] = Pt2 -> Pt[0];
    Points[Y][0] = Pt1 -> Pt[1];
    Points[Y][1] = Pt2 -> Pt[1];
    if (CrvPType == CAGD_PT_E3_TYPE) {
    	Points[Z][0] = Pt1 -> Pt[2];
    	Points[Z][1] = Pt2 -> Pt[2];
    }

    return Crv;
}

/******************************************************************************
* Copy SrcCrv into DestCrv at point index Index.			      *
* DestCrv PType is assumed to hold Src PType.				      *
******************************************************************************/
static void CopyCrvOnCrv(CagdCrvStruct *DestCrv, int Index,
			 CagdCrvStruct *SrcCrv)
{
    CagdBType
	IsNotRational = !CAGD_IS_RATIONAL_CRV(SrcCrv);
    int i, j,
	Len = SrcCrv -> Length,
	MaxCoord = CAGD_NUM_OF_PT_COORD(SrcCrv -> PType);
    CagdRType
	**SrcPoints = SrcCrv -> Points,
	**DestPoints = DestCrv -> Points;

    for (i = IsNotRational; i <= MaxCoord; i++)
	GEN_COPY(&DestPoints[i][Index], SrcPoints[i],
		 Len * sizeof(CagdRType));

    /* Fix the weights if source did not have them. */
    if (IsNotRational && CAGD_IS_RATIONAL_CRV(DestCrv))
	for (i = Index; i < Index + Len; i++)
	    DestPoints[W][i] = 1.0;

    /* Fix the higher coordinates (by coercing them to zero. */
    for (i = MaxCoord + 1; i < CAGD_NUM_OF_PT_COORD(DestCrv -> PType); i++)
	for (j = Index; j < Index + Len; j++)
	    DestPoints[i][j] = 0.0;

}

/******************************************************************************
* Linearly interpolate between the Crv points indices Index1 and Index2       *
******************************************************************************/
static void InterpolateLinearSeg(CagdCrvStruct *Crv, int Index1, int Index2)
{
    CagdBType
	IsNotRational = !CAGD_IS_RATIONAL_CRV(Crv);
    int i, j,
	DIndex = Index2 - Index1,
	MaxCoord = CAGD_NUM_OF_PT_COORD(Crv -> PType);
    CagdRType
	**Points = Crv -> Points;

    if (DIndex < 2) return;		     /* No middle points to interp. */

    for (i = Index1 + 1; i < Index2; i++) {
	CagdRType
	    t1 = ((CagdRType) (i - Index1)) / DIndex,
	    t2 = 1.0 - t1;

	for (j = IsNotRational; j <= MaxCoord; j++)
	    Points[j][i] = t2 * Points[j][Index1] + t1 * Points[j][Index2];
    }
}