/*
 * 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/>.
 */
// $Id: triangulation.c,v 1.15 2008/10/31 06:12:30 cvs Exp $

#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include <limits.h>

#include "hLutChamfer3D.h"

int nextCombi(int* combi, int size, int max) {
    int i = size - 1;
    while (i >= 0 && combi[i] >= max - size + i + 1) i--;
    if (i < 0) return 0;
    combi[i]++;
    for (i = i + 1; i < size; i++)
	combi[i] = combi[i - 1] + 1;
    return 1;
}

void printCombi(int* combi, int size) {
    int i;
    for (i = 0; i < size; i++) {
	printf("%d ", combi[i]);
    }
}

void combiInit(int* combi, int size, int max) {
    int i;
    for (i = 0; i < size; i++) {
	combi[i] = i;
    }
    combi[size - 1]--;
}

void nextPoint(Vector point, int dimension) {
    int i = dimension - 1;
    while (i > 0 && point[i] == point[i-1]) i--;
    point[i]++;
    for (i++; i < dimension; i++)
	point[i] = 0;
}

void nextVisiblePoint(Vector point, int dimension) {
    do {
	nextPoint(point, dimension);
    } while (!vectorIsVisible(point, dimension));
}

int nextSignChange(int* values, int size) {
    int i, j;
    for (i = size - 1; i >= 0 && values[i] <= 0;) i--;
    if (i >= 0) {
	values[i] = -values[i];
    }
    for (j = i + 1; j < size; j++) {
	values[j] = -values[j];
    }
    return i >= 0;
}

int nextPermutation(int* values, int size) {
    int i, j, max;
    for (i = size - 2; (4-values[i]) >= (4-values[i+1]) && i >= 0;) i--;
    
    max = i + 1;
    for (j = i + 2; j < size; j++) {
        if ((4-values[j]) > (4-values[i]) && (4-values[j]) < (4-values[max]))
	    max = j;
    }
    //printf("%d %d\n", i, max);
    
    if (i >= 0) {
	int t;
	t = values[i];
	values[i] = values[max];
	values[max] = t;
    }
    
    if (i < size - 2) {
	//printf("%d (%d)\n", i + 1, size - i - 1);
	qsort(values + i + 1, size - i - 1, sizeof(int), (comparisonFunction)decreasingOrder);
    }
    return i >= 0;
}

int nextTruc(int* values, int size) {
    return nextSignChange(values, size) ||
	   nextPermutation(values, size);
}

int coneDeterminant(Mask* mask, int* combi, int dimension) {
    // Compute determinant
    assert(dimension == 3);
    int det;
    det  = mask->vg[combi[0]].p[0] * (mask->vg[combi[1]].p[1] * mask->vg[combi[2]].p[2] - mask->vg[combi[1]].p[2] * mask->vg[combi[2]].p[1]);
    det -= mask->vg[combi[1]].p[0] * (mask->vg[combi[0]].p[1] * mask->vg[combi[2]].p[2] - mask->vg[combi[0]].p[2] * mask->vg[combi[2]].p[1]);
    det += mask->vg[combi[2]].p[0] * (mask->vg[combi[0]].p[1] * mask->vg[combi[1]].p[2] - mask->vg[combi[0]].p[2] * mask->vg[combi[1]].p[1]);
    //printf("det: %d\n", det);
    return det;
}

