/******************************************************************************
* An implementation of the marching cube algorithm.			      *
*									      *
*						Gershon Elber, Dec 1992.      *
******************************************************************************/

#include <stdio.h>
#include "irit_sm.h"
#include "mrchcube.h"

#define MARCH_CUBE_EPS	1e-8

static int MCComputeEdgeInter(MCCubeCornerScalarStruct *CCS,
			      RealType Threshold,
			      int VrtxIndex1,
			      int VrtxIndex2,
			      PointType Pos,
			      PointType Nrml);
static void MCAppendEList(MCEdgeStruct *NewEdges, MCEdgeStruct **EdgeList);
static MCEdgeStruct *MCGetEdgesFromFace(MCCubeCornerScalarStruct *CCS,
					int InterEdgeIndex1,
					int InterEdgeIndex2,
					int InterEdgeIndex3,
					int InterEdgeIndex4);
static void MCPurgeZeroLenEdges(MCEdgeStruct **AllEList);

/*****************************************************************************
* DESCRIPTION:                                                               M
* Given 8 cube corner values (scalars), compute the polygon(s) in this cube  M
* along the isosurface at Threshold. if CCS has gradient information, it is  M
* used to approximate normals at the vertices.				     M
*                                                                            M
*                                                                            M
*                                                                            V
*                     7            K           6                             V
*                      ***********************                               V
*                    * +                   * *                               V
*                L *   +                 *   *              Vertices 0 - 7   V
*                *     +     I         * J   *              Edges    A - L   V
*            4 *********************** 5     *                               V
*              *       +             *       *                               V
*              *       +             *       * G                             V
*              *       + H           *       *                               V
*              *       +             *       *                               V
*              *       +             * F     *                               V
*            E *       +       C     *       *                               V
*              *       ++++++++++++++*+++++++* 2                             V
*              *   D + 3             *     *                                 V
*              *   +                 *   * B                                 V
*              * +                   * *                                     V
*              ***********************                                       V
*             0           A           1                                      V
*                                                                            *
* PARAMETERS:                                                                M
*   CCS:          The cube's dimensions/information.                         M
*   Threshold:    Iso surface level.                                         M
*                                                                            *
* RETURN VALUE:                                                              M
*   MCPolygonStruct *:   List of polygons (not necessarily triangles), or    M
*                        possibly NULL.                                      M
*                                                                            *
* KEYWORDS:                                                                  M
*   MCThresholdCube, marching cubes                                          M
*****************************************************************************/
MCPolygonStruct *MCThresholdCube(MCCubeCornerScalarStruct *CCS,
				 RealType Threshold)
{
    MCEdgeStruct
	*AllEList = NULL;
    MCPolygonStruct
	*AllPList = NULL;

    /* Computes the position of the 8 _Vertices. */
    MC_COMP_V_POS(0.0,               0.0,               0.0,
		  MC_VRTX_0);
    MC_COMP_V_POS(CCS -> CubeDim[0], 0.0,               0.0,
		  MC_VRTX_1);
    MC_COMP_V_POS(CCS -> CubeDim[0], CCS -> CubeDim[1], 0.0,
		  MC_VRTX_2);
    MC_COMP_V_POS(0.0,               CCS -> CubeDim[1], 0.0,
		  MC_VRTX_3);

    MC_COMP_V_POS(0.0,               0.0,               CCS -> CubeDim[2],
		  MC_VRTX_4);
    MC_COMP_V_POS(CCS -> CubeDim[0], 0.0,               CCS -> CubeDim[2],
		  MC_VRTX_5);
    MC_COMP_V_POS(CCS -> CubeDim[0], CCS -> CubeDim[1], CCS -> CubeDim[2],
		  MC_VRTX_6);
    MC_COMP_V_POS(0.0,               CCS -> CubeDim[1], CCS -> CubeDim[2],
		  MC_VRTX_7);

    /* Test and compute any edge that intersect the Threshold value. */
    CCS -> _Intersect = FALSE;
    CCS -> _InterHighV[MC_EDGE_A] =
	MCComputeEdgeInter(CCS, Threshold, MC_VRTX_0, MC_VRTX_1,
			   CCS -> _InterPos[MC_EDGE_A],
			   CCS -> _InterNrml[MC_EDGE_A]);
    CCS -> _InterHighV[MC_EDGE_B] =
	MCComputeEdgeInter(CCS, Threshold, MC_VRTX_1, MC_VRTX_2,
			   CCS -> _InterPos[MC_EDGE_B],
			   CCS -> _InterNrml[MC_EDGE_B]);
    CCS -> _InterHighV[MC_EDGE_C] =
	MCComputeEdgeInter(CCS, Threshold, MC_VRTX_2, MC_VRTX_3,
			   CCS -> _InterPos[MC_EDGE_C],
			   CCS -> _InterNrml[MC_EDGE_C]);
    CCS -> _InterHighV[MC_EDGE_D] =
	MCComputeEdgeInter(CCS, Threshold, MC_VRTX_3, MC_VRTX_0,
			   CCS -> _InterPos[MC_EDGE_D],
			   CCS -> _InterNrml[MC_EDGE_D]);
    CCS -> _InterHighV[MC_EDGE_E] =
	MCComputeEdgeInter(CCS, Threshold, MC_VRTX_0, MC_VRTX_4,
			   CCS -> _InterPos[MC_EDGE_E],
			   CCS -> _InterNrml[MC_EDGE_E]);
    CCS -> _InterHighV[MC_EDGE_F] =
	MCComputeEdgeInter(CCS, Threshold, MC_VRTX_1, MC_VRTX_5,
			   CCS -> _InterPos[MC_EDGE_F],
			   CCS -> _InterNrml[MC_EDGE_F]);
    CCS -> _InterHighV[MC_EDGE_G] =
	MCComputeEdgeInter(CCS, Threshold, MC_VRTX_2, MC_VRTX_6,
			   CCS -> _InterPos[MC_EDGE_G],
			   CCS -> _InterNrml[MC_EDGE_G]);
    CCS -> _InterHighV[MC_EDGE_H] =
	MCComputeEdgeInter(CCS, Threshold, MC_VRTX_3, MC_VRTX_7,
			   CCS -> _InterPos[MC_EDGE_H],
			   CCS -> _InterNrml[MC_EDGE_H]);
    CCS -> _InterHighV[MC_EDGE_I] =
	MCComputeEdgeInter(CCS, Threshold, MC_VRTX_4, MC_VRTX_5,
			   CCS -> _InterPos[MC_EDGE_I],
			   CCS -> _InterNrml[MC_EDGE_I]);
    CCS -> _InterHighV[MC_EDGE_J] =
	MCComputeEdgeInter(CCS, Threshold, MC_VRTX_5, MC_VRTX_6,
			   CCS -> _InterPos[MC_EDGE_J],
			   CCS -> _InterNrml[MC_EDGE_J]);
    CCS -> _InterHighV[MC_EDGE_K] =
	MCComputeEdgeInter(CCS, Threshold, MC_VRTX_6, MC_VRTX_7,
			   CCS -> _InterPos[MC_EDGE_K],
			   CCS -> _InterNrml[MC_EDGE_K]);
    CCS -> _InterHighV[MC_EDGE_L] =
	MCComputeEdgeInter(CCS, Threshold, MC_VRTX_4, MC_VRTX_7,
			   CCS -> _InterPos[MC_EDGE_L],
			   CCS -> _InterNrml[MC_EDGE_L]);
    if (!CCS->_Intersect)
	return NULL;

    /* For each of the six faces, compute polygon interior edges for that   */
    /* face and save all of them into a global list. 			    */
    MCAppendEList(MCGetEdgesFromFace(CCS, MC_EDGE_D, MC_EDGE_C,
				          MC_EDGE_B, MC_EDGE_A), &AllEList);
    MCAppendEList(MCGetEdgesFromFace(CCS, MC_EDGE_A, MC_EDGE_F,
				          MC_EDGE_I, MC_EDGE_E), &AllEList);
    MCAppendEList(MCGetEdgesFromFace(CCS, MC_EDGE_B, MC_EDGE_G,
				          MC_EDGE_J, MC_EDGE_F), &AllEList);
    MCAppendEList(MCGetEdgesFromFace(CCS, MC_EDGE_C, MC_EDGE_H,
				          MC_EDGE_K, MC_EDGE_G), &AllEList);
    MCAppendEList(MCGetEdgesFromFace(CCS, MC_EDGE_D, MC_EDGE_E,
				          MC_EDGE_L, MC_EDGE_H), &AllEList);
    MCAppendEList(MCGetEdgesFromFace(CCS, MC_EDGE_I, MC_EDGE_J,
				          MC_EDGE_K, MC_EDGE_L), &AllEList);

    MCPurgeZeroLenEdges(&AllEList);

#ifdef DEBUG_PRINT_EDGES
    {
	MCEdgeStruct
	    *E = AllEList;
	
	fprintf(stderr, "\nCube edges [%lf %lf %lf] [%lf %lf %lf]:\n",
		CCS -> Vrtx0Lctn[0], CCS -> Vrtx0Lctn[1], CCS -> Vrtx0Lctn[2],
		CCS -> CubeDim[0], CCS -> CubeDim[1], CCS -> CubeDim[2]);
	for (; E != NULL; E = E -> Pnext) {
	    fprintf(stderr,
		    "Edge = %10.6lg, %10.6lg, %10.6lg,    %10.6lg, %10.6lg, %10.6lg\n",
		    E -> V[0][0], E -> V[0][1], E -> V[0][2],
		    E -> V[1][0], E -> V[1][1], E -> V[1][2]);		    
	}
    }
#endif /* DEBUG_PRINT_EDGES */

    /* Merge the Vertices into polygons. */
    while (AllEList != NULL) {
	int NumOfVertices = 2;
	CagdBType
	    ClosedPoly = FALSE;
	MCPolygonStruct
	    *P = (MCPolygonStruct *) IritMalloc(sizeof(MCPolygonStruct));
	MCEdgeStruct *E2, *E2Prev,
	    *E = AllEList;

	PT_COPY(P -> V[0], E -> V[0]);
	PT_COPY(P -> N[0], E -> N[0]);
	PT_COPY(P -> V[1], E -> V[1]);
	PT_COPY(P -> N[1], E -> N[1]);

	AllEList = AllEList -> Pnext;
	E -> Pnext = NULL;
	IritFree((VoidPtr) E);

	while (!ClosedPoly) {
	    if (AllEList == NULL) {
		TRIV_FATAL_ERROR(TRIV_ERR_NO_CLOSED_POLYGON);
		return NULL;
	    }

	    for (E2 = E2Prev = AllEList; E2 != NULL;) {
		CagdBType
		    FoundMatch = FALSE;

		if (PT_APX_EQ_EPS(P -> V[NumOfVertices - 1], E2 -> V[1],
				  MARCH_CUBE_EPS)) {
		    PT_COPY(P -> V[NumOfVertices], E2 -> V[0]);
		    PT_COPY(P -> N[NumOfVertices], E2 -> N[0]);
		    NumOfVertices++;
		    FoundMatch = TRUE;
		}
		else if (PT_APX_EQ_EPS(P -> V[NumOfVertices - 1],
				       E2 -> V[0], MARCH_CUBE_EPS)) {
		    PT_COPY(P -> V[NumOfVertices], E2 -> V[1]);
		    PT_COPY(P -> N[NumOfVertices], E2 -> N[1]);
		    NumOfVertices++;
		    FoundMatch = TRUE;
		}

		if (FoundMatch) {
		    if (E2 == AllEList) {
			AllEList = AllEList -> Pnext;
		    }
		    else {
			E2Prev -> Pnext = E2 -> Pnext;
		    }
		    IritFree((VoidPtr) E2);
		    E2 = E2Prev = AllEList;
		}
		else {
		    E2Prev = E2;
		    E2 = E2 -> Pnext;
		}

		if (PT_APX_EQ_EPS(P -> V[0], P -> V[NumOfVertices - 1],
				  MARCH_CUBE_EPS)) {
		    P -> NumOfVertices = NumOfVertices;
		    P -> Pnext = AllPList;
		    AllPList = P;
		    ClosedPoly = TRUE;
		    break;
		}
	    }
	}
    }

    return AllPList;
}

