/*! \file diderot.c
 *
 * \author John Reppy
 */

/*
 * COPYRIGHT (c) 2010 The Diderot Project (http://diderot-language.cs.uchicago.edu)
 * All rights reserved.
 */

#include "Diderot/diderot.h"
#include <teem/nrrd.h>


static void PrintMat2x2 (const char *title, Diderot_Mat2x2_t m)
{
#define M(i)	m[(i)/2].r[(i)%2]
    printf ("%s @ %p:\n", title, (void *)m);
    for (int i = 0;  i < 2;  i++) {
	int j = 2*i;
	printf ("  [ %6f  %6f ]\n", M(j+0), M(j+1));
    }
#undef M

}

static void PrintMat3x3 (const char *title, Diderot_Mat3x3_t m)
{
#define M(i)	m[(i)/3].r[(i)%3]
    printf ("%s @ %p:\n", title, (void *)m);
    for (int i = 0;  i < 3;  i++) {
	int j = 3*i;
	printf ("  [ %6f  %6f  %6f ]\n", M(j+0), M(j+1), M(j+2));
    }
#undef M

}

static void PrintMat4x4 (const char *title, Diderot_Mat4x4_t m)
{
#define M(i)	m[(i)>>2].r[(i)&3]
    printf ("%s @ %p:\n", title, (void *)m);
    for (int i = 0;  i < 4;  i++) {
	int j = 4*i;
	printf ("  [ %6f  %6f  %6f  %6f ]\n", M(j+0), M(j+1), M(j+2), M(j+3));
    }
#undef M

}

/* Loading transfomation information from Nrrd files */

static void LoadTransform1D (Nrrd *nin, float *s, float *t) 
{ 
#define M(i,j)	m[i].r[j]

  // compute the offset to the first space axis
    int base = nin->dim - nin->spaceDim;

  // Image axis Scaling and Rotation
    *s = nin->axis[base+0].spaceDirection[0];

  // Image location
    *t = nin->spaceOrigin[0];

#undef M
}

static void LoadTransform2D (Nrrd *nin, Diderot_Mat2x2_t m, vec2f_t *t) 
{ 
#define M(i,j)	m[i].r[j]

  // compute the offset to the first space axis
    int base = nin->dim - nin->spaceDim;

  // Image axis Scaling and Rotation
    M(0,0) = nin->axis[base+0].spaceDirection[0];
    M(1,0) = nin->axis[base+0].spaceDirection[1];
    M(0,1) = nin->axis[base+1].spaceDirection[0];
    M(1,1) = nin->axis[base+1].spaceDirection[1];

  // Image location
    *t = vec2f(
	nin->spaceOrigin[0],
	nin->spaceOrigin[1]);

#undef M
}

static void LoadTransform3D (Nrrd *nin, Diderot_Mat3x3_t m, vec3f_t *t) 
{ 
#define M(i,j)	m[i].r[j]

  // compute the offset to the first space axis
    int base = nin->dim - nin->spaceDim;

  // Image axis Scaling and Rotation
    M(0,0) = nin->axis[base+0].spaceDirection[0];
    M(1,0) = nin->axis[base+0].spaceDirection[1];
    M(2,0) = nin->axis[base+0].spaceDirection[2];
    M(0,1) = nin->axis[base+1].spaceDirection[0];
    M(1,1) = nin->axis[base+1].spaceDirection[1];
    M(2,1) = nin->axis[base+1].spaceDirection[2];
    M(0,2) = nin->axis[base+2].spaceDirection[0];
    M(1,2) = nin->axis[base+2].spaceDirection[1];
    M(2,2) = nin->axis[base+2].spaceDirection[2];

  // Image location
    *t = vec3f(
	nin->spaceOrigin[0],
	nin->spaceOrigin[1],
	nin->spaceOrigin[2]);

#undef M
}


/* Transformation matrix operations */