int computeNormal(Mask* mask, int* combi, int dimension, Weighting* normal) {
    int i, det;

    // Compute determinant
    assert(dimension == 3);
    det = coneDeterminant(mask, combi, dimension);
    //printf("det: %d\n", det);
    /*
     if (abs(det) != 1)
     return 0;
     */
    if (det == 0)
	return 0;
    /*
     a00*det  = points[combi[1]][1] * points[combi[2]][2] - points[combi[1]][2] * points[combi[2]][1]
     a01*det -= points[combi[1]][0] * points[combi[2]][2] - points[combi[1]][2] * points[combi[2]][0]
     a02*det  = points[combi[1]][0] * points[combi[2]][1] - points[combi[1]][1] * points[combi[2]][0]
     
     a10*det -= points[combi[0]][1] * points[combi[2]][2] - points[combi[0]][2] * points[combi[2]][1]
     a11*det  = points[combi[0]][0] * points[combi[2]][2] - points[combi[0]][2] * points[combi[2]][0]
     a12*det -= points[combi[0]][0] * points[combi[2]][1] - points[combi[0]][1] * points[combi[2]][0]
     
     a20*det  = points[combi[0]][1] * points[combi[1]][2] - points[combi[0]][2] * points[combi[1]][1]
     a21*det -= points[combi[0]][0] * points[combi[1]][2] - points[combi[0]][2] * points[combi[1]][0]
     a22*det  = points[combi[0]][0] * points[combi[1]][1] - points[combi[0]][1] * points[combi[1]][0]
     */
    
    int wda00 =  mask->vg[combi[1]].p[1] * mask->vg[combi[2]].p[2] - mask->vg[combi[1]].p[2] * mask->vg[combi[2]].p[1];
    int wda10 = -mask->vg[combi[1]].p[0] * mask->vg[combi[2]].p[2] + mask->vg[combi[1]].p[2] * mask->vg[combi[2]].p[0];
    int wda20 =  mask->vg[combi[1]].p[0] * mask->vg[combi[2]].p[1] - mask->vg[combi[1]].p[1] * mask->vg[combi[2]].p[0];
    
    int wda01 = -mask->vg[combi[0]].p[1] * mask->vg[combi[2]].p[2] + mask->vg[combi[0]].p[2] * mask->vg[combi[2]].p[1];
    int wda11 =  mask->vg[combi[0]].p[0] * mask->vg[combi[2]].p[2] - mask->vg[combi[0]].p[2] * mask->vg[combi[2]].p[0];
    int wda21 = -mask->vg[combi[0]].p[0] * mask->vg[combi[2]].p[1] + mask->vg[combi[0]].p[1] * mask->vg[combi[2]].p[0];
    
    int wda02 =  mask->vg[combi[0]].p[1] * mask->vg[combi[1]].p[2] - mask->vg[combi[0]].p[2] * mask->vg[combi[1]].p[1];
    int wda12 = -mask->vg[combi[0]].p[0] * mask->vg[combi[1]].p[2] + mask->vg[combi[0]].p[2] * mask->vg[combi[1]].p[0];
    int wda22 =  mask->vg[combi[0]].p[0] * mask->vg[combi[1]].p[1] - mask->vg[combi[0]].p[1] * mask->vg[combi[1]].p[0];
    
    /*
     det * a0x = wda0x * w1 * w2
     det * a1x = wda1x * w0 * w2
     det * a2x = wda2x * w0 * w1
     */
    
    //printf("%d %d %d\n", weights[0]*wda00, weights[1]*wda01, weights[2]*wda02);
    //printf("%d %d %d\n", weights[0]*wda10, weights[1]*wda11, weights[2]*wda12);
    //printf("%d %d %d\n", weights[0]*wda20, weights[1]*wda21, weights[2]*wda22);
    
    // inv(A)*[1...1]'
    normal->p[0] = mask->vg[combi[0]].w*wda00 + mask->vg[combi[1]].w*wda01 + mask->vg[combi[2]].w*wda02;
    normal->p[1] = mask->vg[combi[0]].w*wda10 + mask->vg[combi[1]].w*wda11 + mask->vg[combi[2]].w*wda12;
    normal->p[2] = mask->vg[combi[0]].w*wda20 + mask->vg[combi[1]].w*wda21 + mask->vg[combi[2]].w*wda22;
    //computeNormal(normal, points, weights, combi, dimension);
    /*
     assert(normal[0] == weights[combi[0]]*wda00 + weights[combi[1]]*wda01 + weights[combi[2]]*wda02);
     assert(normal[1] == weights[combi[0]]*wda10 + weights[combi[1]]*wda11 + weights[combi[2]]*wda12);
     assert(normal[2] == weights[combi[0]]*wda20 + weights[combi[1]]*wda21 + weights[combi[2]]*wda22);
     */
    
    if (det < 0) {
	for (i = 0; i < dimension; i++) {
	    normal->p[i] = -normal->p[i];
	}
	normal->w = -det;
    }
    else
	normal->w = det;
#ifndef NDEBUG
    // Invariant:
    // the scalar product of cone vector with the normal vector is equal to the weight
    for (i = 0; i < dimension; i++) {
	int sp = vectorScalarProduct(mask->vg[combi[i]].p, normal->p, dimension);
	assert(sp == mask->vg[combi[i]].w * normal->w);
    }
#endif
    /*
    abort(); // dead code
    assert(dimension == 3);
    normal[0] =  points[combi[0]][1] * points[combi[1]][2] - points[combi[0]][2] * points[combi[1]][1];
    normal[1] = -points[combi[0]][0] * points[combi[1]][2] + points[combi[0]][2] * points[combi[1]][0];
    normal[2] =  points[combi[0]][0] * points[combi[1]][1] - points[combi[0]][1] * points[combi[1]][0];
    */
    /*
    normal[0] =  (points[combi[1]][1]-points[combi[0]][1]) * (points[combi[2]][2]-points[combi[0]][2])
		-(points[combi[1]][2]-points[combi[0]][2]) * (points[combi[2]][1]-points[combi[0]][1]);
    normal[1] = -(points[combi[1]][0]-points[combi[0]][0]) * (points[combi[2]][2]-points[combi[0]][2])
		+(points[combi[1]][2]-points[combi[0]][2]) * (points[combi[2]][0]-points[combi[0]][0]);
    normal[2] =  (points[combi[1]][0]-points[combi[0]][0]) * (points[combi[2]][1]-points[combi[0]][1])
		-(points[combi[1]][1]-points[combi[0]][1]) * (points[combi[2]][0]-points[combi[0]][0]);
     */
    /*
    int wda00 =  points[combi[1]][1] * points[combi[2]][2] - points[combi[1]][2] * points[combi[2]][1];
    int wda10 = -points[combi[1]][0] * points[combi[2]][2] + points[combi[1]][2] * points[combi[2]][0];
    int wda20 =  points[combi[1]][0] * points[combi[2]][1] - points[combi[1]][1] * points[combi[2]][0];
    
    int wda01 = -points[combi[0]][1] * points[combi[2]][2] + points[combi[0]][2] * points[combi[2]][1];
    int wda11 =  points[combi[0]][0] * points[combi[2]][2] - points[combi[0]][2] * points[combi[2]][0];
    int wda21 = -points[combi[0]][0] * points[combi[2]][1] + points[combi[0]][1] * points[combi[2]][0];
    
    int wda02 =  points[combi[0]][1] * points[combi[1]][2] - points[combi[0]][2] * points[combi[1]][1];
    int wda12 = -points[combi[0]][0] * points[combi[1]][2] + points[combi[0]][2] * points[combi[1]][0];
    int wda22 =  points[combi[0]][0] * points[combi[1]][1] - points[combi[0]][1] * points[combi[1]][0];

    normal[0] = weights[combi[0]]*wda00 + weights[combi[1]]*wda01 + weights[combi[2]]*wda02;
    normal[1] = weights[combi[0]]*wda10 + weights[combi[1]]*wda11 + weights[combi[2]]*wda12;
    normal[2] = weights[combi[0]]*wda20 + weights[combi[1]]*wda21 + weights[combi[2]]*wda22;
     */
    /*
    if (normal[0] < 0) {
	for (i = 0; i < dimension; i++) {
	    normal[i] = -normal[i];
	}
    }
     */
    return 1;
}

