/*
 * Copyright 2008 Nicolas Normand and Pierre Evenou.
 
 * This file is part of hLutChamfer3D.
 
 * hLutChamfer3D is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 
 * hLutChamfer3D is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 
 * You should have received a copy of the GNU Lesser General Public License
 * along with hLutChamfer3D.  If not, see <http://www.gnu.org/licenses/>.
 */
/*! \file hLutChamfer3D.c
 * \mainpage
 * hLutChamfer3D checks if a G-symmetrical chamfer mask induces a norm or not and,
 * for norms, computes the medial axis test neighborhood and lut values.
 *
 * \author Nicolas Normand and Pierre Evenou
 * \date 2007-2008
 *
 * \$Id: hLutChamfer3D.c,v 1.32 2008/10/31 06:12:30 cvs Exp $
 *
 * http://www.cb.uu.se/~tc18/
 *
 * \b Licence: This program is free software under the terms of the 
 * GNU Lesser General Public License (LGPL) version 3.
 *
 * \b Usage:
\verbatim
hLutChamfer3D	[-r|--maxradius max_radius]
		[-w|--weights a b c d...]
		[-m|--mask weighting_count x y w... x y w]
		[-o|output outputfile]

example: hLutChamfer2D -r 2000 -m 4 1 0 14 3 1 44 2 1 31 1 1 20
\endverbatim
 * <b>Return codes:</b>
 * - \c 0	success
 * - \c 255	not a norm
 *
 * <b>Additional output streams:</b>
 * - \c 3	elapsed time
 * - \c 4	list visited points
 * - \c 5	H-polytope matrix of chamfer balls (facet normal vectors)
 * - \c 6	H-parameters of chamfer balls
 *
 * \b Examples:
 * - compute the test neighborhood and lut values for the mask <4,6,7,9,10> for a
 *   maximal radius equal to 2000:
 *   \code hLutChamfer3D -r 2000 -w 4 6 7 9 10 -o -\endcode
 * - check if the mask <3,4,7> induces a norm (in this case, return code 255: not a
 *   norm)
 *   \code hLutChamfer3D -w 3 4 7\endcode
 * - print the facet normal vectors of the equivalent rational ball for the mask
 *   <7,8,11,14> on the standard output:
 *   \code hLutChamfer3D -w 7 8 11 14 5>&1\endcode
 *
 * This program is an electronic appendix to:
 * \verbatim
@article{normand2008pr,
	 Author = {Normand, Nicolas and Evenou, Pierre},
	 Journal = {Pattern Recognition},
	 Pages = {20 pages},
	 Title = {Medial Axis Lookup Table and Test Neighborhood Computation for 3{D} Chamfer Norms},
	 Note = {To appear},
	 Year = {2008}
}\endverbatim
 */

#include <limits.h>
#include <stdio.h>
#include <stdlib.h>
#include <strings.h>
#include <getopt.h>
#include <assert.h>
#include <sys/time.h>
#include <sys/resource.h>

#include <math.h>
#include "hLutChamfer3D.h"

static char const rcsid[] = "$Id: hLutChamfer3D.c,v 1.32 2008/10/31 06:12:30 cvs Exp $";

#ifndef DOXYGEN_SHOULD_SKIP_THIS
inline int max(int a, int b) {
    return a > b ? a : b;
}

inline int min(int a, int b) {
    return a < b ? a : b;
}
#endif

unsigned int gcd(int a, int b) {
    a = abs(a);
    b = abs(b);

    if (a > b) {
	// Swap a & b
	a += b;
	b = a - b;
	a -= b;
    }

    while (b != 0) {
        int t = a % b;
        a = b;
        b = t;
    }
    
    return a;
}

int decreasingOrder(const int* a, const int* b) {
    return *b - *a;
}

int combinations(int n, int k) {
    //assert(n >= k);
    assert(k >= 0);
    //printf("%d %d ", n, k);
    long long int i;
    int c = 1;
    for (i = n - k + 1; i <= n; i++)
	c *= i;
    for (i = 1; i <= k; i++)
	c /= i;
    //printf("%d\n", c);
    return c;
}

#pragma mark Chamfer Propagation Image

GeneratorImage* imageCreate(Mask *mask) {
    GeneratorImage* image;
    image = (GeneratorImage*) malloc(sizeof(GeneratorImage));
    
    if (image) {
	image->pixels = (int *) malloc(sizeof(int));
	assert(image->pixels);
	image->pixels[0] = 0;
	image->size = 0;
	image->allocatedSize = 1;
	image->mask = mask;
    }
    
    return image;
}

void imageFree(GeneratorImage* image) {
    free(image->pixels);
    free(image);
}

int imageGetPixelIndex(GeneratorImage* image, int p[]) {
    int index = 0;
    int i;
    
    for (i = 0; i < DIMENSION; i++) {
	index += combinations(p[DIMENSION - i - 1] + i, i + 1);
    }
    
    return index;
}

int imageGetPixel(GeneratorImage* image, int p[]) {
    int i, pm = 0;
    int index;
    
    for (i = 0; i < DIMENSION; i++) {
	pm = max(pm, p[i]);
    }
    
    if (pm > image->size) {
	int allocatedSize = combinations(pm + DIMENSION, DIMENSION);
	image->allocatedSize = allocatedSize;
	image->pixels = (int*) realloc(image->pixels, combinations(pm + DIMENSION, DIMENSION) * sizeof(int));
	assert(image->pixels);
	
	int x[DIMENSION];
	int y[DIMENSION];
	for (i = 1; i < DIMENSION; i++)
	    x[i] = 0;
	x[0] = image->size + 1;
	image->size = pm;
	
	while (x[0] <= pm) {
	    int neighbor;
	    int index1 = imageGetPixelIndex(image, x);
	    int dist = INT_MAX;
	    for (neighbor = 0; neighbor < image->mask->ng; neighbor++) {
		vectorCopy(y, x, DIMENSION);
		vectorSubtract(y, image->mask->vg[neighbor].p, DIMENSION);
		symPointInGenerator(y);
		int index2 = imageGetPixelIndex(image, y);
		if (index2 < index1) {
		    int d2 = image->pixels[index2] == INT_MAX ? INT_MAX : image->pixels[index2] + image->mask->vg[neighbor].w;
		    dist = min(dist, d2);
		}
	    }
	    assert(index1 < image->allocatedSize);
	    image->pixels[index1] = dist;
	    //vectorPrint(stdout, x, DIMENSION);
	    //printf(" %d\n", dist);
	    
	    int i0 = DIMENSION - 1;
	    while (i0 > 0 && x[i0] >= x[i0 - 1]) i0--;
	    x[i0]++;
	    for (i0++; i0 < DIMENSION; i0++) {
		x[i0] = 0;
	    }
	}
    }
    
    index = imageGetPixelIndex(image, p);
    assert(index < image->allocatedSize);
    return image->pixels[index];
}

