#!/bin/sh

#  Description: Compute rigid body transformation using purely
#               orientation/position information stored within nifti 
#               file if the ones were properly converted from DICOM files
#

#*****            This code is released to the public domain.            *****
#
#*****  Author: Mark Jenkinson, FMRIB Centre, University of Oxford       *****
#*****  Date:   May 2007                                                 *****
#
#*****  Neither the FMRIB Centre, the University of Oxford, nor any of   *****
#*****  its employees imply any warranty of usefulness of this software  *****
#*****  for any purpose, and do not assume any liability for damages,    *****
#*****  incidental or otherwise, caused by any use of this document.     *****


if [ $# -lt 3 ] ; then
  echo "Usage: $0 <inputimage> <refimage> <outputmat> [qform|sform] [qform|sform]"
  exit 0;
fi

im1=`$FSLDIR/bin/remove_ext $1`
im2=`$FSLDIR/bin/remove_ext $2`

# check that both images are nifti images with valid sform/qform settings
for fn in $im1 $im2 ; do
    filetype=`$FSLDIR/bin/fslval $fn file_type`;
    if [ $filetype = "ANALYZE-7.5" ] ; then
	echo "Both images must be nifti images"
	exit 1;
    fi
    if [ `$FSLDIR/bin/fslval $1 sform_code` -eq 0 -a `$FSLDIR/bin/fslval $1 qform_code` -eq 0 ] ; then
	echo "Both images must have either the qform or the sform set"
	exit 1;
    fi
done


# extract the qform and sform and calculate the flirt vox2mm (samp) matrix
nam=""
oldnam=""
for fn in $im1 $im2 ; do
  oldnam=$nam
  nam=`$FSLDIR/bin/tmpnam`;
  $FSLDIR/bin/fslhd $fn | grep qto_xyz | sed 's/qto_xyz:. *//' > $nam.qform.mtx
  $FSLDIR/bin/fslhd $fn | grep sto_xyz | sed 's/sto_xyz:. *//' > $nam.sform.mtx

  nx=`$FSLDIR/bin/fslval $fn dim1`;
  dx=`$FSLDIR/bin/fslval $fn pixdim1`;
  dy=`$FSLDIR/bin/fslval $fn pixdim2`;
  dz=`$FSLDIR/bin/fslval $fn pixdim3`;

  echo "$dx 0 0 0" > $nam.samp.mtx
  echo "0 $dy 0 0" >> $nam.samp.mtx
  echo "0 0 $dz 0" >> $nam.samp.mtx
  echo "0 0 0 1" >> $nam.samp.mtx

  if [ `$FSLDIR/bin/fslorient -getorient $fn` = NEUROLOGICAL ] ; then
    nx1=`echo \( $nx - 1 \) | bc -l`;
    echo "-1 0 0 $nx1" > $nam.swapx.mtx
    echo "0 1 0 0" >> $nam.swapx.mtx
    echo "0 0 1 0" >> $nam.swapx.mtx
    echo "0 0 0 1" >> $nam.swapx.mtx
    $FSLDIR/bin/convert_xfm -omat $nam.samp.mtx -concat $nam.samp.mtx $nam.swapx.mtx
  fi
done

nam1=$oldnam
nam2=$nam

# decide which of sform or qform will be used
codetype1=sform
code1=`$FSLDIR/bin/fslval $im1 sform_code`;
if [ X$code1 = X0 ] ; then
  codetype1=qform
  code1=`$FSLDIR/bin/fslval $im1 qform_code`;
fi
codetype2=sform
code2=`$FSLDIR/bin/fslval $im2 sform_code`;
if [ X$code2 = X0 ] ; then
  codetype2=qform
  code2=`$FSLDIR/bin/fslval $im2 qform_code`;
fi

if [ $# -ge 4 ] ; then
  codetype1=$4;
  code1=`$FSLDIR/bin/fslval $im1 ${codetype1}_code`;
fi

if [ $# -ge 5 ] ; then
  codetype2=$5;
  code2=`$FSLDIR/bin/fslval $im2 ${codetype2}_code`;
fi


# default init is the identity
cp $FSLDIR/etc/flirtsch/ident.mat $3

# if the sform or qform codes are non-zero then calculate the init matrix
# formula is: init = (samp2) (qsform2)^{-1} (qsform1) (samp1)^{-1}
if [ X$code1 != X0 ] ; then
  # (it might be good to have manual control to force some of this logic)
  # if [ X$code2 = X$code1 ] ; then  # alternative to only do for same space
  if [ X$code2 != X0 ] ; then
     $FSLDIR/bin/convert_xfm -omat ${nam1}.samp.inv.mtx -inverse ${nam1}.samp.mtx 
     $FSLDIR/bin/convert_xfm -omat $3 -concat ${nam1}.samp.inv.mtx $3
     $FSLDIR/bin/convert_xfm -omat $3 -concat ${nam1}.${codetype1}.mtx $3
     $FSLDIR/bin/convert_xfm -omat ${nam2}.${codetype2}.inv.mtx -inverse ${nam2}.${codetype2}.mtx
     $FSLDIR/bin/convert_xfm -omat $3 -concat ${nam2}.${codetype2}.inv.mtx $3
     $FSLDIR/bin/convert_xfm -omat $3 -concat ${nam2}.samp.mtx $3
  fi
fi

# clean up
if [ X${nam1} != X ] ; then rm -f ${nam1}* ; fi
if [ X${nam2} != X ] ; then rm -f ${nam2}* ; fi