void computeDetInv(int* det, Vector detinv[DIMENSION], Mask* mask, int* combi) {
    assert(DIMENSION == 3);
    *det = coneDeterminant(mask, combi, DIMENSION);

    detinv[0][0] =  mask->vg[combi[1]].p[1] * mask->vg[combi[2]].p[2] - mask->vg[combi[1]].p[2] * mask->vg[combi[2]].p[1];
    detinv[1][0] = -mask->vg[combi[1]].p[0] * mask->vg[combi[2]].p[2] + mask->vg[combi[1]].p[2] * mask->vg[combi[2]].p[0];
    detinv[2][0] =  mask->vg[combi[1]].p[0] * mask->vg[combi[2]].p[1] - mask->vg[combi[1]].p[1] * mask->vg[combi[2]].p[0];
    
    detinv[0][1] = -mask->vg[combi[0]].p[1] * mask->vg[combi[2]].p[2] + mask->vg[combi[0]].p[2] * mask->vg[combi[2]].p[1];
    detinv[1][1] =  mask->vg[combi[0]].p[0] * mask->vg[combi[2]].p[2] - mask->vg[combi[0]].p[2] * mask->vg[combi[2]].p[0];
    detinv[2][1] = -mask->vg[combi[0]].p[0] * mask->vg[combi[2]].p[1] + mask->vg[combi[0]].p[1] * mask->vg[combi[2]].p[0];
    
    detinv[0][2] =  mask->vg[combi[0]].p[1] * mask->vg[combi[1]].p[2] - mask->vg[combi[0]].p[2] * mask->vg[combi[1]].p[1];
    detinv[1][2] = -mask->vg[combi[0]].p[0] * mask->vg[combi[1]].p[2] + mask->vg[combi[0]].p[2] * mask->vg[combi[1]].p[0];
    detinv[2][2] =  mask->vg[combi[0]].p[0] * mask->vg[combi[1]].p[1] - mask->vg[combi[0]].p[1] * mask->vg[combi[1]].p[0];
}