/*!
 * \brief Transform the point to its symetrical one in the generator cone.
 *
 * The coordinates of p are first changed to their absolute values and then sorted
 * in decreasing order.
 *
 * \param[in,out]     p point.
 * \sa vectorIsInGenerator(Vector v, int dimension) 
 */
void symPointInGenerator(Vector p) {
    int d;
    
    for (d = 0; d < DIMENSION; d++) {
	p[d] = max(p[d], -p[d]);
    }
    qsort(p, DIMENSION, sizeof(int), (comparisonFunction)decreasingOrder);    
}

#pragma mark Matrix

Matrix* matrixCreate(int dimension) {
    Matrix* matrix;
    /* allocate empty matrix */
    matrix = malloc(2*sizeof(unsigned int));
    if (matrix) {
	matrix->height = 0;
	matrix->width = dimension;
    }
    return matrix;
}

Matrix* matrixAllocateRow(Matrix* old) {
    Matrix* new;
    old->height++;
    if((new = realloc(old, (2+old->width*old->height)*sizeof(unsigned int))) == NULL)
	free(old);
    return new;
}

Matrix* matrixAddRowElts(Matrix* matrix, Vector elts) {
    //int row;
    matrix = matrixAllocateRow(matrix);
    if (matrix) {
	vectorCopy((int*) &matrix->elts[(matrix->height - 1) * matrix->width], elts, matrix->width);
    }
    return matrix;
}

Matrix* matrixAddUniqueRowElts(Matrix* matrix, Vector elts) {
    int row;
    for (row = 0; row < matrix->height; row++) {
	if (memcmp(&matrix->elts[row * matrix->width], elts, matrix->width * sizeof(elts[0])) == 0)
	    return matrix;
    }
    return matrixAddRowElts(matrix, elts);
}

void matrixPrint(FILE*out, Matrix* m) {
    long col;
    long row;

    if (out == NULL)
	out = stdout;

    fprintf(out, "rows %d\n", m->height);
    fprintf(out, "cols %d\n", m->width);
    for (row = 0; row < m->width; row++) {
	for (col = 0; col < m->height; col++) {
	    fprintf(out, "%d\t", m->elts[col * m->width + row]);
	}
	fprintf(out, "\n");
    }
}

unsigned int distanceFromOrigin(Vector p, Matrix* matrix) {
    int d = 0;
    int hs, i;
    for (hs = 0; hs < matrix->height; hs++) {
	int dcone = 0;
	for (i = 0; i < DIMENSION; i++) {
	    dcone += p[i] * matrix->elts[hs * DIMENSION + i];
	}
	d = max(d, dcone);
    }
    return d;
}

#pragma mark H-parameters

void hCoeffCopy(int* dst, int* src, int size) {
    int hs;
#ifndef NDEBUG
    int max = src[0];
#endif
    
    for (hs = 0; hs < size; hs++) {
	dst[hs] = src[hs];
#ifndef NDEBUG
	if (max < src[hs]) {
	    max = src[hs];
	}
#endif
    }
    dst[size] = src[size];
    assert(src[size] == max);
}

void hCoeffAdd(int* dst, int* src, int size) {
    int hs;
    int max = 0;
#ifndef NDEBUG
    int smax = src[0];
    int dmax = dst[0];
#endif
    
    for (hs = 0; hs < size; hs++) {
#ifndef NDEBUG
	if (smax < src[hs]) {
	    smax = src[hs];
	}
	if (dmax < dst[hs]) {
	    dmax = dst[hs];
	}
#endif
	dst[hs] += src[hs];
	if (max < dst[hs])
	    max = dst[hs];
    }
    assert(dst[size] == dmax);
    dst[size] = max;
    assert(src[size] == smax);
}

void hCoeffSubtract(int* dst, int* src, int size) {
    int hs;
    int max = 0;
#ifndef NDEBUG
    int smax = src[0];
    int dmax = dst[0];
#endif
    
    for (hs = 0; hs < size; hs++) {
#ifndef NDEBUG
	if (smax < src[hs]) {
	    smax = src[hs];
	}
	if (dmax < dst[hs]) {
	    dmax = dst[hs];
	}
#endif
	dst[hs] -= src[hs];
	if (max < dst[hs])
	    max = dst[hs];
    }
    assert(dst[size] == dmax);
    dst[size] = max;
    assert(src[size] == smax);
}

#pragma mark Weighting

void weightingPrint(FILE* out, Weighting* weighting, int dimension) {
    int d;

    if (out == NULL)
	out = stdout;

    fprintf(out, "(");
    for (d = 0; d < dimension; d++) {
	fprintf(out, "%2d, ", weighting->p[d]);
    }
    fprintf(out, "%3d)", weighting->w);
}

void weightingCopy(Weighting* dst, Weighting* src, int dimension) {
    int d;

    for (d = 0; d < dimension; d++) {
	dst->p[d] = src->p[d];
    }
    dst->w = src->w;
}

#pragma mark Vector

void vectorCopy(Vector dst, Vector src, int dimension) {
    int d;
    
    for (d = 0; d < dimension; d++) {
	dst[d] = src[d];
    }
}

void vectorAdd(Vector dst, Vector src, int dimension) {
    int d;
    
    for (d = 0; d < dimension; d++) {
	dst[d] += src[d];
    }
}

void vectorSubtract(Vector dst, Vector src, int dimension) {
    int d;
    
    for (d = 0; d < dimension; d++) {
	dst[d] -= src[d];
    }
}

int vectorIsConstant(Vector vector, int dimension) {
    int d;
    
    for (d = 0; d < dimension; d++) {
	if (vector[d] != vector[dimension])
	    return 0;
    }

    return 1;
}

int vectorScalarProduct(Vector v1, Vector v2, int dimension) {
    int i, sp = 0;
    for (i = 0; i < dimension; i++) {
	sp += v1[i] * v2[i];
    }
    return sp;
}

/*!
 * Checks if the vector <em>p</em> belongs to the generator cone (positive
 * decreasing components).
 *
 * \sa symPointInGenerator(Vector p)
 */
int vectorIsInGenerator(Vector v, int dimension) {
    int i;
    for (i = 0; i < dimension - 1; i++) {
	if (v[i] < v[i+1])
	    return 0;
    }
    return v[DIMENSION - 1] >= 0;
}

int vectorIsEqual(Vector v1, Vector v2, int dimension) {
    int i;
    for (i = 0; i < dimension; i++)
	if (v1[i] != v2[i])
	    return 0;
    return 1;
}

int vectorIsVisible(Vector v, int dimension) {
    int i;
    int g = 0;

    for (i = 0; i < dimension; i++)
	g = gcd(v[i], g);

    return g == 1;
}

void vectorPrint(FILE* out, Vector vector, int dimension) {
    int d;

    if (out == NULL)
	out = stdout;

    fprintf(out, "(");
    for (d = 0; d < dimension - 1; d++) {
	fprintf(out, "%2d, ", vector[d]);
    }
    fprintf(out, "%2d)", vector[dimension - 1]);
}

#pragma mark Mask

