/*! \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

}

/* 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 m, Diderot_Mat2x2_t i) 
{  
#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 InvertTransposeM2x2 (Diderot_Mat2x2_t m, Diderot_Mat2x2_t i) 
{  
#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(2) = -scale * M(1);
	I(1) = -scale * M(2);
	I(3) =  scale * M(0);
#undef M
#undef I
} 

static void LoadTransformM2x2 (Nrrd *nin, Diderot_Mat2x2_t m) 
{ 
#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];

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

  // bottom row is 0 1
    M(1,0) = 0;
    M(1,1) = 1;

#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 InvertM3x3 (Diderot_Mat3x3_t m, Diderot_Mat3x3_t i) 
{  
	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 transpose of \arg m, storing the result in \arg i.
 *  \param m the matrix to invert
 *  \param i the inverted matrix
 */
static void InvertTransposeM3x3 (Diderot_Mat3x3_t m, Diderot_Mat3x3_t i) 
{  
	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(3) = scale * Det2 (M(2), M(1), M(8), M(7));
	I(6) = scale * Det2 (M(1), M(2), M(4), M(5));
	I(1) = scale * Det2 (M(5), M(3), M(8), M(6));
	I(4) = scale * Det2 (M(0), M(2), M(6), M(8));
	I(7) = scale * Det2 (M(2), M(0), M(5), M(3));
	I(2) = scale * Det2 (M(3), M(4), M(6), M(7));
	I(5) = 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
} 

static void LoadTransformM3x3 (Nrrd *nin, Diderot_Mat3x3_t m) 
{ 
#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
    M(0,2) = nin->spaceOrigin[0];
    M(1,2) = nin->spaceOrigin[1];

  // bottom row is 0 0 1
    M(2,0) = 0;
    M(2,1) = 0;
    M(2,2) = 1;

#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 InvertM4x4 (Diderot_Mat4x4_t m, Diderot_Mat4x4_t i) 
{  
	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
} 

/*! \brief compute the inverse transpose of \arg m, storing the result in \arg i.
 *  \param m the matrix to invert
 *  \param i the inverted matrix
 */
static void InvertTransposeM4x4 (Diderot_Mat4x4_t m, Diderot_Mat4x4_t i) 
{  
	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( 4) = -Det3(M( 1),M( 2),M( 3),
		      M( 9),M(10),M(11),
		      M(13),M(14),M(15)) * scale;
					   
	I( 8) =  Det3(M( 1),M( 2),M( 3),
		      M( 5),M( 6),M( 7),
		      M(13),M(14),M(15)) * scale;
					   
	I(12) = -Det3(M( 1),M( 2),M( 3),
		      M( 5),M( 6),M( 7),
		      M( 9),M(10),M(11)) * scale;
					   
	I( 1) = -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( 9) = -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( 2) =  Det3(M( 4),M( 5),M( 7),
		      M( 8),M( 9),M(11),
		      M(12),M(13),M(15)) * scale;
					   
	I( 6) = -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(14) = -Det3(M( 0),M( 1),M( 3),
		      M( 4),M( 5),M( 7),
		      M( 8),M( 9),M(11)) * scale;
					   
	I( 3) = -Det3(M( 4),M( 5),M( 6),
		      M( 8),M( 9),M(10),
		      M(12),M(13),M(14)) * scale;
					   
	I( 7) =  Det3(M( 0),M( 1),M( 2),
		      M( 8),M( 9),M(10),
		      M(12),M(13),M(14)) * scale;
					   
	I(11) = -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 void LoadTransformM4x4 (Nrrd *nin, Diderot_Mat4x4_t m) 
{ 
#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
    M(0,3) = nin->spaceOrigin[0];
    M(1,3) = nin->spaceOrigin[1];
    M(2,3) = nin->spaceOrigin[2];

  // bottom row is 0 0 0 1
    M(3,0) = 0;
    M(3,1) = 0;
    M(3,2) = 0;
    M(3,3) = 1;

#undef M
}

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; 
  
}

/* 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));

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

printf("LoadImage \"%s\": dim = 1, axes = <%d>\n", name, img->size[0]);
    LoadTransformM2x2 (nin, img->m);
    InvertM2x2 (img->m, img->mInv);
PrintMat2x2("m", img->m);
PrintMat2x2("mInv", img->mInv);

    *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));

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

printf("LoadImage \"%s\": dim = 2, axes = <%d, %d>\n", name, img->size[0], img->size[1]);
    LoadTransformM3x3 (nin, img->m);
    InvertM3x3 (img->m, img->mInv);
    InvertTransposeM3x3 (img->m, img->mInvT);
PrintMat3x3("m", img->m);
PrintMat3x3("mInv", img->mInv);
PrintMat3x3("mInvT", img->mInvT);

    *imgOut = img;
    return DIDEROT_OK;
}

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

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

printf("LoadImage \"%s\": dim = 3, axes = <%d, %d, %d>\n",
name, img->size[0], img->size[1], img->size[2]);
    LoadTransformM4x4 (nin, img->m);
    InvertM4x4 (img->m, img->mInv);
    InvertTransposeM4x4 (img->m, img->mInvT);
PrintMat4x4("m", img->m);
PrintMat4x4("mInv", img->mInv);
PrintMat4x4("mInvT", img->mInvT);

    *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;;
    }

}