void vecMatProduct(Vector pr, Vector point, Vector mat[DIMENSION]) {
    int i, j;
    for (i = 0; i < DIMENSION; i++) {
	pr[i] = 0;
	for (j = 0; j < DIMENSION; j++) {
	    pr[i] += mat[j][i] * point[j];
	}
    }
}

/*
 * Look for a point that is not representable by a set of independent vectors.
 * point*det(A)*inv(A) has negative components -> point is not in the cone.
 * components of point*det(A)*inv(A) are not all multiples of det(A) -> point
 * is not representable by a integral combination of the vectors.
 */
void findNotRepresentablePointInCone(Vector point, Mask* points, int* combi, int dimension) {
    assert(dimension == 3);
    int i;
    int det;
    Vector detinv[DIMENSION];
    Vector pr;
    computeDetInv(&det, detinv, points, combi);
    assert(abs(det) > 1);
    if (det < 0) {
	det = -det;
	for (i = 0; i < DIMENSION; i++) {
	    int j;
	    for (j = 0; j < DIMENSION; j++) {
		detinv[i][j] = -detinv[i][j];
	    }
	}
    }

    for (i = 0; i < DIMENSION; i++)
	point[i] = 0;
    int inCone = 0;
    int representable;
    do {
	nextPoint(point, DIMENSION);
	vecMatProduct(pr, point, detinv);
	//vectorPrint(stdout, point, DIMENSION);
	//vectorPrint(stdout, pr, DIMENSION);
	//printf("\n");
	inCone = 1;
	representable = 1;
	for (i = 0; inCone && i < DIMENSION; i++) {
	    inCone = pr[i] >= 0;
	    representable &= (pr[i] % det) == 0;
	}
    } while (!inCone || representable);
}

int checkCone(Mask* mask, int* combi, int dimension, Weighting* normal) {
    int i;

    if (!computeNormal(mask, combi, dimension, normal))
	return 0;

    int coneOK = 1;
    for (i = 0; i < mask->ng; i++) {
	int sp = 0, j;
	for (j = 0; j < dimension; j++) {
	    sp += mask->vg[i].p[j] * normal->p[j];
	}
	assert(sp == vectorScalarProduct(normal, mask->vg[i].p, dimension));
	if (sp > mask->vg[i].w * normal->w)
	    coneOK = 0;
	//printf(" %d", sp <= mask->vg[i].w);
    }

    return coneOK;
}

typedef struct {
    int incidencesToFind;
    Vector normal;
    int offset;
    int points[DIMENSION - 1];
} Ridge;

int findUnfinishedRidge(Ridge* ridges, int ridgeCount) {
    int i;
    for (i = 0; i < ridgeCount; i++) {
	if (ridges[i].incidencesToFind > 0)
	    return i;
    }
    return -1;
}

int findRidge(Ridge* ridges, int ridgeCount, int* combi, int* edge) {
    int i, j;
    int equal = 0;
    for (i = 0; !equal && i < ridgeCount; i++) {
	equal = 1;
	for (j = 0; equal && j < DIMENSION - 1; j++) {
	    equal = ridges[i].points[j] == combi[edge[j]];
	}
    }
    return equal ? i - 1 : -1;
}

void checkAddRidge(Ridge* ridges, int* ridgeCount, Mask* mask, int* combi, int* edge, Vector insidePoint) {
    assert(DIMENSION == 3);
    int i1, i2;
    if (combi[edge[0]] < combi[edge[1]]) {
	i1 = combi[edge[1]];
	i2 = combi[edge[0]];
    } else {
	i1 = combi[edge[0]];
	i2 = combi[edge[1]];
    }
    int index = findRidge(ridges, *ridgeCount, combi, edge);
    if (index == -1) {
	ridges[*ridgeCount].incidencesToFind = 1;
	int j;
	for (j = 0; j < DIMENSION - 1; j++) {
	    ridges[*ridgeCount].points[j] = combi[edge[j]];
	}
	// Compute the normal
	assert(DIMENSION == 3);
	Vector* p1 = &mask->vg[i1].p;
	Vector* p2 = &mask->vg[i2].p;
	ridges[*ridgeCount].normal[0] =  (*p1)[1] * (*p2)[2] - (*p1)[2] * (*p2)[1];
	ridges[*ridgeCount].normal[1] = -(*p1)[0] * (*p2)[2] + (*p1)[2] * (*p2)[0];
	ridges[*ridgeCount].normal[2] =  (*p1)[0] * (*p2)[1] - (*p1)[1] * (*p2)[0];
	ridges[*ridgeCount].offset = vectorScalarProduct(ridges[*ridgeCount].normal, *p1, DIMENSION);
	int inside = vectorScalarProduct(ridges[*ridgeCount].normal, insidePoint, DIMENSION);
	if (inside < 0) {
	    for (j = 0; j < DIMENSION; j++) {
		ridges[*ridgeCount].normal[j] = -ridges[*ridgeCount].normal[j];
	    }
	}
	(*ridgeCount)++;
    }
    else {
	ridges[index].incidencesToFind--;
	assert(ridges[index].incidencesToFind == 0);
    }
}