Mask* maskCreate(int weightingCount, int dimension) {
    Mask* mask;
    mask = (Mask *)malloc(sizeof(Mask));
    if (mask) {
	mask->ng = 0;
    }
    return mask;
}

void maskDestroy(Mask* mask) {
    free(mask);
}

/*!
 * \brief Adds a weighting to a mask.
 */
void maskAddWeighting(Mask* mask, Vector vector, int weight, int dimension) {
    int i;

    assert(weight > 0);
    assert(vector[0] > 0);
    assert(vectorIsInGenerator(vector, dimension));
    
    for (i = 0; i < mask->ng; i++)
	if (vectorIsEqual(vector, mask->vg[i].p, dimension)) {
	    mask->vg[i].w = min(mask->vg[i].w, weight);
	    return;
	}
    assert(mask->ng < MAX_WEIGHT_COUNT - 1);
    for (i = 0; i < dimension - 1; i++) {
	assert(vector[i+1] <= vector[i]);
    }
    for (i = 0; i < dimension; i++) {
	mask->vg[mask->ng].p[i] = vector[i];
    }
    mask->vg[mask->ng].w = weight;
    mask->ng++;
}

int maskWeightingCount(Mask* mask) {
    return mask->ng;
}

void maskPrint(FILE* out, Mask* mask, int displayOrder[MAX_WEIGHT_COUNT], int apparitionRadius[MAX_WEIGHT_COUNT], int innerRadius[MAX_WEIGHT_COUNT]) {
    int i;
    if (out == NULL)
	out = stdout;

    for (i = 0; i < mask->ng; i++) {
	int index = displayOrder == NULL ? i : displayOrder[i];
	weightingPrint(out, &mask->vg[index], DIMENSION);
	if (apparitionRadius != NULL)
	    fprintf(out, " added for R = %d", apparitionRadius[index]);
	if (innerRadius != NULL)
	    fprintf(out, " (r = %d)", innerRadius[index]);
	fprintf(out, "\n");
    }
}

#pragma mark HPolytopeTable

void hPolytopeTablePrint(FILE* out, HPolytopeTable* hPolytopeTable) {
    int cone;
    //int dim;
    int radius;
    fprintf(out, "HPolytopeTable : \n");
    fprintf(out, "halfspaceCount : %d\n", hPolytopeTable->halfspaceCount);
    fprintf(out, "polytopeCount : %d\n", hPolytopeTable->polytopeCount);
    fprintf(out, "coeff :\n");
    for(radius = 0; radius < hPolytopeTable->polytopeCount; radius++) {
	fprintf(out, "%d ", radius);
	for (cone = 0; cone < hPolytopeTable->halfspaceCount; cone++) {
	    fprintf(out, "%d ", hPolytopeTable->coeff [radius][cone]);
	}
	fprintf(out, "\n");
    }
}

/*!
 * Computes the radius of the smallest disk centered at the origin \f$O\f$
 * that contains the disk \f$B(c,r)\f$ or equivalently
 * the radius of the smallest disk centered at \f$c\f$ that contains
 * \f$B(O,r)\f$ (value of the covering function of \f$B(O,r)\f$ at point \f$c\f$).
 *
 * \param[in]	  hPolytopeTable The \f$\hat{\mathcal{H}}\f$-description of the
 *		  family of chamfer balls.
 * \param[in]	  c The center \f$c\f$ of the outer disk.
 * \param[in]	  r The radius \f$r\f$ of the inner disk;
 *
 * \returns The radius of minimal ball centered in \f$c\f$ covering \f$B(O,r)\f$.
 */
int coveringBallRadius(HPolytopeTable* hPolytopeTable, Vector c, int r) {
    unsigned int cone;
    unsigned int d;
    unsigned int distMaxInDisk;
    unsigned int distInCone;

    // Get an equivalent displacement in the first octant 0 <= y <= x
    for (d = 0; d < DIMENSION - 1; d++) {
	assert(c[d] >= c[d+1]);
    }
    assert(c[DIMENSION - 1] >= 0);
    /*
    c[1] = max(c[1], -c[1]);
    if (c[0] < c[1]) {
	int t = c[1];
	c[1] = c[0];
	c[0] = t;
    }*/
    
    distMaxInDisk = 0;
    for (cone = 0; cone < hPolytopeTable->halfspaceCount; cone++) {
#ifndef NDEBUG
	assert(r < hPolytopeTable->polytopeCount);
#endif
	distInCone = hPolytopeTable->coeff[r][cone];
	for (d = 0; d < DIMENSION; d++) {
	    distInCone += c[d] * hPolytopeTable->matrix->elts[cone * DIMENSION + d];
	}
	distMaxInDisk = max(distMaxInDisk, distInCone);
    }
    
    return distMaxInDisk;
}

/*!
 * \brief Computes a column of the LUT.
 *
 * Computes a column of the LUT for the neighbor <em>neighbor</em> in the test neighborhood <em>mask</em>.
 * 
 * \param[in]	  mask Test neighborhood
 * \param[in]	  hPolytopeTable The \f$\hat{\mathcal{H}}\f$-description of the
 *		  family of chamfer balls.
 * \param[in,out] lut preallocated look-up-table
 * \param[in]	  radiusMax
 * \param[in]	  neighbor index of the neighbor in mask
 * \todo remove the need for neighbor argument by passing a vector and a lut column
 */
void computeLutCol(Mask* mask, HPolytopeTable* hPolytopeTable, LookUpTable lut, int radiusMax, int neighbor) {
    unsigned int cone, d;
    unsigned int radius;
    unsigned int hPolytopeTableInc[MAX_CONE_COUNT];
    unsigned int distMaxInDisk;

    // First compute H-description coefficients increments for this neighbor
    for (cone = 0; cone < hPolytopeTable->halfspaceCount; cone++) {
	hPolytopeTableInc[cone] = 0;
	for (d = 0; d < DIMENSION; d++) {
	    hPolytopeTableInc[cone] +=
		mask->vg[neighbor].p[d] * hPolytopeTable->matrix->elts[cone * DIMENSION + d];
	}
    }

    for (radius = 0; radius <= radiusMax; radius++) {
	distMaxInDisk = 0;
	for (cone = 0; cone < hPolytopeTable->halfspaceCount; cone++) {
#ifndef NDEBUG
	    assert(radius < hPolytopeTable->polytopeCount);
#endif
	    distMaxInDisk = max(distMaxInDisk, hPolytopeTable->coeff[radius][cone] + hPolytopeTableInc[cone]);
	}
#ifndef DOXYGEN_SHOULD_SKIP_THIS
#ifndef DNDEBUG
	assert(distMaxInDisk == coveringBallRadius(hPolytopeTable, mask->vg[neighbor].p, radius));
#endif
#endif
	lut[neighbor][radius] = distMaxInDisk + 1;
    }
}