/*****************************************************************************
* DESCRIPTION:                                                               *
* Drop zero length edges.						     *
*                                                                            *
* PARAMETERS:                                                                *
*   AllEList:  To clean up.                                                  *
*                                                                            *
* RETURN VALUE:                                                              *
*   void                                                                     *
*****************************************************************************/
static void MCPurgeZeroLenEdges(MCEdgeStruct **AllEList)
{
    MCEdgeStruct
	*E = *AllEList,
	*PrevE = E;

    while (E != NULL) {
	PointType Pt;

	PT_SUB(Pt, E -> V[0], E -> V[1]);

	if (PT_LENGTH(Pt) < MARCH_CUBE_EPS) {
	    if (E == *AllEList) {
		*AllEList = E -> Pnext;
		IritFree((VoidPtr) E);
		E = PrevE = *AllEList;
	    }
	    else {
		PrevE -> Pnext = E -> Pnext;
		IritFree((VoidPtr) E);
		E = PrevE -> Pnext;
	    }
	}
	else {
	    PrevE = E;
	    E = E -> Pnext;
	}
    }
}

/*****************************************************************************
* DESCRIPTION:                                                               *
* Computes the intersection of a cube edge with the scalar threshold level,  *
* given the edge two indices.						     *
*   Sets CCS _Intersect if found intersection.				     *
*   Updates the Pos parameter to the intersection position or set it to      *
* INFINITY if no Intersection is found.					     *
*   Returns the Index of the vertex with value ABOVE the Threshold level or  *
* MC_VRTX_NONE of no intersection.					     *
*                                                                            *
* PARAMETERS:                                                                *
*   CCS:           Cube to consider.					     *
*   Threshold:     Iso surface level.					     *
*   VrtxIndex1, VrtxIndex2:    Two vertices of intersecting edge.            *
*   Pos:           Where position is to be saved.                            *
*   Nrml:          Where Normal at position is to be saved.                  *
*                                                                            *
* RETURN VALUE:                                                              *
*   int:           Index of edge's vertex above Threshold, or MC_VRTX_NONE   *
*                  if no intersection.                                       *
*****************************************************************************/
static int MCComputeEdgeInter(MCCubeCornerScalarStruct *CCS,
			      RealType Threshold,
			      int VrtxIndex1,
			      int VrtxIndex2,
			      PointType Pos,
			      PointType Nrml)
{
    int i;
    RealType t,
	Val1 = CCS -> Corners[VrtxIndex1],
	Val2 = CCS -> Corners[VrtxIndex2],
	MaxVal = MAX(Val1, Val2),
	MinVal = MIN(Val1, Val2);

    if (MinVal >= Threshold || MaxVal < Threshold) {
	Pos[0] = Pos[1] = Pos[2] = INFINITY;
	return MC_VRTX_NONE;
    }

    t = (Val1 - Threshold) / (Val1 - Val2);

    for (i = 0; i < 3; i++)
	Pos[i] = CCS -> _VrtxPos[VrtxIndex2][i] * t +
	         CCS -> _VrtxPos[VrtxIndex1][i] * (1 - t);

    if (CCS -> HasGradient) {
	Nrml[0] = CCS -> GradientX[VrtxIndex2] * t +
		  CCS -> GradientX[VrtxIndex1] * (1 - t);
	Nrml[1] = CCS -> GradientY[VrtxIndex2] * t +
		  CCS -> GradientY[VrtxIndex1] * (1 - t);
	Nrml[2] = CCS -> GradientZ[VrtxIndex2] * t +
		  CCS -> GradientZ[VrtxIndex1] * (1 - t);
	PT_NORMALIZE(Nrml);
    }
    else {
	Nrml[0] = Nrml[1] = Nrml[2] = 0.0;
    }

    CCS -> _Intersect = TRUE;

    return Val1 > Val2 ? VrtxIndex1 : VrtxIndex2;
}