int pointInCone(Vector point, Vector* normals, int dimension) {
    int i;
    for (i = 0; i < dimension; i++) {
	if (vectorScalarProduct(point, normals[i], dimension) < 0)
	    return 0;
    }
    return 1;
}

Mask* addSymmetricalPoints(GeneratorImage* dt, Mask* chamferMask) {
    Mask* result = maskCreate(0 /* size, unused */, DIMENSION);
    int i, j, k;
    int l;
    Vector vector;
    Vector* symm;
    int* symmW;
    int symmCount = 0;
    assert(DIMENSION == 3);
    // Each point has at most 47 symmetrical points in other cones
    symm = (Vector*) malloc(48 * chamferMask->ng * sizeof(Vector));
    assert(symm);
    symmW = (int*) malloc(48 * chamferMask->ng * sizeof(int));
    assert(symmW);
    //printf("Symmetrical points\n");
    // Take every point in the generator cone
    for (i = 0; i < chamferMask->ng; i++) {
	// Add to the result points
	maskAddWeighting(result, chamferMask->vg[i].p, imageGetPixel(dt, chamferMask->vg[i].p), DIMENSION);
	// Compute all symmetric points
	do {
	    vectorCopy(symm[symmCount], chamferMask->vg[i].p, DIMENSION);
	    symmW[symmCount] = chamferMask->vg[i].w;
	    //vectorPrint(stdout, symm[symmCount], DIMENSION);
	    //printf(": %d\n", symmW[symmCount]);
	    symmCount++;
	} while (nextTruc(chamferMask->vg[i].p, DIMENSION));
    }

    // Take each couple of points
    for (i = 0; i < symmCount - 1; i++) {
	for (j = i + 1; j < symmCount; j++) {
	    // For each facet of the generator cone
	    for (k = 0; k < DIMENSION - 1; k++) {
		//printf("%d %d %d\n", i, j, k);
		int num;
		int den;

		if (k < DIMENSION - 1) {
		    den = symm[j][k+1]-symm[i][k+1]-symm[j][k]+symm[i][k];
		    num = symm[i][k] - symm[i][k+1];
		    // Invariant
		    assert(den*symm[i][k]+num*(symm[j][k]-symm[i][k]) ==
			   den*symm[i][k+1]+num*(symm[j][k+1]-symm[i][k+1]));
		}
		else {
		    den = -symm[j][k]+symm[i][k];
		    num = symm[i][k];
		    assert(den*symm[i][k]+num*(symm[j][k]-symm[i][k]) == 0);
		}
		if (den < 0) {
		    num = -num;
		    den = -den;
		}
		if (den == 0 || num <= 0 || num >= den)
		    continue;
		for (l = 0; l < DIMENSION; l++) {
		    vector[l] = den*symm[i][l] + num*(symm[j][l]-symm[i][l]);
		}
		if (!vectorIsInGenerator(vector, DIMENSION))
		    continue;
		if (vector[0] == 0)
		    continue;
		int w = den*symmW[i] + num*(symmW[j]-symmW[i]);
		//int w2 = imageGetPixel(dt, vector);
		if (w <= 0)
		    continue;
		maskAddWeighting(result, vector, w, DIMENSION);
		//maskAddWeighting(result, vector, w, DIMENSION);
		/*
		printf("%d/%d %d/%d %d/%d (%d/%d)\n",
		       vector[0], den,
		       vector[1], den,
		       vector[2], den,
		       w, den);
		 */
	    }
	}
    }
    free(symm);
    free(symmW);

    // Find intersections between the facets of the rational ball and the
    // axes of the generator.
    // In each direction (1,0,0) (1,1,0) and (1,1,1), find the furthest vector
    // in the mask (+ additionnal points ?).
    static Vector axes[3] = {{1,0,0},{1,1,0},{1,1,1}};
    int spj = 0;
    int wj = 0;
    j = 0;
    for (i = 1; i < result->ng; i++) {
	int spi = vectorScalarProduct(result->vg[i].p, axes[0], DIMENSION);
	if (spi * wj > spj * result->vg[i].w) {
	    spj = spi;
	    wj = result->vg[i].w;
	    j = i;
	}
	//if (result->vg[i].p[0] * result->vg[j].w > result->vg[j].p[0] * result->vg[i].w)
	//    j = i;
    }
    vector[0] = 2*result->vg[j].p[0];
    vector[1] = 0;
    vector[2] = 0;
    maskAddWeighting(result, vector, 2*result->vg[j].w, DIMENSION);
    //printf("x max %d\n", j);

    j = 0;
    for (i = 1; i < result->ng; i++) {
	if ((result->vg[i].p[0] + result->vg[i].p[1]) * result->vg[j].w >
	    (result->vg[j].p[0] + result->vg[j].p[1]) * result->vg[i].w)
	    j = i;
    }
    vector[0] = result->vg[j].p[0] + result->vg[j].p[1];
    vector[1] = vector[0];
    vector[2] = 0;
    maskAddWeighting(result, vector, 2*result->vg[j].w, DIMENSION);
    //printf("x + y max %d\n", j);

    j = 0;
    for (i = 1; i < result->ng; i++) {
	if ((result->vg[i].p[0] + result->vg[i].p[1] + result->vg[i].p[2]) * result->vg[j].w >
	    (result->vg[j].p[0] + result->vg[j].p[1] + result->vg[j].p[2]) * result->vg[i].w)
	    j = i;
    }
    vector[0] = result->vg[j].p[0] + result->vg[j].p[1] + result->vg[j].p[2];
    vector[1] = vector[0];
    vector[2] = vector[0];
    maskAddWeighting(result, vector, 3*result->vg[j].w, DIMENSION);
    //printf("x + y + z max %d\n", j);
    
    return result;
}