STATIC_INLINE Diderot_real_t Det2 (
    Diderot_real_t a, Diderot_real_t b,
    Diderot_real_t c, Diderot_real_t d) 
{
    return (a*d - b*c); 
}

STATIC_INLINE Diderot_real_t Det3 (
    Diderot_real_t a, Diderot_real_t b, Diderot_real_t c,
    Diderot_real_t d, Diderot_real_t e, Diderot_real_t f,
    Diderot_real_t g, Diderot_real_t h, Diderot_real_t i) 
{
    return (a*e*i 
	  + d*h*c 
	  + g*b*f 
	  - g*e*c 
	  - d*b*i 
	  - a*h*f); 
}

static Diderot_real_t DetM3x3 (Diderot_Mat3x3_t m) 
{
#define M(i)	m[(i)/3].r[(i)%3]
    return Det3(M(0), M(1), M(2), 
		M(3), M(4), M(5), 
		M(6), M(7), M(8));
#undef M
}

static Diderot_real_t DetM4x4 (Diderot_Mat4x4_t m) 
{
#define M(i)	m[(i)>>2].r[(i)&3]
	return	(M( 0) * Det3(M( 5), M( 6), M( 7), 
			      M( 9), M(10), M(11), 
			      M(13), M(14), M(15)) 						   
	       - M( 1) * Det3(M( 4), M( 6), M( 7), 
			      M( 8), M(10), M(11), 
			      M(12), M(14), M(15)) 
	       + M( 2) * Det3(M( 4), M( 5), M( 7), 
			      M( 8), M( 9), M(11), 
			      M(12), M(13), M(15)) 						   
	       - M( 3) * Det3(M( 4), M( 5), M( 6), 
			      M( 8), M( 9), M(10), 
			      M(12), M(13), M(14))); 
#undef M

}

/*! \brief compute the inverse of \arg m, storing the result in \arg i.
 *  \param m the matrix to invert
 *  \param i the inverted matrix
 */
static void InvertM2x2 (Diderot_Mat2x2_t i, Diderot_Mat2x2_t m) 
{  
#define M(i)	m[(i)/2].r[(i)%2]
#define I(j)	i[(j)/2].r[(j)%2]
	Diderot_real_t scale = 1.0 / Det2(M(0), M(1), M(2), M(3));

	I(0) =  scale * M(3);
	I(1) = -scale * M(1);
	I(2) = -scale * M(2);
	I(3) =  scale * M(0);
#undef M
#undef I
} 

/*! \brief compute the inverse of \arg m, storing the result in \arg i.
 *  \param m the matrix to invert
 *  \param i the inverted matrix
 */
static void InvertM3x3 (Diderot_Mat3x3_t i, Diderot_Mat3x3_t m) 
{  
	Diderot_real_t scale = 1.0 / DetM3x3(m);

#define M(i)	m[(i)/3].r[(i)%3]
#define I(j)	i[(j)/3].r[(j)%3]
	I(0) = scale * Det2 (M(4), M(5), M(7), M(8));
	I(1) = scale * Det2 (M(2), M(1), M(8), M(7));
	I(2) = scale * Det2 (M(1), M(2), M(4), M(5));
	I(3) = scale * Det2 (M(5), M(3), M(8), M(6));
	I(4) = scale * Det2 (M(0), M(2), M(6), M(8));
	I(5) = scale * Det2 (M(2), M(0), M(5), M(3));
	I(6) = scale * Det2 (M(3), M(4), M(6), M(7));
	I(7) = scale * Det2 (M(1), M(0), M(7), M(6));
	I(8) = scale * Det2 (M(0), M(1), M(3), M(4));
#undef M
#undef I
}

/*! \brief compute the inverse of \arg m, storing the result in \arg i.
 *  \param m the matrix to invert
 *  \param i the inverted matrix
 */