/*!
 * Tests if the ball \f$B(p,r_o)\f$ is indirectly covering \f$B(0,r_i)\f$,
 * <em>i.e.</em> if there is \f$B(O+\vec{v},r_m)\f$ covering \f$B(0,r_i)\f$ and
 * covered by \f$B(p,r_o)\f$.
 * For each known vector \f$\vec{v}\f$,
 * we compute \f$r_m\f$ the radius of the smallest ball of center \f$O+\vec{v}\f$
 *   covering \f$B(0,r_i)\f$ and we check if it is contained in \f$B(p,r_o)\f$.
 *
 * \param[in]	  hPolytopeTable The \f$\hat{\mathcal{H}}\f$-description of the
 *		  family of chamfer balls.
 * \param[in]	  weightings A set of known weightings.
 * \param[in]	  weightingCount The number of weightings to check.
 * \param[in]	  innerRadius The radius \f$r_i\f$ of the inner ball.
 * \param[in]	  p The center \f$p\f$ of the outer covering ball.
 * \param[in]	  outerRadius The radius \f$r_o\f$ of the outer covering ball.
 *
 * \return        A boolean.
 */
int isIndirectCoveringBall(HPolytopeTable* hPolytopeTable,
			   Weighting* weightings,
			   int weightingCount,
			   int innerRadius,
			   Vector p, 
			   int outerRadius) {
    int neighbor, d;
    
    // For each neighbor v in lutMask, check if the bounding disk centered in v
    // is included in this disk.
    for (neighbor = 0; neighbor < weightingCount; neighbor++) {
	int neighborRadius = coveringBallRadius(hPolytopeTable, weightings[neighbor].p, innerRadius);
	if (neighborRadius >= hPolytopeTable->polytopeCount)
	    return 1;

	Vector translated;
	for (d = 0; d < DIMENSION; d++) {
	    translated[d] = p[d] - weightings[neighbor].p[d];
	}
	symPointInGenerator(translated);

	if (outerRadius >= coveringBallRadius(hPolytopeTable, translated, neighborRadius))
	    return 1;
    }
    return 0;
}

/*!
 * Visits a point and adds it to the test neighborhood if needed.
 *
 * \param[in]	  point The point to check.
 * \param[in]	  hBall H-parameters of the inner ball translated by <em>point</em>.
 * \param[in,out] lutMask Test neighborhood.
 * \param[in]	  hPolytopeTable The \f$\hat{\mathcal{H}}\f$-description of the family of chamfer balls.
 * \param	  apparitionRadius
 * \param[in]	  radius The radius \f$r\f$ of the inner disk.
 */
void visitPoint(Vector point,
		int hBall[MAX_HALFSPACE_COUNT],
		Mask* lutMask,
		HPolytopeTable* hPolytopeTable,
		int apparitionRadius[],
		int radius) {
#ifndef NDEBUG
    Vector origPoint;

    vectorCopy(origPoint, point, DIMENSION);
#endif
    
    static int inited = 0;
    static FILE* pointout = 0;
    if (!inited) {
	inited = 1;
	pointout = fdopen(VISITED_POINTS_FILENO, "w");
    }
    static int previousRadius = -1;
    if (pointout) {
	if (radius != previousRadius) {
	    fprintf(pointout, "Radius: %d\n", radius);
	    previousRadius = radius;
	}
	fprintf(pointout, "Visited point:");
	vectorPrint(pointout, point, DIMENSION);
	fprintf(pointout, "\n");
    }
    
    int hs;
    int r22;
    r22 = 0;
    for (hs = 0; hs < hPolytopeTable->halfspaceCount; hs++) {
	r22 = max(r22, hBall[hs]);
    }
    assert(r22 == hBall[hPolytopeTable->halfspaceCount]);
    // Check if this point must be added to the lut mask
    if (r22 >= hPolytopeTable->polytopeCount)
	return;
    if (!isIndirectCoveringBall(hPolytopeTable,
				lutMask->vg,
				lutMask->ng,
				radius,
				point, 
				r22)) {
	apparitionRadius[lutMask->ng] = r22;
	//int savedWeight = w->w;
	//w->w = distanceFromOrigin(w->p, hPolytopeTable->matrix);
	maskAddWeighting(lutMask, point, distanceFromOrigin(point, hPolytopeTable->matrix), DIMENSION);
	//w->w = savedWeight;
    }
#ifndef NDEBUG
    int d;
    for (d = 0; d < DIMENSION; d++)
	assert(origPoint[d] == point[d]);
#endif
}

#pragma mark Cone

Cone* coneCreate(Mask* chamferMask, HPolytopeTable* hPolytopeTable, FILE* out) {
    int d, dd;
    int hs;
    int* hCoeffIncrement;
    int* vectors;
    Cone* cone;
    
    cone = (Cone*) malloc(sizeof(Cone));
    assert(cone);
    
    cone->next = (int *) malloc(DIMENSION * sizeof(int));
    hCoeffIncrement = (int *) malloc(DIMENSION * MAX_HALFSPACE_COUNT * sizeof(int));
    cone->hIncrements = (int **) malloc(DIMENSION * sizeof(int *));
    vectors = (int *) malloc(DIMENSION * DIMENSION * sizeof(int));
    cone->coneDirs = (int **) malloc(DIMENSION * sizeof(int *));
    assert(cone->next);
    assert(hCoeffIncrement);
    assert(cone->hIncrements);
    assert(vectors);
    assert(cone->coneDirs);
    
    for (d = 0; d < DIMENSION; d++) {
	cone->hIncrements[d] = hCoeffIncrement + d * MAX_HALFSPACE_COUNT;
	cone->coneDirs[d] = vectors + d * DIMENSION;
	for (dd = 0; dd < DIMENSION; dd++) {
	    cone->coneDirs[d][dd] = dd <= d;
	}
	
	int max = 0;
	for (hs = 0; hs < hPolytopeTable->halfspaceCount; hs++) {
	    if (d > 0)
		cone->hIncrements[d][hs] = cone->hIncrements[d - 1][hs];
	    else
		cone->hIncrements[d][hs] = 0;
	    cone->hIncrements[d][hs] += hPolytopeTable->matrix->elts[hs * DIMENSION + d];
	    if (max < cone->hIncrements[d][hs])
		max = cone->hIncrements[d][hs];
	}
	cone->hIncrements[d][hPolytopeTable->halfspaceCount] = max;
    }
    
    for (d = 0; d < DIMENSION - 1; d++)
	cone->next[d] = d + 1;
    cone->next[DIMENSION - 1] = 0;
    cone->last = DIMENSION - 1;

    cone->out = out;
    return cone;
}

void coneDestroy(Cone* cone) {
    free(cone->hIncrements[0]);
    free(cone->hIncrements);
    free(cone->next);
    free(cone->coneDirs[0]);
    free(cone->coneDirs);
    free(cone);
}

void conePrint(FILE* out, Cone* cone, int dimension, int hs) {
    int vector;

    if (out == NULL)
	out = stdout;

    vector = cone->next[cone->last];
    do {
	vectorPrint(out, cone->coneDirs[vector], dimension);
	fprintf(out, " ");
	vectorPrint(out, cone->hIncrements[vector], hs);
	fprintf(out, "\n");

	vector = cone->next[vector];
    } while (vector != cone->next[cone->last]);
}

