// * Last edited: Feb 10 13:57 1997 (sw)


#include <iostream.h>
#include <iomanip.h>
#include <strstream.h>
#include <fstream.h>
#include <builtin.h>
#include <assert.h>
#include <X11/Xlib.h>

#include "etc.hh"
#include "mat-defs.hh"
#include "quaternion.hh"
#include "pose.hh"

#include "image-defs.hh"
#include "volume-defs.hh"
#include "win.hh"
#include "volume-disp.hh"
#include "perspective.hh"
#include "pn-list.hh"

#include "reg-rsna.h"
#include "aux.hh"


// Some facilities for conversion between pose_3d's and 
// graphics style homogenous coordinates.

// (graphics style) transforms are represented as float*
// that points to sixteen numbers representing the
// homogenous transformation.  
//  
// The conceptual layout of the homogeneous transformation is:
//
//                     R    T
//                     000  1
//
// where R is a 3x3 rotation matrix, T is a 3x1 column vector
// representing the displacement and the bottom row contains
// three zeroes and a one.  The layout of the data structure
// is row major.

// This incorporates stolen code for rotation matrix / quaternion conversion.
// Hope it works!!

void pose_to_transform(pose_3d& the_pose, float *transform)
{
    real m00, m01, m02, m10, m11, m12, m20, m21, m22;
    real r;
    quaternion& q = the_pose.rotation;

    // sw: in my opinion, mii have their indices reversed
    m00 = m11 = m22  =  q[0] * q[0] ;
    
    m00 += (r = q[1]*q[1]);
    m11 -= r;
    m22 -= r;
    
    m11 += (r = q[2]*q[2]);
    m00 -= r;
    m22 -= r;
    
    m22 += (r = q[3]*q[3]);
    m00 -= r;
    m11 -= r;
    
    m10 = m01 = q[1]*q[2];
    m20 = m02 = q[1]*q[3];
    m12 = m21 = q[2]*q[3];
    
    m01 += (r = q[0]*q[3]);
    m10 -= r;
    
    m20 += (r = q[0]*q[2]);
    m02 -= r;
    
    m12 += (r = q[0]*q[1]);
    m21 -= r;
    
    m01 *= TWO;
    m02 *= TWO;
    m12 *= TWO;
    m10 *= TWO;
    m20 *= TWO;
    m21 *= TWO;


    transform[0]  = m00;
    transform[1]  = m10;
    transform[2]  = m20;
    transform[3]  = the_pose.translation[0];

    transform[4]  = m01;
    transform[5]  = m11;
    transform[6]  = m21;
    transform[7]  = the_pose.translation[1];

    transform[8]  = m02;
    transform[9]  = m12;
    transform[10] = m22;
    transform[11] = the_pose.translation[2];

    transform[12] = ZERO;  
    transform[13] = ZERO; 
    transform[14] = ZERO;  
    transform[15] = ONE;
}




pose_3d pose_from_transform(float *transform)
{
    
    quaternion q;
    vec_3 T;

    // sw: in my opinion, mii have their indices reversed
    real m00 = transform[0];
    real m10 = transform[1];
    real m20 = transform[2];
    T[0]     = transform[3];

    real m01 = transform[4];
    real m11 = transform[5];
    real m21 = transform[6];
    T[1]     = transform[7];

    real m02 = transform[8];
    real m12 = transform[9];
    real m22 = transform[10];
    T[2]     = transform[11];

    real rho;
    
    rho = ONE_QUARTER * (ONE + m00 + m11 + m22);
    if (rho)
      { q[0] = sqrt (rho);
	rho = ONE_QUARTER / q[0];
	q[1] = rho * (m12 - m21);
	q[2] = rho * (m20 - m02);
	q[3] = rho * (m01 - m10);
	
	return pose_3d(T, q);
    }
    
    q[0] = ZERO;
    rho = ONE_QUARTER * (ONE + m00 - m11 + m22);
    if (rho)
      { q[1] = sqrt (rho);
	rho = ONE_HALF / q[1];
	q[2] = rho * m01;
	q[3] = rho * m20;
	
	return pose_3d(T, q);
    }
    
    q[1] = ZERO;
    rho = ONE_QUARTER * (ONE - m00 + m11 - m22);
    if (rho)
      { q[2] = sqrt (rho);
	q[3] = ONE_HALF * m21 / q[2];
	
	return pose_3d(T, q);
    }
    
    q[2] = ZERO;
    q[3] = ONE_HALF * sqrt (ONE - m00 - m11 + m22);
    
    return pose_3d(T, q);
}
 
 

