#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include "gsl/gsl_vector.h"
#include "gsl/gsl_multiroots.h"
#include "gsl/gsl_permutation.h"
#include "gsl/gsl_linalg.h"

/* #undef USE_DERIVATIVES */
/* #define SOLVER gsl_multiroot_fsolver_hybrids */
/* #define SOLVER gsl_multiroot_fsolver_hybrid */
/* #define SOLVER gsl_multiroot_fsolver_dnewton */
#define USE_DERIVATIVES
/* #define SOLVER gsl_multiroot_fdfsolver_hybridsj */
/* #define SOLVER gsl_multiroot_fdfsolver_hybridj */
/* #define SOLVER gsl_multiroot_fdfsolver_newton */
#define SOLVER gsl_multiroot_fdfsolver_gnewton

#define CONSTANT INT_MAX

/**
 * trigrid_data -- parameters and results for a triangular grid fitting
 */

struct trigrid_data {
	/* parameters */
	unsigned int X, Y;
	double * A;

	/* results */
	int status;
	unsigned int iter;
#ifdef USE_DERIVATIVES
	gsl_multiroot_fdfsolver * s;
#else
	gsl_multiroot_fsolver * s;
#endif
	unsigned int xs_start, ys_start;
	gsl_vector * xs;
};


void trigrid_fit(struct trigrid_data * data);
int trigrid_f(const gsl_vector * xs, void * params, gsl_vector * f);
int trigrid_df(const gsl_vector * xs, void * params, gsl_matrix * J);
int trigrid_fdf(const gsl_vector * xs, void * params, gsl_vector * f, gsl_matrix * J);
double trigrid_get_x(const struct trigrid_data * data, const gsl_vector * xs,
		unsigned int x, unsigned int y);
double trigrid_get_y(const struct trigrid_data * data, const gsl_vector * xs,
		unsigned int x, unsigned int y);
unsigned int trigrid_offset_x(const struct trigrid_data * data,
		unsigned int x, unsigned int y);
unsigned int trigrid_offset_y(const struct trigrid_data * data,
		unsigned int x, unsigned int y);
int main(void);
void trigrid_print(struct trigrid_data * data);


/**
 * trigrid_fit -- create a triangular grid with triangle areas A
 *
 * p	struct trigrid_data with the following fields filled in:
 *	X	number of pairs of triangles in a row
 *	Y	number of rows of triangles
 *	A	target areas from bottom left by row, 2 * X * Y values
 *
 * p is updated with the result, which can be conveniently accessed
 * using trigrid_get_x and trigrid_get_y
 */

void trigrid_fit(struct trigrid_data * data)
{
	unsigned int X = data->X, Y = data->Y;
	const size_t n = X * Y * 2;
	unsigned int x, y, i;

	int status;
	size_t iter = 0;


#ifdef USE_DERIVATIVES
	gsl_multiroot_function_fdf f = {&trigrid_f, &trigrid_df, &trigrid_fdf, n, data};
#else
	gsl_multiroot_function f = {&trigrid_f, n, data};
#endif

	gsl_vector * xs = gsl_vector_alloc(n);
	data->xs = xs;

	i = 0;
	data->xs_start = i;
	for (y = 0; y != Y + 1; y++)
		for (x = 1; x != X; x++)
			gsl_vector_set(xs, i++, x + 0.1 * ((double) rand() / RAND_MAX - 0.5));
	gsl_vector_set(xs, i++, X + 0.1 * ((double) rand() / RAND_MAX - 0.5));
	data->ys_start = i;
	for (x = 0; x != X + 1; x++)
		for (y = 1; y != Y; y++)
			gsl_vector_set(xs, i++, y + 0.1 * ((double) rand() / RAND_MAX - 0.5));
	gsl_vector_set(xs, i++, Y + 0.1 * ((double) rand() / RAND_MAX - 0.5));
	assert(i == n);

#ifdef USE_DERIVATIVES
	data->s = gsl_multiroot_fdfsolver_alloc(SOLVER, n);
	gsl_multiroot_fdfsolver_set(data->s, &f, xs);
#else
	data->s = gsl_multiroot_fsolver_alloc(SOLVER, n);
	gsl_multiroot_fsolver_set(data->s, &f, xs);
#endif

/* 	trigrid_print(data); */

	do {
		iter++;
		fprintf(stderr, "iter = %3u\n", iter);

#ifdef USE_DERIVATIVES
		status = gsl_multiroot_fdfsolver_iterate(data->s);
#else
		status = gsl_multiroot_fsolver_iterate(data->s);
#endif

/* 		trigrid_print(data); */

		if (status)
			break;

		status = gsl_multiroot_test_residual(data->s->f, 0.01);
	} while (status == GSL_CONTINUE && iter != 1000);

	fprintf(stderr, "status = %s\n", gsl_strerror(status));

	data->status = status;
	data->iter = iter;
}