static void InvertM4x4 (Diderot_Mat4x4_t i, Diderot_Mat4x4_t m) 
{  
	Diderot_real_t scale = 1.0 / DetM4x4(m);

#define M(i)	m[(i)>>2].r[(i)&3]
#define I(j)	i[(j)>>2].r[(j)&3]
	I( 0) =  Det3(M( 5),M( 6),M( 7),
		      M( 9),M(10),M(11),
		      M(13),M(14),M(15)) * scale;
	
	I( 1) = -Det3(M( 1),M( 2),M( 3),
		      M( 9),M(10),M(11),
		      M(13),M(14),M(15)) * scale;
					   
	I( 2) =  Det3(M( 1),M( 2),M( 3),
		      M( 5),M( 6),M( 7),
		      M(13),M(14),M(15)) * scale;
					   
	I( 3) = -Det3(M( 1),M( 2),M( 3),
		      M( 5),M( 6),M( 7),
		      M( 9),M(10),M(11)) * scale;
					   
	I( 4) = -Det3(M( 4),M( 6),M( 7),
		      M( 8),M(10),M(11),
		      M(12),M(14),M(15)) * scale;
					   
	I( 5) =  Det3(M( 0),M( 2),M( 3),
		      M( 8),M(10),M(11),
		      M(12),M(14),M(15)) * scale;
					   
	I( 6) = -Det3(M( 0),M( 2),M( 3),
		      M( 4),M( 6),M( 7),
		      M(12),M(14),M(15)) * scale;
					   
	I( 7) =  Det3(M( 0),M( 2),M( 3),
		      M( 4),M( 6),M( 7),
		      M( 8),M(10),M(11)) * scale;
					   
	I( 8) =  Det3(M( 4),M( 5),M( 7),
		      M( 8),M( 9),M(11),
		      M(12),M(13),M(15)) * scale;
					   
	I( 9) = -Det3(M( 0),M( 1),M( 3),
		      M( 8),M( 9),M(11),
		      M(12),M(13),M(15)) * scale;
					   
	I(10) =  Det3(M( 0),M( 1),M( 3),
		      M( 4),M( 5),M( 7),
		      M(12),M(13),M(15)) * scale;
					   
	I(11) = -Det3(M( 0),M( 1),M( 3),
		      M( 4),M( 5),M( 7),
		      M( 8),M( 9),M(11)) * scale;
					   
	I(12) = -Det3(M( 4),M( 5),M( 6),
		      M( 8),M( 9),M(10),
		      M(12),M(13),M(14)) * scale;
					   
	I(13) =  Det3(M( 0),M( 1),M( 2),
		      M( 8),M( 9),M(10),
		      M(12),M(13),M(14)) * scale;
					   
	I(14) = -Det3(M( 0),M( 1),M( 2),
		      M( 4),M( 5),M( 6),
		      M(12),M(13),M(14)) * scale;
					   
	I(15) =  Det3(M( 0),M( 1),M( 2),
		      M( 4),M( 5),M( 6),
		      M( 8),M( 9),M(10)) * scale;
#undef M
#undef I
} 

static Nrrd *LoadNrrdFile (const char *filename) 
{
  /* create a nrrd; at this point this is just an empty container */
    Nrrd *nin = nrrdNew();

  /* read in the nrrd from file */
    if (nrrdLoad(nin, filename, NULL)) {
	char *err = biffGetDone(NRRD);
    	fprintf (stderr, "Diderot: trouble reading \"%s\":\n%s", filename, err);
    	free (err);
	return NULL;
    }
         
    return nin; 
  
}

STATIC_INLINE bool CheckAxis (Nrrd *nin, int i)
{
    return (nin->axis[i].kind != nrrdKindSpace);
}