///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// Test scaffolding from here down...
#if 0


// For testing...

vec_3 
apply_transform(float *tr, vec_3 v)
{
    mat_33 r;
    vec_3 d;

    r(0,0) = tr[0];
    r(0,1) = tr[1];
    r(0,2) = tr[2];
    d[0]   = tr[3];

    r(1,0) = tr[4];
    r(1,1) = tr[5];
    r(1,2) = tr[6];
    d[1]   = tr[7];

    r(2,0) = tr[8];
    r(2,1) = tr[9];
    r(2,2) = tr[10];
    d[2]   = tr[11];

    return r * v + d;
}




void
pose_to_transform2(pose_3d& pose, float *tr)
{
    vec_3 rx = rotate(x_hat, pose.rotation);
    vec_3 ry = rotate(y_hat, pose.rotation);
    vec_3 rz = rotate(z_hat, pose.rotation);

    vec_3 d = pose.translation;

    tr[0] = rx[0];
    tr[1] = ry[0];
    tr[2] = rz[0];
    tr[3] =  d[0];

    tr[4] = rx[1];
    tr[5] = ry[1];
    tr[6] = rz[1];
    tr[7] =  d[1];

    tr[8]  = rx[2];
    tr[9]  = ry[2];
    tr[10] = rz[2];
    tr[11] =  d[2];

    tr[12] = ZERO;
    tr[13] = ZERO;
    tr[14] = ZERO;
    tr[15] =  ONE;
}






#ifndef sgi
// A little help with random numbers.
extern "C" int random(void);
#endif


main()
{
    for(int i = 0; i < 100; i++) {
	// make a random test pose...
	real angle = PI + random(PI);
	vec_3 direction = normalize(colvec(random(), random(), random()));
	vec_3 d = colvec(random((real)1000.0), random((real)1000.0), 
			 random((real)1000.0));
	quaternion qrot = rotation_quaternion(direction, angle);
	pose_3d pose_1 = pose_3d(d, qrot);

	// make a random test vector
	vec_3 tvec = colvec(random((real)1000.0), random((real)1000.0), 
			 random((real)1000.0));
	


	// check the consistency of the conversion:
	float transform[16];
	pose_to_transform(pose_1, transform);
	pose_3d pose_2 = pose_from_transform(transform);

	// pose_1 and pose_2 should agree if the conversion code
	// is consistent in both directions
	// so p1 and p2 should agree...
	vec_3 p1 = pose_1 * tvec;
	vec_3 p2 = pose_2 * tvec;
	DDT(length(p1 - p2));	// should be zero


	// check that we agree on the interpretation of the
	// transforms
	float other_tr[16];
	pose_to_transform2(pose_1, other_tr);

	{
	    real err = ZERO;
	    for (int i = 0; i < 16; i++) {
	      real diff = transform[i] - other_tr[i];
	      err += diff * diff;
	    }
	    DDT(err);		// should be zero
	}


	// Double check interpretation by comparing our idea
	// transformations and poses...

	vec_3 res1 = pose_1 * tvec;
	vec_3 res2 = apply_transform(transform, tvec);
	DDT( length(res1 - res2));	// should be zero



	cout << "\n";
    }
// * Last edited: May 14 14:42 1996 (sw)
}
#endif