/**
 * trigrid_f -- function evaluator for triangular grid fitter
 */

int trigrid_f(const gsl_vector * xs, void * params, gsl_vector * f)
{
	unsigned int x, y, X, Y, i;
	struct trigrid_data * data = (struct trigrid_data *) params;

	X = data->X;
	Y = data->Y;

	for (i = 0, y = 0; y != Y; y++) {
		for (x = 0; x != X; x++) {
			double A, v;
			double ax, ay, bx, by, cx, cy, dx, dy;

			ax = trigrid_get_x(data, xs, x, y);
			ay = trigrid_get_y(data, xs, x, y);
			bx = trigrid_get_x(data, xs, x + 1, y + 1);
			by = trigrid_get_y(data, xs, x + 1, y + 1);
			cx = trigrid_get_x(data, xs, x, y + 1);
			cy = trigrid_get_y(data, xs, x, y + 1);
			dx = trigrid_get_x(data, xs, x + 1, y);
			dy = trigrid_get_y(data, xs, x + 1, y);

/* 			printf("x = %u, y = %u, a = %g %g, b = %g %g, c = %g %g, d = %g %g\n", */
/* 					x, y, ax, ay, bx, by, cx, cy, dx, dy); */

			/* upper triangle */
			A = data->A[i];
			v = 0.5 * (ax * (by - cy) + bx * (cy - ay) + cx * (ay - by));
/* 			printf("upper A = %g, v = %g\n", A, v); */

			gsl_vector_set(f, i++, v - A);

			/* lower triangle */
			A = data->A[i];
			v = 0.5 * (ax * (dy - by) + dx * (by - ay) + bx * (ay - dy));
/* 			printf("lower A = %g, v = %g\n\n", A, v); */

			gsl_vector_set(f, i++, v - A);
		}
	}

	return GSL_SUCCESS;
}


/**
 * trigrid_df -- function derivative evaluator for triangular grid fitter
 */