/* load image data from Nrrd files */
Status_t Diderot_LoadImage1D (Diderot_string_t name, Diderot_image1D_t **imgOut)
{
    Nrrd *nin = LoadNrrdFile (name);
    Diderot_image1D_t *img = (Diderot_image1D_t *)malloc(sizeof(Diderot_image1D_t));

  // compute the offset to the first space axis
    int base = nin->dim - nin->spaceDim;

// FIXME: we should also be checking the size of the axes, the shape of the voxels,
// and the type of the samples.
  // check that the image format is what we expect
    if (nin->spaceDim != 1) {
	fprintf(stderr, "nrrd file \"%s\" has unexpected dimension %d, expected 1\n",
	    name, nin->spaceDim);
	return DIDEROT_FAIL;
    }
    else if (CheckAxis(nin, base)) {
	fprintf(stderr,
	    "nrrd file \"%s\" has unexpected axis structure: %s\n",
	    name,
	    airEnumStr(nrrdKind, nin->axis[base].kind));
	return DIDEROT_FAIL;
    }

    img->dim = 1;
    img->size[0] = nin->axis[base+0].size;
    img->data = nin->data;

printf("LoadImage \"%s\": space dim = 1, axes = <%d>\n",
name, img->size[base+0]);

  // from the Nrrd file, we load the scaling and translation
    float s, t;
    LoadTransform1D (nin, &s, &t);

    img->s = 1.0 / s;
    img->t = -t / s;

    *imgOut = img;
    return DIDEROT_OK;
}

Status_t Diderot_LoadImage2D (Diderot_string_t name, Diderot_image2D_t **imgOut)
{
    Nrrd *nin = LoadNrrdFile (name);
    Diderot_image2D_t *img = (Diderot_image2D_t *)malloc(sizeof(Diderot_image2D_t));

  // compute the offset to the first space axis
    int base = nin->dim - nin->spaceDim;

  // check that the image format is what we expect
    if (nin->spaceDim != 2) {
	fprintf(stderr, "nrrd file \"%s\" has unexpected dimension %d, expected 2\n",
	    name, nin->spaceDim);
	return DIDEROT_FAIL;
    }
    else if (CheckAxis(nin, base) || CheckAxis(nin, base+1)) {
	fprintf(stderr,
	    "nrrd file \"%s\" has unexpected axis structure: %s %s\n",
	    name,
	    airEnumStr(nrrdKind, nin->axis[base].kind),
	    airEnumStr(nrrdKind, nin->axis[base+1].kind));
	return DIDEROT_FAIL;
    }

    img->dim = 2;
    img->size[0] = nin->axis[base+0].size;
    img->size[1] = nin->axis[base+1].size;
    img->data = nin->data;

printf("LoadImage \"%s\": space dim = 2, axes = <%d, %d>\n",
name, img->size[0], img->size[1]);

  // from the Nrrd file, we load the affine image-to-world transform matrix
    Diderot_Mat2x2_t m;		// rotation and scaling
    vec2f_t t;			// translation part of the transform
    LoadTransform2D (nin, m, &t);

PrintMat2x2("m", m);
printf("t = <%f, %f>\n", ((union2f_t)t).r[0], ((union2f_t)t).r[1]);

  // compute inverse of m, which is the transform from the world basis to the image basis
    InvertM2x2 (img->w2i, m);
    img->tVec = -mulMat2x2Vec2f(img->w2i, t);  // inv([M t]) = [inv(M) -inv(M)t]
PrintMat2x2("w2i", img->w2i);
printf("tVec = <%f, %f>\n", ((union2f_t)img->tVec).r[0], ((union2f_t)img->tVec).r[1]);

  // compute transpose of w2i
    transpose2x2f (img->w2iT, img->w2i);

    *imgOut = img;
    return DIDEROT_OK;
}