/*!
 * \brief Checks if a simplicial cone is a covering cone.
 *
 * \param[in]	  coneDimension The dimension of the cone.
 * \param[in]	  hBall \f$\hat{\mathcal{H}}\f$-parameters of the inner ball
		  translated to the center of the cone.
 * \param[in]	  cone the cone to test.
 * \param[in]	  hsCount The number of polytope facets.
 * \todo ConeDimension redundant with cone->dimension?
 * \todo Change the order of parameters.
 */
int isCoveringSimplicialCone(int coneDimension,
			     int hBall[MAX_HALFSPACE_COUNT],
			     SimplicialCone* cone,
			     int hsCount) {
    int hs;
    int vector;
    int ok = 0;

    // Check if the same component is maximal for all vectors
    for (hs = 0; !ok && hs < hsCount; hs++) {
	if (hBall[hs] == hBall[hsCount]) {
	    ok = 1;
	    if (cone->dimension == 1)
		ok = cone->vectorSumHParams[hs] == cone->vectorSumHParams[hsCount];
	    else for (vector = 0; ok && vector < coneDimension; vector++) {
		SimplicialCone *cone1D = cone->faces[1][vector];
		ok = cone1D->vectorSumHParams[hs] == cone1D->vectorSumHParams[hsCount];
	    }
	}
    }
    return ok;
}

void visitCone(Vector coneVertex,
	       int hBall[MAX_HALFSPACE_COUNT],
	       SimplicialCone* cone,
	       Mask* lutMask,
	       HPolytopeTable* hPolytopeTable,
	       int apparitionRadius[],
	       int radius,
	       int visitVertex) {
#ifndef NDEBUG
    int orig[MAX_HALFSPACE_COUNT];
    Vector origLoc;
    
    vectorCopy(origLoc, coneVertex, DIMENSION);
    hCoeffCopy(orig, hBall, hPolytopeTable->halfspaceCount);
#endif

    // Visit the vertex of the cone
    visitPoint(coneVertex,
	       hBall,
	       lutMask,
	       hPolytopeTable,
	       apparitionRadius,
	       radius);

    if (!isCoveringSimplicialCone(cone->dimension, hBall, cone, hPolytopeTable->halfspaceCount)) {
	// Partition the cone
	int dim;
	for (dim = 1; dim < cone->dimension; dim++) {
	    int faceCount = combinations(cone->dimension, dim);
	    int face;
	    for (face = 0; face < faceCount; face++) {
		vectorAdd(coneVertex, cone->faces[dim][face]->vectorSum, DIMENSION);
		hCoeffAdd(hBall, cone->faces[dim][face]->vectorSumHParams, hPolytopeTable->halfspaceCount);

		visitCone(coneVertex,
			  hBall,
			  cone->faces[dim][face],
			  lutMask,
			  hPolytopeTable,
			  apparitionRadius,
			  radius,
			  0);

		vectorSubtract(coneVertex, cone->faces[dim][face]->vectorSum, DIMENSION);
		hCoeffSubtract(hBall, cone->faces[dim][face]->vectorSumHParams, hPolytopeTable->halfspaceCount);
	    }
	}
    }

#ifndef NDEBUG
    int hs;
    for (hs = 0; hs < hPolytopeTable->halfspaceCount; hs++) {
	assert(orig[hs] == hBall[hs]);
    }
    int d;
    for (d = 0; d < DIMENSION; d++) {
	assert(origLoc[d] == coneVertex[d]);
    }
#endif
}

/*!
 * This function computes the test neighborhood and the lut values.
 *
 * \param[in]	  mask The chamfer mask.
 * \param[in]	  decomposition A unimodular triangulation of the equivalent
		  rational ball (restricted to the generator cone).
 * \param[out]	  lutMask The test neighborhood to compute.
 * \param[in]	  hPolytopeTable The \f$\hat{\mathcal{H}}\f$-description of the
 *		  family of chamfer balls.
 * \param[out]	  lut The lookup tables to compute.
 * \param[in]	  radiusMax The maximal radius.
 * \param	  apparitionRadius
 * \param	  innerRadius
 */
void computeLutMaskAndCols(Mask* mask,
			   SimplicialConeDecomposition* decomposition,
			   Mask* lutMask,
			   HPolytopeTable* hPolytopeTable,
			   LookUpTable lut, int radiusMax,
			   int apparitionRadius[MAX_WEIGHT_COUNT],
			   int innerRadius[MAX_WEIGHT_COUNT]) {

    unsigned int validatedWeightingCount;
    unsigned int radius;
    //unsigned int d;
    int dim, i;
    unsigned int neighbor;

    validatedWeightingCount = 0;
    lutMask->ng = 0;
    
    Vector origin;
    bzero(&origin, sizeof(origin));
    
    for (radius = 0; radius <= radiusMax; radius++) {
	if (hPolytopeTable->coeff[radius][hPolytopeTable->halfspaceCount] != radius)
	    continue;
	
	int hTrans[MAX_HALFSPACE_COUNT];
	hCoeffCopy(hTrans, hPolytopeTable->coeff[radius], hPolytopeTable->halfspaceCount);
	
	for (dim = 1; dim <= DIMENSION; dim++)
	    for (i = 0; i < decomposition->coneCount[dim]; i++) {
		vectorAdd(origin, decomposition->conesPtr[dim][i]->vectorSum, DIMENSION);
		hCoeffAdd(hTrans, decomposition->conesPtr[dim][i]->vectorSumHParams, hPolytopeTable->halfspaceCount);

		visitCone(origin,
			  hTrans,
			  decomposition->conesPtr[dim][i],
			    
			  lutMask,
			  hPolytopeTable,
			  apparitionRadius,
			  radius,
			  dim == 1);

		vectorSubtract(origin, decomposition->conesPtr[dim][i]->vectorSum, DIMENSION);
		hCoeffSubtract(hTrans, decomposition->conesPtr[dim][i]->vectorSumHParams, hPolytopeTable->halfspaceCount);
	    }

	// Due to the algorithm processing order we may have included
	// useless points in our lut mask which need to be removed.
	int checkedWeightingCount = validatedWeightingCount;
	while (checkedWeightingCount < lutMask->ng) {
	    if (!isIndirectCoveringBall(hPolytopeTable,
					lutMask->vg + checkedWeightingCount + 1,
					lutMask->ng - checkedWeightingCount - 1,
					radius,
					lutMask->vg[checkedWeightingCount].p,
					apparitionRadius[checkedWeightingCount])) {
		/*
		weightingPrint(stdout, &lutMask->vg[checkedWeightingCount], DIMENSION);
		printf(" added for R = %d (r = %d)\n",
		       apparitionRadius[checkedWeightingCount],
		       radius);
		 */
		innerRadius[checkedWeightingCount] = radius;

		weightingCopy(lutMask->vg + validatedWeightingCount, lutMask->vg + checkedWeightingCount, DIMENSION);
		apparitionRadius[validatedWeightingCount] = apparitionRadius[checkedWeightingCount];

		validatedWeightingCount++;
		checkedWeightingCount++;
	    }
	    else {
		checkedWeightingCount++;
	    }
	}
	lutMask->ng=validatedWeightingCount;
    } // for radius
    // Compute lut columuns
    for (neighbor = 0; neighbor < lutMask->ng; neighbor++) {
	computeLutCol(lutMask, hPolytopeTable, lut, radiusMax, neighbor);
    }
}