int trigrid_df(const gsl_vector * xs, void * params, gsl_matrix * J)
{
	unsigned int x, y, X, Y, i;
	struct trigrid_data * data = (struct trigrid_data *) params;

	X = data->X;
	Y = data->Y;
	gsl_matrix_set_zero(J);

/* 	printf("xs =\n"); */
/* 	gsl_vector_fprintf(stdout, xs, "%g"); */

	for (i = 0, y = 0; y != Y; y++) {
		for (x = 0; x != X; x++) {
			double ax, ay, bx, by, cx, cy, dx, dy;
			unsigned int axo, ayo, bxo, byo, cxo, cyo, dxo, dyo;

			ax = trigrid_get_x(data, xs, x, y);
			ay = trigrid_get_y(data, xs, x, y);
			bx = trigrid_get_x(data, xs, x + 1, y + 1);
			by = trigrid_get_y(data, xs, x + 1, y + 1);
			cx = trigrid_get_x(data, xs, x, y + 1);
			cy = trigrid_get_y(data, xs, x, y + 1);
			dx = trigrid_get_x(data, xs, x + 1, y);
			dy = trigrid_get_y(data, xs, x + 1, y);

			axo = trigrid_offset_x(data, x, y);
			ayo = trigrid_offset_y(data, x, y);
			bxo = trigrid_offset_x(data, x + 1, y + 1);
			byo = trigrid_offset_y(data, x + 1, y + 1);
			cxo = trigrid_offset_x(data, x, y + 1);
			cyo = trigrid_offset_y(data, x, y + 1);
			dxo = trigrid_offset_x(data, x + 1, y);
			dyo = trigrid_offset_y(data, x + 1, y);

/* 			printf("x = %u, y = %u, a = %g %g, b = %g %g, c = %g %g, d = %g %g\n", */
/* 					x, y, ax, ay, bx, by, cx, cy, dx, dy); */

			/* upper triangle */
/* 			v = 0.5 * (ax * (by - cy) + bx * (cy - ay) + cx * (ay - by)); */

			if (axo != CONSTANT) gsl_matrix_set(J, i, axo, 0.5 * (by - cy));
			if (ayo != CONSTANT) gsl_matrix_set(J, i, ayo, 0.5 * (cx - bx));
			if (bxo != CONSTANT) gsl_matrix_set(J, i, bxo, 0.5 * (cy - ay));
			if (byo != CONSTANT) gsl_matrix_set(J, i, byo, 0.5 * (ax - cx));
			if (cxo != CONSTANT) gsl_matrix_set(J, i, cxo, 0.5 * (ay - by));
			if (cyo != CONSTANT) gsl_matrix_set(J, i, cyo, 0.5 * (bx - ax));

			i++;

			/* lower triangle */
/* 			v = 0.5 * (ax * (dy - by) + dx * (by - ay) + bx * (ay - dy)); */

			if (axo != CONSTANT) gsl_matrix_set(J, i, axo, 0.5 * (dy - by));
			if (ayo != CONSTANT) gsl_matrix_set(J, i, ayo, 0.5 * (bx - dx));
			if (dxo != CONSTANT) gsl_matrix_set(J, i, dxo, 0.5 * (by - ay));
			if (dyo != CONSTANT) gsl_matrix_set(J, i, dyo, 0.5 * (ax - bx));
			if (bxo != CONSTANT) gsl_matrix_set(J, i, bxo, 0.5 * (ay - dy));
			if (byo != CONSTANT) gsl_matrix_set(J, i, byo, 0.5 * (dx - ax));

			i++;
                }
        }

/*	printf("J =\n");
	gsl_matrix_fprintf(stdout, J, "%f");
	printf("\n");
*/
	{
		gsl_matrix * JJ = gsl_matrix_alloc(2 * data->X * data->Y, 2 * data->X * data->Y);
		gsl_permutation * p = gsl_permutation_alloc(2 * data->X * data->Y);
		int signum;

		gsl_matrix_memcpy(JJ, J);
		gsl_linalg_LU_decomp(JJ, p, &signum);
		fprintf(stderr, "det = %g\n", gsl_linalg_LU_det(JJ, signum));
	}

	return GSL_SUCCESS;
}


int trigrid_fdf(const gsl_vector * xs, void * params, gsl_vector * f, gsl_matrix * J)
{
	trigrid_f(xs, params, f);
	trigrid_df(xs, params, J);

	return GSL_SUCCESS;
}


/**
 * trigrid_get_[xy] -- extract grid coordinates from a struct trigrid_data
 */

double trigrid_get_x(const struct trigrid_data * data, const gsl_vector * xs,
		 unsigned int x, unsigned int y)
{
	unsigned int i = trigrid_offset_x(data, x, y);
	if (i != CONSTANT)
		return gsl_vector_get(xs, i);
	if (x == 0)
		return 0;
	if (x == data->X)
		return data->X;
	assert(0);
}

unsigned int trigrid_offset_x(const struct trigrid_data * data,
		unsigned int x, unsigned int y)
{
	if ((x == data->X) && (y == data->Y))
		/* top right corner */
		return data->ys_start - 1;
	if (x == 0)
		return CONSTANT;
	if (x == data->X)
		return CONSTANT;
	return data->xs_start + y * (data->X - 1) + x - 1;
}

double trigrid_get_y(const struct trigrid_data * data, const gsl_vector * xs,
		unsigned int x, unsigned int y)
{
	unsigned int i = trigrid_offset_y(data, x, y);
	if (i != CONSTANT)
		return gsl_vector_get(xs, i);
	if (y == 0)
		return 0;
	if (y == data->Y)
		return data->Y;
	assert(0);
}

