/* -----------------------------------------------------------------------
   See COPYRIGHT.TXT and LICENSE.TXT for copyright and license information
   ----------------------------------------------------------------------- */
#include "plmbase_config.h"
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <math.h>
#include "itkImageRegionIterator.h"
#include <stdio.h>
#include <math.h>


#include "bspline_xform.h"
#include "direction_cosines.h"
#include "direction_matrices.h"
#include "itk_directions.h"
#include "plm_image.h"
#include "plm_image_header.h"
#include "print_and_exit.h"
#include "volume.h"
#include "volume_header.h"

void
Plm_image_header::set_dim (const plm_long dim[3])
{
    ImageRegionType::SizeType itk_size;
    ImageRegionType::IndexType itk_index;
    for (unsigned int d = 0; d < 3; d++) {
	itk_index[d] = 0;
	itk_size[d] = dim[d];
    }
    m_region.SetSize (itk_size);
    m_region.SetIndex (itk_index);
}

void
Plm_image_header::set_origin (const float origin[3])
{
    for (unsigned int d = 0; d < 3; d++) {
	this->m_origin[d] = origin[d];
    }
}

void
Plm_image_header::set_spacing (const float spacing[3])
{
    for (unsigned int d = 0; d < 3; d++) {
	this->m_spacing[d] = spacing[d];
    }
}

void
Plm_image_header::set_direction_cosines (const float direction_cosines[9])
{
    if (direction_cosines) {
	itk_direction_from_dc (&m_direction, direction_cosines);
    } else {
	itk_direction_set_identity (&m_direction);
    }
}

void
Plm_image_header::set_direction_cosines (const Direction_cosines& dc)
{
    itk_direction_from_dc (&m_direction, dc);
}

void
Plm_image_header::set (const Plm_image_header& src)
{
    this->m_origin = src.m_origin;
    this->m_spacing = src.m_spacing;
    this->m_region = src.m_region;
    this->m_direction = src.m_direction;
}

void
Plm_image_header::set (
    const plm_long dim[3],
    const float origin[3],
    const float spacing[3],
    const float direction_cosines[9])
{
    this->set_dim (dim);
    this->set_origin (origin);
    this->set_spacing (spacing);
    this->set_direction_cosines (direction_cosines);
}

void
Plm_image_header::set (
    const plm_long dim[3],
    const float origin[3],
    const float spacing[3],
    const Direction_cosines& dc)
{
    this->set (dim, origin, spacing, dc.get_matrix());
}

void
Plm_image_header::set_from_gpuit (
    const plm_long dim[3],
    const float origin[3],
    const float spacing[3],
    const float direction_cosines[9])
{
    this->set (dim, origin, spacing, direction_cosines);
}

void
Plm_image_header::set_from_gpuit_bspline (Bspline_xform *bxf)
{
    this->set (
	bxf->img_dim,
	bxf->img_origin,
	bxf->img_spacing,
	bxf->dc);
}

void
Plm_image_header::set_from_plm_image (const Plm_image *pli)
{
    switch (pli->m_type) {
    case PLM_IMG_TYPE_ITK_UCHAR:
	this->set_from_itk_image (pli->m_itk_uchar);
	break;
    case PLM_IMG_TYPE_ITK_SHORT:
	this->set_from_itk_image (pli->m_itk_short);
	break;
    case PLM_IMG_TYPE_ITK_USHORT:
	this->set_from_itk_image (pli->m_itk_ushort);
	break;
    case PLM_IMG_TYPE_ITK_LONG:
	this->set_from_itk_image (pli->m_itk_int32);
	break;
    case PLM_IMG_TYPE_ITK_ULONG:
	this->set_from_itk_image (pli->m_itk_uint32);
	break;
    case PLM_IMG_TYPE_ITK_FLOAT:
	this->set_from_itk_image (pli->m_itk_float);
	break;
    case PLM_IMG_TYPE_ITK_DOUBLE:
	this->set_from_itk_image (pli->m_itk_double);
	break;
    case PLM_IMG_TYPE_GPUIT_UCHAR:
    case PLM_IMG_TYPE_GPUIT_SHORT:
    case PLM_IMG_TYPE_GPUIT_UINT32:
    case PLM_IMG_TYPE_GPUIT_FLOAT:
    case PLM_IMG_TYPE_GPUIT_FLOAT_FIELD:
    {
	const Volume* vol = pli->get_vol ();
	set_from_gpuit (vol->dim, vol->origin, vol->spacing,
	    vol->direction_cosines);
	break;
    }
    case PLM_IMG_TYPE_ITK_UCHAR_VEC:
	this->set_from_itk_image (pli->m_itk_uchar_vec);
	break;
    case PLM_IMG_TYPE_ITK_FLOAT_FIELD:
    case PLM_IMG_TYPE_ITK_CHAR:
    default:
	print_and_exit ("Unhandled image type (%s) in set_from_plm_image\n",
	    plm_image_type_string (pli->m_type));
	break;
    }
}