/*!
 * Print the LookUpTable
 */
void printLut(FILE* f, Mask* mask, HPolytopeTable* hPolytopeTable, LookUpTable lut, int radiusMax, int displayOrder[]) {
    unsigned int radius;
    unsigned int realRadius = 0;
    unsigned int cone;
    unsigned int neighbor;
    unsigned int needPrint;
    
    for (radius = 0; radius < radiusMax; radius++) {
	realRadius = 0;
	for (cone = 0; cone < hPolytopeTable->halfspaceCount; cone++) {
#ifndef NDEBUG
	    assert(radius + 1 < hPolytopeTable->polytopeCount);
#endif
	    realRadius = max(realRadius, hPolytopeTable->coeff[radius+1][cone]);
	}
	
	needPrint = 0;
	for (neighbor = 0; neighbor < mask->ng; neighbor++) {
	    needPrint |= lut[displayOrder[neighbor]][radius] - 1 < radius + mask->vg[displayOrder[neighbor]].w;
	}

	if (needPrint && radius + 1 == realRadius) {
	    fprintf(f, "Lut[][%5d ] = ", realRadius);
	    
	    for (neighbor = 0; neighbor < mask->ng; neighbor++) {
		if (lut[displayOrder[neighbor]][radius] - 1 < radius + mask->vg[displayOrder[neighbor]].w)
		    fprintf(f, "%5d ", lut[displayOrder[neighbor]][radius]);
		else
		    fprintf (f, "      ");
	    }
	    fprintf(f, "\n");
	}
    }
    fprintf(f, "\n");
}

#pragma mark Simplicial Cone

void simplicialConeAllocFaces(SimplicialCone* cone, int dimension) {
    int count = 0;
    int i;

    for (i = 1; i < dimension; i++) {
	count += combinations(dimension, i);
    }
    SimplicialCone** theFaces = (SimplicialCone**) malloc(count * sizeof(SimplicialCone*));
    bzero(theFaces, count * sizeof(SimplicialCone*));
    cone->faces = (SimplicialCone***) malloc(dimension * sizeof(SimplicialCone**));
    bzero(cone->faces, dimension * sizeof(SimplicialCone**));
    cone->faces[0] = theFaces;
    count = 0;
    for (i = 1; i < dimension; i++) {
	cone->faces[i] = theFaces + count;
	count += combinations(dimension, i);
    }
}

void simplicialConeInit(SimplicialCone* cone, int dimension) {
    bzero(cone, sizeof(SimplicialCone));
    cone->dimension = dimension;
    simplicialConeAllocFaces(cone, dimension);
}

void simplicialConeAddFace(SimplicialCone* cone, SimplicialCone* face) {
    int dim;
    int i, j;
    assert(cone->dimension > face->dimension);
    for (i = 0; cone->faces[face->dimension][i] != NULL; i++) {
	assert(cone->faces[face->dimension][i] != face);
    }
    assert(i < combinations(cone->dimension, face->dimension));
    cone->faces[face->dimension][i] = face;
    if (face->dimension == 1) {
	vectorAdd(cone->vectorSum, face->vectorSum, DIMENSION);
    }

    for (dim = 1; dim < face->dimension; dim++) {
	int ii = combinations(cone->dimension, dim);
	int jj = combinations(face->dimension, dim);

	// For each dim-subface of face...
	for (j = 0; j < jj && face->faces[dim][j] != NULL; j++) {
	    for (i = 0;
		 i < ii
		 && cone->faces[dim][i] != NULL
		 && cone->faces[dim][i] != face->faces[dim][j];) i++;

	    if (i < ii && cone->faces[dim][i] == NULL) {
		//assert(i < combinations(cone->dimension, dim));
		cone->faces[dim][i] = face->faces[dim][j];
		if (dim == 1) {
		    vectorAdd(cone->vectorSum, face->faces[dim][j]->vectorSum, DIMENSION);
		}
	    }
	}
    }
}

void simplicialConeCheckFacesInited(SimplicialCone* cone, HPolytopeTable* hPolytopeTable) {
    Vector sum;
    int max = 0;
    int hParams[MAX_HALFSPACE_COUNT+1];

    int dim, i;

    for (dim = 1; dim < cone->dimension - 1; dim++) {
	assert(cone->faces[combinations(cone->dimension, dim)-1] != NULL);
    }

    for (i = 0; i < hPolytopeTable->halfspaceCount; i++) {
	sum[i] = 0;
	hParams[i] = 0;
    }
    if (cone->dimension == 1)
	return;
    for (i = 0; i < cone->dimension; i++) {
	vectorAdd(sum, cone->faces[1][i]->vectorSum, DIMENSION);
	vectorAdd(hParams, cone->faces[1][i]->vectorSumHParams, hPolytopeTable->halfspaceCount);
    }
    for (i = 0; i < hPolytopeTable->halfspaceCount; i++) {
	assert(sum[i] == cone->vectorSum[i]);
	assert(hParams[i] == cone->vectorSumHParams[i]);
	if (max < hParams[i])
	    max = hParams[i];
    }
    assert(cone->vectorSumHParams[hPolytopeTable->halfspaceCount] == max);
}

void simplicialConeComputeHIncrements(SimplicialCone* cone, HPolytopeTable* hPolytopeTable) {
    int hs, d;
    int max = 0;
    for (hs = 0; hs < hPolytopeTable->halfspaceCount; hs++) {
	cone->vectorSumHParams[hs] = 0;
	for (d = 0; d < DIMENSION; d++)
	    cone->vectorSumHParams[hs] +=
	    cone->vectorSum[d] *
	    hPolytopeTable->matrix->elts[hs * hPolytopeTable->matrix->width + d];
	if (max < cone->vectorSumHParams[hs])
	    max = cone->vectorSumHParams[hs];
    }
    cone->vectorSumHParams[hPolytopeTable->halfspaceCount] = max;
}

void simplicialConePrint(FILE *out, SimplicialCone* cone) {
    int i;

    if (out == NULL)
	out = stdout;

    vectorPrint(out, cone->vectorSum, DIMENSION);
    if (cone->dimension == 1)
	fprintf(out, "\n");
    else {
	fprintf(out, "{\n");
	for (i = 0; i < cone->dimension && cone->faces[cone->dimension-1][i] != NULL; i++) {
	    simplicialConePrint(out, cone->faces[cone->dimension-1][i]);
	}
	fprintf(out, "}\n");
    }
    
}