/*****************************************************************************
* DESCRIPTION:                                                               *
* Appends NewEdges to EdgeList						     *
*                                                                            *
* PARAMETERS:                                                                *
*   NewEdges:   Edges to append to EdgeList.                                 *
*   EdgeList:   Exists edge list to append NewEdges to.                      *
*                                                                            *
* RETURN VALUE:                                                              *
*   void                                                                     *
*****************************************************************************/
static void MCAppendEList(MCEdgeStruct *NewEdges, MCEdgeStruct **EdgeList)
{
    MCEdgeStruct
	*E = NewEdges;

    if (E != NULL) {
	while (E -> Pnext != NULL)
	    E = E -> Pnext;
	E -> Pnext = *EdgeList;
	*EdgeList = NewEdges;
    }
}

/*****************************************************************************
* DESCRIPTION:                                                               *
* Extract intersection edges from a given face (if any).		     *
*                                                                            *
* PARAMETERS:                                                                *
*   CCS:                     One voxel to consider.                          *
*   InterEdgeIndex1/2/3/4:   Four indices of face.                           *
*                                                                            *
* RETURN VALUE:                                                              *
*   MCEdgeStruct *:          List of edges of iso surface intersecting face. *
*****************************************************************************/
static MCEdgeStruct *MCGetEdgesFromFace(MCCubeCornerScalarStruct *CCS,
					int InterEdgeIndex1,
					int InterEdgeIndex2,
					int InterEdgeIndex3,
					int InterEdgeIndex4)
{
    CagdBType
	Inter1 = CCS -> _InterPos[InterEdgeIndex1][0] != INFINITY,
	Inter2 = CCS -> _InterPos[InterEdgeIndex2][0] != INFINITY,
	Inter3 = CCS -> _InterPos[InterEdgeIndex3][0] != INFINITY,
	Inter4 = CCS -> _InterPos[InterEdgeIndex4][0] != INFINITY;
    int i, j,
	NumOfInters = Inter1 + Inter2 + Inter3 + Inter4;

    /* We can have either two or four intersections. */
    if (NumOfInters == 0) {
	return NULL;
    }
    else if (NumOfInters == 2) {
	int NumOfVertices = 0;
	struct MCEdgeStruct
	    *Edge = (MCEdgeStruct *) IritMalloc(sizeof(MCEdgeStruct));

	/* Make an edge from one intersection to the next. */
	if (Inter1) {
	    PT_COPY(Edge -> V[NumOfVertices],
		    CCS -> _InterPos[InterEdgeIndex1]);
	    PT_COPY(Edge -> N[NumOfVertices],
		    CCS -> _InterNrml[InterEdgeIndex1]);
	    NumOfVertices++;
	}
	if (Inter2) {
	    PT_COPY(Edge -> V[NumOfVertices],
		    CCS -> _InterPos[InterEdgeIndex2]);
	    PT_COPY(Edge -> N[NumOfVertices],
		    CCS -> _InterNrml[InterEdgeIndex2]);
	    NumOfVertices++;
	}
	if (Inter3) {
	    PT_COPY(Edge -> V[NumOfVertices],
		    CCS -> _InterPos[InterEdgeIndex3]);
	    PT_COPY(Edge -> N[NumOfVertices],
		    CCS -> _InterNrml[InterEdgeIndex3]);
	    NumOfVertices++;
	}
	if (Inter4) {
	    PT_COPY(Edge -> V[NumOfVertices],
		    CCS -> _InterPos[InterEdgeIndex4]);
	    PT_COPY(Edge -> N[NumOfVertices],
		    CCS -> _InterNrml[InterEdgeIndex4]);
	    NumOfVertices++;
	}
	if (NumOfVertices != 2) {
	    TRIV_FATAL_ERROR(TRIV_ERR_TWO_INTERSECTIONS);
	    return NULL;
	}

	Edge -> Pnext = NULL;

	return Edge;
    }
    else if (NumOfInters == 4) {
	int Indirect[4];
	CagdBType Found;
	struct MCEdgeStruct
	    *Edge1 = (MCEdgeStruct *) IritMalloc(sizeof(MCEdgeStruct)),
	    *Edge2 = (MCEdgeStruct *) IritMalloc(sizeof(MCEdgeStruct));

	Indirect[0] = InterEdgeIndex1;
	Indirect[1] = InterEdgeIndex2;
	Indirect[2] = InterEdgeIndex3;
	Indirect[3] = InterEdgeIndex4;

	/* Use the _InterHighV To make a decision. */

	/* Find first pair. */
	for (i = 0, Found = FALSE; i < 3 && !Found; i++) {
	    for (j = i + 1; j < 4 && !Found; j++) {
		if (CCS -> _InterHighV[Indirect[i]] ==
		    CCS -> _InterHighV[Indirect[j]]) 
		    Found = TRUE;
	    }
	}
	if (Found) {
	    i--;
	    j--;
	}
	else {
	    TRIV_FATAL_ERROR(TRIV_ERR_NO_MATCH_PAIR);
	    return NULL;
	}

	PT_COPY(Edge1 -> V[0], CCS -> _InterPos[Indirect[i]]);
	PT_COPY(Edge1 -> N[0], CCS -> _InterNrml[Indirect[i]]);
	PT_COPY(Edge1 -> V[1], CCS -> _InterPos[Indirect[j]]);
	PT_COPY(Edge1 -> N[1], CCS -> _InterNrml[Indirect[j]]);
	Indirect[i] = MC_EDGE_NONE;
	Indirect[j] = MC_EDGE_NONE;

	/* Find second pair. */
	for (i = j = 0; i < 4; i++) {
	    if (Indirect[i] != MC_EDGE_NONE) {
		PT_COPY(Edge2 -> V[j], CCS -> _InterPos[Indirect[i]]);
		PT_COPY(Edge2 -> N[j], CCS -> _InterNrml[Indirect[i]]);
		j++;
	    }
	}

	Edge1 -> Pnext = Edge2;
	Edge2 -> Pnext = NULL;

	return Edge1;
    }
    else {
	TRIV_FATAL_ERROR(TRIV_ERR_2_OR_4_INTERS);
	return NULL;
    }
}
