/******************************************************************************
* Ray-trap.c - computes solution for rays bouncing for every between curves.  *
*******************************************************************************
* (C) Gershon Elber, Technion, Israel Institute of Technology                 *
*******************************************************************************
* Written by Gershon Elber, Aug. 99.					      *
******************************************************************************/

#include "irit_sm.h"
#include "iritprsr.h"
#include "allocate.h"
#include "geom_lib.h"
#include "cagd_lib.h"
#include "mvar_loc.h"

#define MVAR_RAY_TRAP_MAX_CRVS 10
#define MVAR_RAY_TRAP_FILTER_REVERSED   /* Filter rays bouncing into object. */

static MvarMVStruct *MvarBuildOneRayTrapConstrnt(MvarMVStruct *Mv1,
						 MvarMVStruct *Mv2,
						 MvarMVStruct *Mv3,
						 MvarMVStruct *Mv2Nrml);

/*****************************************************************************
* DESCRIPTION:                                                               M
*   Computes solutions to locations on the given curves that would bounce    M
* rays from one curve to the next in a closed loop.                          M
*                                                                            *
* PARAMETERS:                                                                M
*   Crvs:         List of curves to handle in order (cyclically).            M
*   SubdivTol:    Tolerance of the solution.  This tolerance is measured in  M
*		  the parametric space of the curves.			     M
*   NumerTol:     Numeric tolerance of a possible numeric improvment stage.  M
*		  The numeric stage is employed if NumericTol < SubdivTol.   M
*                                                                            *
* RETURN VALUE:                                                              M
*   MvarPtStruct *:  Linked list of solutions, each holding the parameter    M
*		values of the different Crvs.				     M
*                                                                            *
* KEYWORDS:                                                                  M
*   MvarComputeRayTraps                                                      M
*****************************************************************************/
MvarPtStruct *MvarComputeRayTraps(CagdCrvStruct *Crvs,
				  CagdRType SubdivTol,
				  CagdRType NumerTol)
{
    int i, j,
	n = CagdListLength(Crvs);
    CagdCrvStruct *Crv;
    CagdRType TMin[MVAR_RAY_TRAP_MAX_CRVS], TMax[MVAR_RAY_TRAP_MAX_CRVS];
    MvarMVStruct *MVCrvs[MVAR_RAY_TRAP_MAX_CRVS],
		 *MVNrmls[MVAR_RAY_TRAP_MAX_CRVS],
		 *MVCnstrns[MVAR_RAY_TRAP_MAX_CRVS];
    MvarConstraintType Constraints[MVAR_RAY_TRAP_MAX_CRVS];
    MvarPtStruct *Pts, *Pt;

    if (n > MVAR_RAY_TRAP_MAX_CRVS) {
	MVAR_FATAL_ERROR(MVAR_ERR_TOO_MANY_PARAMS);
	return NULL;
    }

    /* Convert the curves into multivariates and promote each to an          */
    /* n-dimensional variety, spanning dimension i.			     */
    for (Crv = Crvs, i = 0; Crv != NULL; Crv = Crv -> Pnext, i++) {
	CagdRType **Points;
	CagdCrvStruct *Crv2, *DCrv;
	MvarMVStruct *MVTmp;

	Crv2 = CagdCrvCopy(Crv);
	CagdCrvDomain(Crv2, &TMin[i], &TMax[i]);
	if (CAGD_IS_BSPLINE_CRV(Crv2))
	    BspKnotAffineTransOrder2(Crv2 -> KnotVector, Crv2 -> Order,
				     Crv2 -> Length + Crv2 -> Order, 0.0, 1.0);
	MVTmp = MvarCrvToMV(Crv2);

	MVCrvs[i] = MvarPromoteMVToMV2(MVTmp, n, i);
	MvarMVFree(MVTmp);

	/* Now compute a normal field for the curve. */
	DCrv = CagdCrvDerive(Crv2);
	Points = DCrv -> Points;
	for (j = 0; j < DCrv -> Length; j++) {
	    SWAP(CagdRType, Points[1][j], Points[2][j]);
	    Points[1][j] = -Points[1][j];
	}
	MVTmp = MvarCrvToMV(DCrv);
	CagdCrvFree(Crv2);
	CagdCrvFree(DCrv);
	MVNrmls[i] = MvarPromoteMVToMV2(MVTmp, n, i);
	MvarMVFree(MVTmp);
    }

    /* Build the bouncing ray's constraints. */
    for (i = 0; i < n; i++) {
	MVCnstrns[i] = MvarBuildOneRayTrapConstrnt(MVCrvs[i],
						   MVCrvs[(i + 1) % n],
						   MVCrvs[(i + 2) % n],
						   MVNrmls[(i + 1) % n]);
	Constraints[i] = MVAR_CNSTRNT_ZERO;
    }
    
    Pts = MvarMVsZeros(MVCnstrns, Constraints, n, SubdivTol, NumerTol);

    for (i = 0; i < n; i++) {
	MvarMVFree(MVCrvs[i]);
	MvarMVFree(MVNrmls[i]);
        MvarMVFree(MVCnstrns[i]);
    }

    /* Recover the solution's domain as we transformed to (0, 1) domains. */
    for (Pt = Pts; Pt != NULL; Pt = Pt -> Pnext) {
	for (i = 0; i < n; i++)
	    Pt -> Pt[i] = Pt -> Pt[i] * (TMax[i] - TMin[i]) + TMin[i];
    }

#ifdef MVAR_RAY_TRAP_FILTER_REVERSED
    {
	MvarPtStruct *PtNext,
	    *Pt = Pts;
	CagdPType Pos[MVAR_RAY_TRAP_MAX_CRVS];
	CagdVType Nrml[MVAR_RAY_TRAP_MAX_CRVS];

	Pts = NULL;
	while (Pt != NULL) {
	    PtNext = Pt -> Pnext;

	    Pt -> Pnext = NULL;

	    /* Evaluate each points in case we are in wrong orientation. */
	    for (Crv = Crvs, i = 0; Crv != NULL; Crv = Crv -> Pnext, i++) {
		CagdRType
		    *R = CagdCrvEval(Crv, Pt -> Pt[i]);
		CagdVecStruct
		    *V = CagdCrvNormal(Crv, Pt -> Pt[i], FALSE);

		CagdCoerceToE3(Pos[i], &R, -1, Crv -> PType);
		VEC_COPY(Nrml[i], V -> Vec);
	    }

	    /* Now test orientation of the constraint for a different sign. */
	    for (i = 0; i < n; i++) {
		CagdVType V1, V2, V1Tmp, V2Tmp;
		CagdRType
		    *N = Nrml[(i + 1) % n];

		VEC_SUB(V1, Pos[(i + 1) % n], Pos[i]);
		VEC_SUB(V2, Pos[(i + 1) % n], Pos[(i + 2) % n]);

		/* Test that V1/V2 point in the same normal direction. */
		if (DOT_PROD(V1, N) *
		    DOT_PROD(V2, N) < 0.0)
		    break;
		/* Test that V1 and V2 are in opposite sides of the normal. */
		CROSS_PROD(V1Tmp, V1, N);
		CROSS_PROD(V2Tmp, N, V2);
		if (DOT_PROD(V1Tmp, V2Tmp) < 0.0)
		    break;
	    }
	    if (i < n) {
		/* Failed - purge this location. */
		MvarPtFree(Pt);
	    }
	    else {
		LIST_PUSH(Pt, Pts);
	    }

	    Pt = PtNext;
	}
    }
    
#endif /* MVAR_RAY_TRAP_FILTER_REVERSED */

    return Pts;
}