SimplicialConeDecomposition* simplicialConeDecompositionCreate() {
    int dim;
    SimplicialConeDecomposition* decomposition = (SimplicialConeDecomposition*) malloc(sizeof(SimplicialConeDecomposition));
    assert(decomposition);
    for (dim = 0; dim < DIMENSION + 1; dim++) {
	decomposition->coneCount[dim] = 0;
	decomposition->conesPtr[dim] = (SimplicialCone**) malloc(0);
	assert(decomposition->conesPtr[dim]);
    }
    return decomposition;
}

SimplicialCone* simplicialConeDecompositionFindCone(SimplicialConeDecomposition* decomposition, int* combi, int dimension) {
    int i, j;
    SimplicialCone* cone;
    for (i = 0; i < decomposition->coneCount[dimension]; i++) {
	int equal = 1;
	cone = decomposition->conesPtr[dimension][i];
	for (j = 0; equal && j < dimension; j++) {
	    equal = combi[j] == cone->vectorIndex[j];
	}
	if (equal)
	    return cone;
    }
    return NULL;
}

void simplicialConeDecompositionPrint(FILE* out, SimplicialConeDecomposition* decomposition) {
    int dim, i;

    if (out == NULL)
	out = stdout;

    for (dim = 1; dim < DIMENSION + 1; dim++) {
	for (i = 0; i < decomposition->coneCount[dim]; i++) {
	    vectorPrint(out, decomposition->conesPtr[dim][i]->vectorIndex, dim);
	    fprintf(out, ":");

	    vectorPrint(out, decomposition->conesPtr[dim][i]->vectorSum, DIMENSION);
	    fprintf(out, "\n");
	}
    }
}

SimplicialCone* simplicialConeDecompositionAddCone(SimplicialConeDecomposition* decomposition, Mask* mask, int* combi, int dimension) {
    int i, j;
    SimplicialCone* cone;
    SimplicialCone* face;

    cone = simplicialConeDecompositionFindCone(decomposition, combi, dimension);
    if (cone)
	return cone;

    //printf("Creating cone: ");
    //printCombi(combi, dimension);
    //printf("\n");
    
    decomposition->conesPtr[dimension] = (SimplicialCone**) realloc(decomposition->conesPtr[dimension], (decomposition->coneCount[dimension] + 1) * sizeof (SimplicialCone*));
    assert(decomposition->conesPtr[dimension]);
    cone = (SimplicialCone*) malloc(sizeof(SimplicialCone));
    decomposition->conesPtr[dimension][decomposition->coneCount[dimension]] = cone;
    decomposition->coneCount[dimension]++;
    simplicialConeInit(cone, dimension);

    vectorCopy(cone->vectorIndex, combi, dimension);

    if (dimension == 1) {
	vectorCopy(cone->vectorSum, mask->vg[combi[0]].p, DIMENSION);
	//simplicialConeComputeHIncrements(cones, hPolytopeTable);
	return cone;
    }

    // For each combination of (dimension - 1) vectors among (dimension), find the
    // cone or create it, then add it to the faces of this cone
    int scombi[DIMENSION];
    //vectorCopy(scombi, combi, dimension);
    for (i = 0; i < dimension; i++) {
	for (j = 0; j < dimension - 1; j++) {
	    scombi[j] = combi[j + (j >= i)];
	}
	face = simplicialConeDecompositionAddCone(decomposition, mask, scombi, dimension - 1);
	simplicialConeAddFace(cone, face);
    }

    return cone;
}

void initSimplicialDecomposition(Mask* chamferMask, SimplicialConeDecomposition* decomposition, HPolytopeTable* hPolytopeTable, int radiusMax) {
    unsigned int radius;
    unsigned int cone;
    unsigned int neighbor;
    unsigned int coeff;
    unsigned int d;
    
    assert(radiusMax <= MAX_LUT_ENTRY);
    assert(chamferMask->ng >= 0);
    // Not correct: a 3D mask may contain only vector a
    // assert(chamferMask->ng >= DIMENSION);

    triangulation(chamferMask, decomposition, hPolytopeTable);
    //matrixPrint(stdout, hPolytopeTable->matrix);
    //hPolytopeTable->halfspaceCount = mask->ng - 1;
    hPolytopeTable->halfspaceCount = hPolytopeTable->matrix->height;
    hPolytopeTable->polytopeCount = radiusMax + 1;

    for (d = 1; d <= DIMENSION; d++) {
	for (cone = 0; cone < decomposition->coneCount[d]; cone++) {
	    simplicialConeComputeHIncrements(decomposition->conesPtr[d][cone], hPolytopeTable);
	}
    }
    
    for (cone = 0; cone < hPolytopeTable->halfspaceCount; cone++) {
	// Initialize hyperplane coefficients to zero for radius 0
	hPolytopeTable->coeff[0][cone] = 0;
	
	// The displacement along each neighbor induces a increment
	// of the hyperplane coefficient
	unsigned int hPolytopeTableInc[MAX_WEIGHT_COUNT];
	for (neighbor = 0; neighbor < chamferMask->ng; neighbor++) {
	    hPolytopeTableInc[neighbor] = 0;
	    for (d = 0; d < DIMENSION; d++) {
		hPolytopeTableInc[neighbor] +=
		chamferMask->vg[neighbor].p[d] * hPolytopeTable->matrix->elts[cone * DIMENSION + d];
	    }
	}
	
	for (radius = 1; radius <= radiusMax; radius++) {
	    coeff = hPolytopeTable->coeff[radius - 1][cone];
	    for (neighbor = 0; neighbor < chamferMask->ng; neighbor++) {
		if (radius >= chamferMask->vg[neighbor].w)
		    coeff = max(coeff, hPolytopeTable->coeff[radius - chamferMask->vg[neighbor].w][cone] + hPolytopeTableInc[neighbor]);
	    }
#ifndef NDEBUG
	    assert(radius < hPolytopeTable->polytopeCount);
#endif
	    hPolytopeTable->coeff[radius][cone] = coeff;
	}
    }
    
    for (radius = 0; radius <= radiusMax; radius++) {
	int max = hPolytopeTable->coeff[radius][0];
	for (cone = 0; cone < hPolytopeTable->halfspaceCount; cone++) {
	    if (max < hPolytopeTable->coeff[radius][cone])
		max = hPolytopeTable->coeff[radius][cone];
	}
	hPolytopeTable->coeff[radius][hPolytopeTable->halfspaceCount] = max;
    }
}

/*!
 * \brief Prints a brief usage message and exits.
 */