void
Plm_image_header::set_from_plm_image (const Plm_image& pli)
{
    this->set_from_plm_image (&pli);
}

void
Plm_image_header::set_from_plm_image (const Plm_image::Pointer& pli)
{
    this->set_from_plm_image (pli.get());
}

void
Plm_image_header::set (const Volume_header& vh)
{
    this->set_from_gpuit (vh.get_dim(), vh.get_origin(), 
	vh.get_spacing(), vh.get_direction_cosines());
}

void
Plm_image_header::set_from_volume_header (const Volume_header& vh)
{
    this->set (vh);
}

void
Plm_image_header::set (const Volume& vol)
{
    this->set_from_gpuit (vol.dim, vol.origin,
	vol.spacing, vol.direction_cosines);
}

void
Plm_image_header::set (const Volume* vol)
{
    this->set_from_gpuit (vol->dim, vol->origin,
	vol->spacing, vol->direction_cosines);
}

void 
Plm_image_header::expand_to_contain (
    const FloatPoint3DType& position)
{
    /* Compute index for this position */
    FloatPoint3DType idx = this->get_index (position);

    /* Get the step & proj matrices */
    /* GCS FIX: This is inefficient, already computed in get_index() */
    float spacing[3], step[9], proj[9];
    Direction_cosines dc (m_direction);
    this->get_spacing (spacing);
    compute_direction_matrices (step, proj, dc, spacing);

    ImageRegionType::SizeType itk_size = m_region.GetSize();

    /* Expand the volume to contain the point */
    for (int d1 = 0; d1 < 3; d1++) {
        if (idx[d1] < 0) {
            float extra = (float) floor ((double) idx[d1]);
            for (int d2 = 0; d2 < 3; d2++) {
                m_origin += extra * step[d1*3+d2];
            }
            itk_size[d1] += -extra;
        }
        else if (idx[d1] > itk_size[d1]) {
            itk_size[d1] = (int) floor ((double) idx[d1]) + 1;
        }
    }
}

void 
Plm_image_header::set_geometry_to_contain (
    const Plm_image_header& reference_pih,
    const Plm_image_header& compare_pih)
{
    /* Initialize to reference image */
    this->set (reference_pih);

    /* Expand to contain all eight corners of compare image */
    FloatPoint3DType pos;
    float idx[3];
    idx[0] = 0;
    idx[1] = 0;
    idx[2] = 0;
    pos = compare_pih.get_position (idx);
    this->expand_to_contain (pos);

    idx[0] = 0;
    idx[1] = 0;
    idx[2] = compare_pih.dim(2) - 1;
    pos = compare_pih.get_position (idx);
    this->expand_to_contain (pos);

    idx[0] = 0;
    idx[1] = compare_pih.dim(1) - 1;
    idx[2] = 0;
    pos = compare_pih.get_position (idx);
    this->expand_to_contain (pos);

    idx[0] = 0;
    idx[1] = compare_pih.dim(1) - 1;
    idx[2] = compare_pih.dim(2) - 1;
    pos = compare_pih.get_position (idx);
    this->expand_to_contain (pos);

    idx[0] = compare_pih.dim(0) - 1;
    idx[1] = 0;
    idx[2] = 0;
    pos = compare_pih.get_position (idx);
    this->expand_to_contain (pos);

    idx[0] = compare_pih.dim(0) - 1;
    idx[1] = 0;
    idx[2] = compare_pih.dim(2) - 1;
    pos = compare_pih.get_position (idx);
    this->expand_to_contain (pos);

    idx[0] = compare_pih.dim(0) - 1;
    idx[1] = compare_pih.dim(1) - 1;
    idx[2] = 0;
    pos = compare_pih.get_position (idx);
    this->expand_to_contain (pos);

    idx[0] = compare_pih.dim(0) - 1;
    idx[1] = compare_pih.dim(1) - 1;
    idx[2] = compare_pih.dim(2) - 1;
    pos = compare_pih.get_position (idx);
    this->expand_to_contain (pos);
}