int triangulation(Mask* chamferMask, SimplicialConeDecomposition* decomposition, HPolytopeTable* hPolytopeTable) {
#define MAX_POINT_COUNT 30
    Mask* ballPoints;
    //int pointCount = 6;
    //Vector points[MAX_POINT_COUNT];
    //SimplicialCone cones[DIMENSION][30];
    //int weights[MAX_POINT_COUNT] = {7, 8, 11, 14, 16, 28};
    int combi[DIMENSION];
    int minDet = INT_MAX;
    int minDetIndex = 0;
    Ridge ridges[30];
    int ridgeCount = 0;
    int index;
    GeneratorImage* dt = imageCreate(chamferMask);
    /* allocate empty matrix */
    hPolytopeTable->matrix = matrixCreate(DIMENSION);

    int i, j;
    Weighting normal;
    int det = 0;

    ballPoints = addSymmetricalPoints(dt, chamferMask);

    // Choose first point:
    // furthest point from O in direction (1, 0, 0)
    j = -1;
    for (i = 0; i < ballPoints->ng; i++) {
	if (ballPoints->vg[i].p[1] == 0 && ballPoints->vg[i].p[2] == 0) {
	    if (j == -1 || ballPoints->vg[j].w * ballPoints->vg[i].p[0] > ballPoints->vg[i].w * ballPoints->vg[j].p[0])
		j = i;
	}
    }
    {
	// Swap points 0 and j
	int t;
	for (i = 0; i < DIMENSION; i++) {
	    t = ballPoints->vg[j].p[i];
	    ballPoints->vg[j].p[i] = ballPoints->vg[0].p[i];
	    ballPoints->vg[0].p[i] = t;
	}
	t = ballPoints->vg[j].w;
	ballPoints->vg[j].w = ballPoints->vg[0].w;
	ballPoints->vg[0].w = t;
    }

    // Choose second point:
    // maximize angle with first point in cone (1,0,0)(1,1,0)
    j = -1;
    for (i = 1; i < ballPoints->ng; i++) {
	if (ballPoints->vg[i].p[2] == 0) {
	    // det ballPoints->vg[j]-ballPoints->vg[0],ballPoints->vg[i]-ballPoints->vg[0]
	    // ballPoints->vg[j].p[0]/ballPoints->vg[j].w - ballPoints->vg[0].p[0]/ballPoints->vg[0].w
	    // ballPoints->vg[j].p[1]/ballPoints->vg[j].w - ballPoints->vg[0].p[1]/ballPoints->vg[0].w
	    // ballPoints->vg[i].p[0]/ballPoints->vg[i].w - ballPoints->vg[0].p[0]/ballPoints->vg[0].w
	    // ballPoints->vg[i].p[1]/ballPoints->vg[i].w - ballPoints->vg[0].p[1]/ballPoints->vg[0].w
	    // multiply all values by
	    // ballPoints->vg[0].w*ballPoints->vg[i].w*ballPoints->vg[j].w
	    int x1,x2,y1,y2;
	    x1 = ballPoints->vg[j].p[0]*ballPoints->vg[0].w*ballPoints->vg[i].w
	       - ballPoints->vg[0].p[0]*ballPoints->vg[i].w*ballPoints->vg[j].w;
	    y1 = ballPoints->vg[j].p[1]*ballPoints->vg[0].w*ballPoints->vg[i].w
	       - ballPoints->vg[0].p[1]*ballPoints->vg[i].w*ballPoints->vg[j].w;
	    x2 = ballPoints->vg[i].p[0]*ballPoints->vg[0].w*ballPoints->vg[j].w
	       - ballPoints->vg[0].p[0]*ballPoints->vg[i].w*ballPoints->vg[j].w;
	    y2 = ballPoints->vg[i].p[1]*ballPoints->vg[0].w*ballPoints->vg[j].w
	       - ballPoints->vg[0].p[1]*ballPoints->vg[i].w*ballPoints->vg[j].w;
	    if (j == -1 || x1 * y2 < x2 * y1)
		j = i;
	    if (x1 * y2 == x2 * y1) {
		if (y1 < y2) {
		    j = i;
		}
	    }
	}
    }
    {
	// Swap points 1 and j
	int t;
	for (i = 0; i < DIMENSION; i++) {
	    t = ballPoints->vg[j].p[i];
	    ballPoints->vg[j].p[i] = ballPoints->vg[1].p[i];
	    ballPoints->vg[1].p[i] = t;
	}
	t = ballPoints->vg[j].w;
	ballPoints->vg[j].w = ballPoints->vg[1].w;
	ballPoints->vg[1].w = t;
    }
    
    /*
    for (i = 0; i < pointCount; i++) {
	simplicialConeInit(&cones[1][i], 1);
	vectorCopy(cones[1][i].vectorSum, points[i], DIMENSION);
    }
 */
    {
	combi[0] = 0;
	combi[1] = 1;
	combi[2] = 1;
	int edge[DIMENSION - 1];
	Vector outside;
	edge[0] = 0;
	edge[1] = 1;
	outside[0] = 1;
	outside[1] = 1;
	outside[2] = -1;
	checkAddRidge(ridges, &ridgeCount, ballPoints, combi, edge, outside);
    }

    while ((index = findUnfinishedRidge(ridges, ridgeCount)) != -1) {
	minDet = INT_MAX;
	assert(DIMENSION == 3);
	combi[0] = ridges[index].points[0];
	combi[1] = ridges[index].points[1];
	/*
	 Pour chaque point
	    si pas du bon côté passer au point suivant
	    si coplanaire passer au point suivant
	    si intérieur passer au point suivant
	    si extérieur remplacer l'ancien point
	    si juste sur le bord et déterminant plus petit, remplacer l'ancien point
	 */
	i = -1;
	for (combi[2] = 0; combi[2] < ballPoints->ng; combi[2]++) {
	    int sp;
	    // Not on the right side
	    if (vectorScalarProduct(ridges[index].normal, ballPoints->vg[combi[2]].p, DIMENSION) >= 0)
		continue;
	    det = abs(coneDeterminant(ballPoints, combi, DIMENSION));
	    if (det == 0) continue;
	    if (i == -1 ||		    // No third point chosen yet
		(sp = vectorScalarProduct(normal.p, ballPoints->vg[combi[2]].p, DIMENSION)) > ballPoints->vg[combi[2]].w * normal.w ||
						    // The point is outside
		(sp == ballPoints->vg[combi[2]].w * normal.w && det < minDet)		    // The point makes a smaller triangle on the border
		) {
		
		i = combi[2];
		minDetIndex = combi[2];
		minDet = det;
		computeNormal(ballPoints, combi, DIMENSION, &normal);
	    }
	}
	combi[2] = i;
	/*
	for (combi[2] = 0; minDet != 1 && combi[2] < ballPoints->ng; combi[2]++) {
	    // Not on the right side
	    if (vectorScalarProduct(ridges[index].normal, ballPoints->vg[combi[2]].p, DIMENSION) >= 0)
		continue;
	    det = abs(coneDeterminant(ballPoints, combi, DIMENSION));

	    if (checkCone(ballPoints, combi, DIMENSION, &normal)) {
		if (det > 0 && det < minDet) {
		    minDet = det;
		    minDetIndex = combi[2];
		}
	    }
	    //printf("comb %d\n", combi[2]);
	}
	combi[2]--;
	 */
	if (minDet == 1) {
	    assert(combi[2] == minDetIndex);
	    // Bingo! We found a cone
	    assert(normal.w == 1);
	    simplicialConeDecompositionAddCone(decomposition, ballPoints, combi, DIMENSION);
	    hPolytopeTable->matrix = matrixAddUniqueRowElts(hPolytopeTable->matrix, normal.p);
	    
	    // Check homogeneity in this cone
	    for (i = 0; i < DIMENSION; i++) {
		int distP = imageGetPixel(dt, ballPoints->vg[combi[i]].p);
		int distH = vectorScalarProduct(normal.p, ballPoints->vg[combi[i]].p, DIMENSION);
		if (distH != distP) {
		    // Shouldn't get there !
		    assert(0);
		    fprintf(stderr, "Not a norm\n");
		    exit(NOT_A_NORM);
		}
	    }

	    int edge[DIMENSION - 1];
	    Vector sum;
	    assert(DIMENSION == 3);
	    vectorCopy(sum, ballPoints->vg[combi[0]].p, DIMENSION);
	    vectorAdd(sum, ballPoints->vg[combi[1]].p, DIMENSION);
	    vectorAdd(sum, ballPoints->vg[combi[2]].p, DIMENSION);
	    // == decomposition->conesPtr[3][1]->vectorSum
	    combiInit(edge, DIMENSION - 1, DIMENSION);
	    while (nextCombi(edge, DIMENSION - 1, DIMENSION - 1)) {
		//printf("arête:");
		//for (i = 0; i < DIMENSION - 1; i++) {
		    //printf("%d ", combi[edge[i]]);
		//}
		//printf("\n");
		checkAddRidge(ridges, &ridgeCount, ballPoints, combi, edge, sum);
	    }
	}
	else if (minDet < INT_MAX) {
	    // Split the cone
	    //printf("Split the cone ");
	    combi[DIMENSION - 1] = minDetIndex;
	    //vectorPrint(stdout, combi, DIMENSION);
	    //printf("\n");
/*
	    // Compute the normals to all ridges
	    assert(DIMENSION == 3);
	    Vector sum;
	    vectorCopy(sum, points[combi[0]], DIMENSION);
	    vectorAdd(sum, points[combi[1]], DIMENSION);
	    vectorAdd(sum, points[combi[2]], DIMENSION);
	    
	    Vector normals[3];

	    int j;
	    for (j = 0; j < DIMENSION; j++) {
		normals[0][j] = -ridges[index].normal[j];
	    }
	    int edge[DIMENSION - 1];
	    combiInit(edge, DIMENSION - 1, DIMENSION);
	    // Skip first combination
	    nextCombi(edge, DIMENSION - 1, DIMENSION - 1);
	    i = 1;
	    while (nextCombi(edge, DIMENSION - 1, DIMENSION - 1)) {
		int machin[DIMENSION-1];
		for (j = 0; j < DIMENSION-1; j++)
		    machin[j] = combi[edge[j]];
		computeNormal(normals[i], points, weights, machin, DIMENSION);
		if (vectorScalarProduct(normals[i], sum, DIMENSION) < 0) {
		    for (j = 0; j < DIMENSION; j++) {
			normals[i][j] = -normals[i][j];
		    }
		}
		i++;
	    }
*/
	    // Find a point in the cone, not representable as an integral
	    // non negative combination of the cone vectors
	    Vector point;
	    findNotRepresentablePointInCone(point, ballPoints, combi, DIMENSION);
	    // Compute normal
	    if (!computeNormal(ballPoints, combi, DIMENSION, &normal))
		abort();
	    int dist = imageGetPixel(dt, point);
	    if (vectorScalarProduct(point, normal.p, DIMENSION) != dist * normal.w) {
		fprintf(stderr, "Not a norm ");
		vectorPrint(stderr, point, DIMENSION);
		fprintf(stderr, ": %d and ", dist);
		for (i = 0; i < DIMENSION; i++) {
		    point[i] *= minDet;
		}
		vectorPrint(stderr, point, DIMENSION);
		fprintf(stderr, ": %d\n", imageGetPixel(dt, point));
		exit(NOT_A_NORM);
	    }
	    // Add this vector to our points
	    maskAddWeighting(ballPoints, point, imageGetPixel(dt, point), DIMENSION);
	}
	else {
	    // Give up this ridge
	    ridges[index].incidencesToFind--;
	}
    }

    imageFree(dt);
    return 1;
}