void usage() __attribute__ ((noreturn));
void usage() {
    fprintf(stdout,"\
usage:hLutChamfer3D [-r|--maxradius max_radius]\n\
		    [-w|--weights a b c d...]\n\
	            [-m|--mask weighting_count x y w... x y w]\n\
		    [-o|output outputfile]\n\n\
hLutChamfer3D checks if a G-symmetrical chamfer mask induces a norm or not and,\n\
for norms, computes the medial axis test neighborhood and lut values.\n\
\n\
Return codes\n\
	  0	success\n\
	255	not a norm\n\
\n\
Additional output streams\n\
	  3	elapsed time\n\
	  4	list visited points\n\
	  5	H-polytope matrix of chamfer balls (facet normal vectors)\n\
	  6	H-parameters of chamfer balls\n\
\n\
Examples\n\
- compute the test neighborhood and lut values for the mask <4,6,7,9,10> for a\n\
  maximal radius equal to 2000:\n\
  hLutChamfer3D -r 2000 -w 4 6 7 9 10 -o -\n\
- check if the mask <3,4,7> induces a norm (in this case, return code 255: not a\n\
  norm)\n\
  hLutChamfer3D -w 3 4 7\n\
- print the facet normal vectors of the equivalent rational ball for the mask\n\
  <7,8,11,14> on the standard output:\n\
  hLutChamfer3D -w 7 8 11 14 5>&1\n\
%s\n", rcsid);
    exit(EXIT_FAILURE);
}

typedef struct {
    Vector* vec;
    int	    apparitionRadius;
    int	    index;
} lutEntry;

int lutEntryCompare(lutEntry* a, lutEntry* b) {
    int i;

    if (a->apparitionRadius != b->apparitionRadius)
	return a->apparitionRadius - b->apparitionRadius;
    for (i = 0; i < DIMENSION; i++) {
	if ((*a->vec)[i] != (*b->vec)[i])
	    return (*a->vec)[i] - (*b->vec)[i];
    }
    return 0;
}

void lutMaskSort(int displayOrder[MAX_WEIGHT_COUNT], Mask* lutMask, int apparitionRadius[MAX_WEIGHT_COUNT]) {
    int i;
    lutEntry lutEntries[MAX_WEIGHT_COUNT];

    for (i = 0; i < lutMask->ng; i++) {
	lutEntries[i].index = i;
	lutEntries[i].vec = &lutMask->vg[i].p;
	lutEntries[i].apparitionRadius = apparitionRadius[i];
    }
    qsort(lutEntries, lutMask->ng, sizeof(lutEntry), (comparisonFunction)lutEntryCompare);
    for (i = 0; i < lutMask->ng; i++) {
	displayOrder[i] = lutEntries[i].index;
    }
}

int main(int argc, char* argv[]) {
    Mask* chamferMask = NULL;
    static Mask lutMask;
    static LookUpTable lut;
    static HPolytopeTable hPolytopeTable;
    unsigned int radiusMax = 0;
    //static int displayOrder[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
    struct rusage ruse;
    double t0, t1;
    static char* filename = NULL;
    FILE* f = NULL;

    char ch;
    lutMask.ng = 0;
    hPolytopeTable.halfspaceCount = 0;

    /* option descriptor */
    static struct option longopts[] = {
	{ "maxradius",		required_argument,	NULL,		'r' },
	{ "output",		required_argument,	NULL,		'o' },
	{ "mask",		required_argument,	NULL,		'm' },
	{ "weights",		required_argument,	NULL,		'w' },
	{ NULL,			0,			NULL,		 0 }
    };

    while ((ch = getopt_long(argc, argv, "r:o:m:w:", longopts, NULL)) != -1)
	switch (ch) {
	    case 'w': {
		Weighting weighting;
		int d;
		for (d = 0; d < DIMENSION; d++)
		    weighting.p[d] = 0;
		//weightingCount = atoi(optarg);
		//assert(weightingCount > 0);
		assert(chamferMask == NULL);
		chamferMask = maskCreate(1, DIMENSION);

		optind--;
		while (argv[optind] != NULL && argv[optind][0] != '-') {
		    nextVisiblePoint(weighting.p, DIMENSION);
		    //vectorPrint(stdout, weighting.p, DIMENSION);
		    //printf(" %s\n", argv[optind]);
		    maskAddWeighting(chamferMask, weighting.p, atoi(argv[optind]), DIMENSION);
		    optind++;
		}
	    }
		break;
	    case 'm': {
		int v, weightingCount;
		weightingCount = atoi(optarg);
		assert(weightingCount > 0);
		assert(chamferMask == NULL);
		chamferMask = maskCreate(weightingCount, DIMENSION);
		for (v = 1; v < 1 + (DIMENSION+1) * weightingCount; v += DIMENSION+1, optind += DIMENSION+1) {
		    Weighting weighting;
		    int d;
		    for (d = 0; d < DIMENSION; d++)
			weighting.p[d] = atoi(argv[optind+d]);
		    weighting.w   = atoi(argv[optind+DIMENSION]);

		    maskAddWeighting(chamferMask, weighting.p, weighting.w, DIMENSION);
		}
		
	    }
		break;
	    case 'r':
		radiusMax = atoi(optarg);
		break;
	    case 'o':
		filename = optarg;
		break;
	    default:
		usage();
	}
    argc -= optind;
    argv += optind;

    if (chamferMask == NULL || maskWeightingCount(chamferMask) == 0) usage();

    getrusage(RUSAGE_SELF, &ruse);
    t0 = ruse.ru_utime.tv_sec + 1e-6 * ruse.ru_utime.tv_usec;

    //initHPolytopeTableFromMask(&hPolytopeTable, chamferMask, radiusMax);
    //SimplicialCone* cones[DIMENSION+1];
    //int coneCount[DIMENSION+1];
    SimplicialConeDecomposition *decomposition = simplicialConeDecompositionCreate();

    initSimplicialDecomposition(chamferMask, decomposition, &hPolytopeTable, radiusMax);

    int apparitionRadius[MAX_WEIGHT_COUNT];
    int innerRadius[MAX_WEIGHT_COUNT];
    computeLutMaskAndCols(chamferMask, decomposition, &lutMask, &hPolytopeTable, lut, radiusMax, apparitionRadius, innerRadius);

    getrusage(RUSAGE_SELF, &ruse);
    t1 = ruse.ru_utime.tv_sec + 1e-6 * ruse.ru_utime.tv_usec;

    maskDestroy(chamferMask);

    FILE* auxout;
    auxout = fdopen(HPARAMETERS_FILENO, "w");
    if (auxout) {
	hPolytopeTablePrint(auxout, &hPolytopeTable);
	fclose(auxout);
    }
    
    auxout = fdopen(MATRIX_FILENO, "w");
    if (auxout) {
	matrixPrint(auxout, hPolytopeTable.matrix);
	fclose(auxout);
    }
    
    auxout = fdopen(ELAPSED_TIME_FILENO, "w");
    if (auxout) {
	fprintf(auxout, "Elapsed time %.6f\n", t1 - t0);
	fclose(auxout);
    }
    
    /*
     * Write test neighborhood in standard output
     */
    int displayOrder[MAX_WEIGHT_COUNT];
    lutMaskSort(displayOrder, &lutMask, apparitionRadius);
    maskPrint(stdout, &lutMask, displayOrder, apparitionRadius, innerRadius);

    /*
     * Write lut values into a file
     */
    if (filename) {
	f = strcmp(filename, "-") == 0 ? stdout : fopen (filename, "w");
    }
    if (f) {
	printLut(f, &lutMask, &hPolytopeTable, lut, radiusMax, displayOrder);
	fclose (f);
    }

    return EXIT_SUCCESS;
}