void
Plm_image_header::get_volume_header (Volume_header *vh) const
{
    this->get_origin (vh->get_origin());
    this->get_dim (vh->get_dim());
    this->get_spacing (vh->get_spacing());
    this->get_direction_cosines (vh->get_direction_cosines());
}

void 
Plm_image_header::get_origin (float origin[3]) const
{
    for (unsigned int d = 0; d < 3; d++) {
	origin[d] = m_origin[d];
    }
}

void 
Plm_image_header::get_spacing (float spacing[3]) const
{
    for (unsigned int d = 0; d < 3; d++) {
	spacing[d] = m_spacing[d];
    }
}

void 
Plm_image_header::get_dim (plm_long dim[3]) const
{
    ImageRegionType::SizeType itk_size = m_region.GetSize ();
    for (unsigned int d = 0; d < 3; d++) {
	dim[d] = itk_size[d];
    }
}

void 
Plm_image_header::get_direction_cosines (float direction_cosines[9]) const
{
    dc_from_itk_direction (direction_cosines, &m_direction);
}

void
Plm_image_header::print (void) const
{
    ImageRegionType::SizeType itk_size;
    itk_size = m_region.GetSize ();
    float dc[9];
    this->get_direction_cosines (dc);

    printf ("Origin =");
    for (unsigned int d = 0; d < 3; d++) {
	printf (" %g", m_origin[d]);
    }
    printf ("\nSize =");
    for (unsigned int d = 0; d < 3; d++) {
	printf (" %lu", itk_size[d]);
    }
    printf ("\nSpacing =");
    for (unsigned int d = 0; d < 3; d++) {
	printf (" %g", m_spacing[d]);
    }
    printf ("\nDirection =");
    for (unsigned int d1 = 0; d1 < 3; d1++) {
	for (unsigned int d2 = 0; d2 < 3; d2++) {
	    printf (" %g", dc[d1*3+d2]);
	}
    }

    printf ("\n");
}

FloatPoint3DType
Plm_image_header::get_index (const FloatPoint3DType& pos) const
{
    FloatPoint3DType idx;
    FloatPoint3DType tmp;

    float spacing[3], step[9], proj[9];
    Direction_cosines dc (m_direction);
    this->get_spacing (spacing);

    compute_direction_matrices (step, proj, dc, spacing);

    for (int d1 = 0; d1 < 3; d1++) {
        tmp[d1] = pos[d1] - m_origin[d1];
        idx[d1] = 0;
        for (int d2 = 0; d2 < 3; d2++) {
            idx[d1] += pos[d2] * proj[d1*3+d2];
        }
    }

    return idx;
}

FloatPoint3DType
Plm_image_header::get_position (const float index[3]) const
{
    FloatPoint3DType pos;

    for (int d = 0; d < 3; d++) {
        pos[d] = 0.f;
        for (int dc = 0; dc < 3; dc++) {
            pos[d] += m_spacing[d] * index[dc] * m_direction[dc][d];
        }
    }
    return pos;
}

void 
Plm_image_header::get_image_center (float center[3]) const
{
    int d;
    for (d = 0; d < 3; d++) {
	center[d] = this->m_origin[d] 
	    + this->m_spacing[d] * (this->Size(d) - 1) / 2;
    }
}

plm_long
Plm_image_header::get_num_voxels (void) const
{
    return this->Size(0) * this->Size(1) * this->Size(2);
}

void 
Plm_image_header::get_image_extent (float extent[3]) const
{
    int d;
    for (d = 0; d < 3; d++) {
	extent[d] = this->m_spacing[d] * (this->Size(d) - 1);
    }
}

/* Return true if the two headers are the same, within tolerance */
bool
Plm_image_header::compare (Plm_image_header *pli1, Plm_image_header *pli2,
    float threshold)
{
    int d;
    for (d = 0; d < 3; d++) {
        if (fabs (pli1->m_origin[d] - pli2->m_origin[d]) > threshold) {
           return false;
        }
        if (fabs (pli1->m_spacing[d] - pli2->m_spacing[d]) > threshold) {
            return false;
        }
        if (pli1->Size(d) != pli2->Size(d)) {
            return false;
        }
    }

    /* GCS FIX: check direction cosines */

    return true;
}