/*****************************************************************************
* DESCRIPTION:                                                               *
*   Build the ray constraint of a ray coming from Mv1 bouncing of Mv2 to Mv3 *
* as an angular constraint on the coming (to Mv2) and leaving (Mv2) angles   *
* with respect to the normal of Mv2.					     *
*                                                                            *
* PARAMETERS:                                                                *
*   Mv1, Mv2, Mv3: Three consecutive curves to formulate the constraint for. *
*   Mv2Nrml:	   The normal field of Mv2.				     *
*                                                                            *
* RETURN VALUE:                                                              *
*   MvarMVStruct *:  A zero set constraint for the angular equality.         *
*****************************************************************************/
static MvarMVStruct *MvarBuildOneRayTrapConstrnt(MvarMVStruct *Mv1,
						 MvarMVStruct *Mv2,
						 MvarMVStruct *Mv3,
						 MvarMVStruct *Mv2Nrml)
{
    MvarMVStruct
	*Dist12 = MvarMVSub(Mv1, Mv2),
	*Dist32 = MvarMVSub(Mv3, Mv2),
	*Ang12 = MvarMVDotProd(Dist12, Mv2Nrml),
	*Ang32 = MvarMVDotProd(Dist32, Mv2Nrml),
	*Ang12Sqr = MvarMVMult(Ang12, Ang12),
	*Ang32Sqr = MvarMVMult(Ang32, Ang32),
	*Dist12Sqr = MvarMVDotProd(Dist12, Dist12),
	*Dist32Sqr = MvarMVDotProd(Dist32, Dist32),
	*Mult1 = MvarMVMult(Ang12Sqr, Dist32Sqr),
	*Mult2 = MvarMVMult(Ang32Sqr, Dist12Sqr),
	*RetVal = MvarMVSub(Mult1, Mult2);

    MvarMVFree(Dist12);
    MvarMVFree(Dist32);
    MvarMVFree(Ang12);
    MvarMVFree(Ang32);
    MvarMVFree(Ang12Sqr);
    MvarMVFree(Ang32Sqr);
    MvarMVFree(Dist12Sqr);
    MvarMVFree(Dist32Sqr);
    MvarMVFree(Mult1);
    MvarMVFree(Mult2);

    return RetVal;
}