unsigned int trigrid_offset_y(const struct trigrid_data * data,
		unsigned int x, unsigned int y)
{
	if ((x == data->X) && (y == data->Y))
		/* top right corner */
		return data->ys_start + (data->X + 1) * (data->Y - 1);
	if (y == 0)
		return CONSTANT;
	if (y == data->Y)
		return CONSTANT;
	return data->ys_start + x * (data->Y - 1) + y - 1;
}






int main(void)
{
	unsigned int X = 10, Y = 10;
	double A[] = {
0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5,   0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5,
0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5,   0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5,
0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5,   0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5,
0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.7,0.7,   0.7,0.7, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5,
0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.7,0.7,   0.7,0.7, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5,

0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.7,0.7,   0.7,0.7, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5,
0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.7,0.7,   0.7,0.7, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5,
0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5,   0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5,
0.1,0.1, 0.1,0.1, 0.5,0.5, 0.5,0.5, 0.5,0.5,   0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5,
0.1,0.1, 0.1,0.1, 0.5,0.5, 0.5,0.5, 0.5,0.5,   0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5
	};

	struct trigrid_data data = {X, Y, A, 0, 0, 0, 0, 0, 0};
	trigrid_fit(&data);
	trigrid_print(&data);

	return 0;
}


void trigrid_print(struct trigrid_data * data)
{
	unsigned int x, y, i = 0;
#ifdef USE_DERIVATIVES
	gsl_vector * root = gsl_multiroot_fdfsolver_root(data->s);
#else
	gsl_vector * root = gsl_multiroot_fsolver_root(data->s);
#endif

	fprintf(stderr, "root =\n");
	gsl_vector_fprintf(stderr, root, "%g");
	fprintf(stderr, "\n");

	fprintf(stderr, "f =\n");
	gsl_vector_fprintf(stderr, data->s->f, "%g");
	fprintf(stderr, "\n");

	printf("PROCClosedLines\n\n");
	printf("PROCFont(\"Corpus.Medium\", 5, 80)\n");

	for (y = 0; y != data->Y; y++) {
		for (x = 0; x != data->X; x++) {
			double ax, ay, bx, by, cx, cy, dx, dy;

			ax = trigrid_get_x(data, root, x, y);
			ay = trigrid_get_y(data, root, x, y);
			bx = trigrid_get_x(data, root, x + 1, y + 1);
			by = trigrid_get_y(data, root, x + 1, y + 1);
			cx = trigrid_get_x(data, root, x, y + 1);
			cy = trigrid_get_y(data, root, x, y + 1);
			dx = trigrid_get_x(data, root, x + 1, y);
			dy = trigrid_get_y(data, root, x + 1, y);

/*			printf("%.3f %.3f  ", ax, ay);*/

			printf("PROCGroup\n");
			printf("PROCMove(%g, %g)\n", ax, ay);
			printf("PROCDraw(%g, %g)\n", bx, by);
			printf("PROCDraw(%g, %g)\n", cx, cy);
			printf("PROCPrint(\"%.3f\", %g, %g)\n",
					/*data->A[i] +*/ gsl_vector_get(data->s->f, i),
					(ax + bx + cx) / 3, (ay + by + cy) / 3);
			printf("PROCEndGroup\n\n");
			i++;

			printf("PROCGroup\n");
			printf("PROCMove(%g, %g)\n", ax, ay);
			printf("PROCDraw(%g, %g)\n", dx, dy);
			printf("PROCDraw(%g, %g)\n", bx, by);
			printf("PROCPrint(\"%.3f\", %g, %g)\n",
					/*data->A[i] +*/ gsl_vector_get(data->s->f, i),
					(ax + dx + bx) / 3, (ay + dy + by) / 3);
			printf("PROCEndGroup\n\n");
			i++;
		}
		printf("\n");
	}
	printf("\n");

	printf("PROCPrint(\"'%s' solver, %u iterations, end status '%s'\", 0, %u)\n",
#ifdef USE_DERIVATIVES
			gsl_multiroot_fdfsolver_name(data->s),
#else
			gsl_multiroot_fsolver_name(data->s),
#endif
			data->iter,
			gsl_strerror(data->status),
			data->Y + 1);
}