Status_t Diderot_LoadImage3D (Diderot_string_t name, Diderot_image3D_t **imgOut)
{
    Nrrd *nin = LoadNrrdFile (name);

  // compute the offset to the first space axis
    int base = nin->dim - nin->spaceDim;

  // check that the image format is what we expect
    if (nin->spaceDim != 3) {
	fprintf(stderr, "nrrd file \"%s\" has unexpected dimension %d, expected 3\n",
	    name, nin->spaceDim);
	return DIDEROT_FAIL;
    }
    else if (CheckAxis(nin, base) || CheckAxis(nin, base+1) || CheckAxis(nin, base+2)) {
	fprintf(stderr,
	    "nrrd file \"%s\" has unexpected axis structure: %s %s %s\n",
	    name,
	    airEnumStr(nrrdKind, nin->axis[base].kind),
	    airEnumStr(nrrdKind, nin->axis[base+1].kind),
	    airEnumStr(nrrdKind, nin->axis[base+2].kind));
	return DIDEROT_FAIL;
    }

    Diderot_image3D_t *img = (Diderot_image3D_t *)malloc(sizeof(Diderot_image3D_t));

    img->dim = 3;
    img->size[0] = nin->axis[base+0].size;
    img->size[1] = nin->axis[base+1].size;
    img->size[2] = nin->axis[base+2].size;
    img->data = nin->data;

printf("LoadImage \"%s\": space dim = 3, axes = <%d, %d, %d>\n",
name, img->size[0], img->size[1], img->size[2]);

  // from the Nrrd file, we load the affine image-to-world transform matrix
    Diderot_Mat3x3_t m;		// rotation and scaling
    vec3f_t t;			// translation part of the transform
    LoadTransform3D (nin, m, &t);

PrintMat3x3("m", m);
printf("t = <%f, %f, %f>\n", ((union3f_t)t).r[0], ((union3f_t)t).r[1], ((union3f_t)t).r[2]);

  // compute inverse of m, which is the transform from the world basis to the image basis
    InvertM3x3 (img->w2i, m);
    img->tVec = -mulMat3x3Vec3f(img->w2i, t);  // inv([M t]) = [inv(M) -inv(M)t]
PrintMat3x3("w2i", img->w2i);
printf("tVec = <%f, %f, %f>\n", ((union3f_t)img->tVec).r[0], ((union3f_t)img->tVec).r[1], ((union3f_t)img->tVec).r[2]);

  // compute transpose of w2i
    transpose3x3f (img->w2iT, img->w2i);

    *imgOut = img;
    return DIDEROT_OK;
}

/* functions to get input-parameter values */
Status_t Diderot_InputString (const char *name, const char **v, bool hasDflt)
{
    return DIDEROT_FAIL;
}

Status_t Diderot_Inputf (const char *name, float *v, bool hasDflt)
{
    char	buf[256];
    float	f;

    while (true) {
	if (hasDflt)
	    printf("Enter value for %s (default %f): ", name, *v);
	else
	    printf("Enter value for %s: ", name);
	fflush (stdout);

	char *p = fgets(buf, sizeof(buf), stdin);
	if (p == NULL)
	    return DIDEROT_FAIL;	// EOF
	int n = sscanf(buf, "%f\n", &f);
	if (n == 1) {
	    *v = f;
	    return DIDEROT_OK;;
	}
	else if (hasDflt)
	    return DIDEROT_OK;;
    }

}

Status_t Diderot_InputVec3f (const char *name, vec3f_t *v, bool hasDflt)
{
    char	buf[256];
    float	f1, f2, f3;

    while (true) {
	if (hasDflt) {
	    union3f_t u;
	    u.v = *v;
	    printf("Enter value for %s (default %f %f %f): ",
		name, u.r[0], u.r[1], u.r[2]);
	}
	else
	    printf("Enter value for %s: ", name);
	fflush (stdout);

	char *p = fgets(buf, sizeof(buf), stdin);
	if (p == NULL)
	    return DIDEROT_FAIL;	// EOF
	int n = sscanf(buf, "%f %f %f\n", &f1, &f2, &f3);
	if (n == 3) {
	    *v = vec3f(f1, f2, f3);
	    return DIDEROT_OK;;
	}
	else if (hasDflt)
	    return DIDEROT_OK;;
    }

}