/// NiftiOutputter.h
/**
*/

#ifndef NIFTI_OUTPUTTER_H_
#define NIFTI_OUTPUTTER_H_

#include <string>
#include <map>
#include <math.h>
#include <vector>

#include "SeriesHandler.h"
#include "Basic3DOutputter.h"
#include "BasicVolumeFormat.h"
#include "NiftiVolume.h"
#include "NiftiOutputterBase.h"

namespace jcs {

  /// Basic NIfTY Outputter.
  class NiftiOutputter : public NiftiOutputterBase {

    public:
    NiftiOutputter();
    ~NiftiOutputter();
    int ConvertSeries(SeriesHandler* handler);

  private:
    static Options CreateOptions();
  };

  template <class T>
  class NiftiConversion: public Basic3DConversion <T> {

  public:
    NiftiConversion(NiftiOutputterBase* outputter, SeriesHandler* handler);
    ~NiftiConversion();

  protected:
    virtual void GetHeaderForSeries();
    virtual void CompleteHeaderForVolume(std::pair<VolId, Volume<T> > volPair);

    NiftiHeader* GetHeader() { return mHeader; }

    NiftiHeader* mHeader;

    virtual std::vector<double> GetRotationMatrix(const VolId& id);

    std::vector<float>
    DicomToNiftiQuat(std::vector<double> rotation);

  };


  template <class T>
  NiftiConversion<T>::NiftiConversion(NiftiOutputterBase* outputter, SeriesHandler* handler)
  : Basic3DConversion<T>(outputter, handler)
  {
    mHeader = new NiftiHeader();
  }

  template <class T>
  NiftiConversion<T>::~NiftiConversion()
  {
    delete mHeader;
  }


  ///
  /**
  */
  template <class T> std::vector<double>
  NiftiConversion<T>::GetRotationMatrix(const VolId& id)
  {
    double image_size_x = this->mHandler->GetColumns() 
    * this->mHandler->GetVoxelSize()[0];

    double image_size_y = this->mHandler->GetRows() 
    * this->mHandler->GetVoxelSize()[1];


    std::vector<double> ipp = this->mHandler->GetIppForFirstSlice(id);
    std::vector<double> rotation = this->mHandler->GetRotationMatrix(id);

    std::vector<double> r4;

    r4.push_back(-rotation.at(0));
    r4.push_back(-rotation.at(1));
    r4.push_back(rotation.at(2));
    r4.push_back(0);

    r4.push_back(-rotation.at(3));
    r4.push_back(-rotation.at(4));
    r4.push_back(rotation.at(5));
    r4.push_back(0);

    r4.push_back(-rotation.at(6));
    r4.push_back(-rotation.at(7));
    r4.push_back(rotation.at(8));
    r4.push_back(0);

    r4.push_back(-ipp.at(0));
    r4.push_back(-ipp.at(1));
    r4.push_back(ipp.at(2));
    r4.push_back(1);

    return r4;
  }


  ///
  /**
  */
  template <class T> void
  NiftiConversion<T>::GetHeaderForSeries()
  {
    mHeader->InitHeader();

    mHeader->hdr.sizeof_hdr = 348;

    char freq_dim;
    char phase_dim;
    char slice_dim;
    std::string s;
    this->mHandler->Find("InPlanePhaseEncodingDirection", s);
    if (s == "ROW") {
      freq_dim = 2;
      phase_dim = 1;
    }
    else if (s == "COL") {
      freq_dim = 1;
      phase_dim = 2;
    }
    else {
      freq_dim = 0;
      phase_dim = 0;
    }

    slice_dim = 3;

    mHeader->SetDimInfo(freq_dim, phase_dim, slice_dim);

    int nVolumes = this->GetNumberOfVolumes();

    int dimensionality = this->mOutputter->GetDimensionality(this->mHandler->GetSeriesUid());
    if (dimensionality == 4 && nVolumes > 1) {
      mHeader->hdr.dim[0] = 4;
      mHeader->hdr.dim[4] = nVolumes;
    }
    else {
      mHeader->hdr.dim[0] = 3;
      mHeader->hdr.dim[4] = 1;
    }

    mHeader->hdr.dim[1] = this->mHandler->GetColumns();
    mHeader->hdr.dim[2] = this->mHandler->GetRows();
    mHeader->hdr.dim[3] = this->mHandler->GetNumberOfSlices();
    // dim[3] recalculated in mConvert

    mHeader->hdr.dim[5] = 1;
    mHeader->hdr.dim[6] = 1;
    mHeader->hdr.dim[7] = 1;

    mHeader->hdr.intent_code = 0;

    int bits_allocated, pixel_rep, samples = 0;
    this->mHandler->Find("BitsAllocated", bits_allocated);
    this->mHandler->Find("PixelRepresentation", pixel_rep);
    this->mHandler->Find("SamplesPerPixel", samples);

    this->mHandler->Find("PhotometricInterpretation", s);
    if (s == "RGB") {
      mHeader->hdr.datatype = DT_RGB;
    }
    else {

      switch (bits_allocated + pixel_rep) {

      case 8 : mHeader->hdr.datatype  = DT_UINT8;
        break;
      case 9 : mHeader->hdr.datatype  = DT_INT8;
        break;
      case 16 : mHeader->hdr.datatype = DT_UINT16;
        break;
      case 17 : mHeader->hdr.datatype = DT_INT16;
        break;
      case 32 : mHeader->hdr.datatype = DT_UINT32;
        break;
      case 33 : mHeader->hdr.datatype = DT_INT32;
        break;
        default : mHeader->hdr.datatype = DT_UNKNOWN;
      }
    }

    mHeader->hdr.bitpix = bits_allocated * samples;

    mHeader->hdr.slice_start = 0;
    mHeader->hdr.slice_end = mHeader->hdr.dim[3] - 1;

    std::vector<double>voxel_size = this->mHandler->GetVoxelSize();
    mHeader->hdr.pixdim[1] = voxel_size[0];
    mHeader->hdr.pixdim[2] = voxel_size[1];
    mHeader->hdr.pixdim[3] = voxel_size[2];
    // pixdim[3] recalculated during conversion

    if (dimensionality == 4 && nVolumes > 1) {
		mHeader->hdr.pixdim[4] = this->mHandler->GetVolumeInterval();
    }
    else {
		mHeader->hdr.pixdim[4] = 0;
    }

    mHeader->hdr.pixdim[5] = 0;
    mHeader->hdr.pixdim[6] = 0;
    mHeader->hdr.pixdim[7] = 0;


    AoCode ac_order = this->mHandler->GetAcquisitionOrder();
    switch (ac_order) {

    case ASCENDING:
      mHeader->hdr.slice_code = NIFTI_SLICE_SEQ_INC;
      break;

    case DESCENDING:
      mHeader->hdr.slice_code = NIFTI_SLICE_SEQ_DEC;
      break;

    case INTERLEAVED_ODD:
      mHeader->hdr.slice_code = NIFTI_SLICE_ALT_INC;
      break;

    case INTERLEAVED_EVEN:
      mHeader->hdr.slice_code = NIFTI_SLICE_ALT_INC2;
      break;

    default:
      mHeader->hdr.slice_code = NIFTI_SLICE_UNKNOWN;
    }

    char space_units = NIFTI_UNITS_MM;
    char time_units = NIFTI_UNITS_UNKNOWN;

    if (dimensionality == 4) {
      time_units = NIFTI_UNITS_SEC;
    }

 	if (this->mOutputter->rescale) {
		mHeader->hdr.datatype = DT_FLOAT;
	    mHeader->hdr.bitpix = 32 * samples; 
	}
    mHeader->hdr.scl_slope = 1; // we'll fix it in "complete header" for rescale = 1
    mHeader->hdr.scl_inter = 0;
    
    mHeader->hdr.xyzt_units = (space_units & 0x07) | (time_units & 0x38); 

    mHeader->hdr.cal_max = 0;
    mHeader->hdr.cal_min = 0;

    mHeader->hdr.slice_duration = this->mHandler->GetSliceDuration()/1000.0;

    mHeader->hdr.toffset = 0;

    this->mHandler->Find("SeriesDescription", s);
    s.copy(mHeader->hdr.descrip, sizeof(mHeader->hdr.descrip));

    mHeader->hdr.sform_code = NIFTI_XFORM_UNKNOWN;
    for (int i = 0; i < 4; ++i) {
      mHeader->hdr.srow_x[i] = 0;
      mHeader->hdr.srow_y[i] = 0;
      mHeader->hdr.srow_z[i] = 0;
    }
    mHeader->hdr.srow_x[0] = 1;
    mHeader->hdr.srow_y[1] = 1;
    mHeader->hdr.srow_z[2] = 1;

    NiftiOutputterBase* out = dynamic_cast<NiftiOutputterBase*>(this->mOutputter);
    if (out->saveNii) {
      strcpy(mHeader->hdr.magic, "n+1");
      mHeader->hdr.vox_offset = 352;
    }
    else {
      strcpy(mHeader->hdr.magic, "ni1");
      mHeader->hdr.vox_offset = 0;
    }
    
    if (this->mHandler->IsDti()) { Basic3DConversion<T>::WriteGradientFiles(); }
  }


  ///
  /**
  */
  template <class T> void
  NiftiConversion<T>::CompleteHeaderForVolume(std::pair<VolId, Volume<T> > volPair)
  {
    // Calculate number of slices for this volume
    int n_slices = volPair.second.size();
    mHeader->SetNumberOfSlices(n_slices);
    
    // Calculate voxel size in Z from slice spacing for multi-slice files
    if (n_slices > 1) {
      mHeader->SetSliceSpacing(volPair.second.GetSpacing());
    }

    mHeader->hdr.qform_code = NIFTI_XFORM_SCANNER_ANAT;

    std::vector<float> quat = DicomToNiftiQuat(GetRotationMatrix(volPair.first));
    mHeader->hdr.qoffset_x = quat.at(0);
    mHeader->hdr.qoffset_y = quat.at(1);
    mHeader->hdr.qoffset_z = quat.at(2);
    mHeader->hdr.quatern_b = quat.at(3);
    mHeader->hdr.quatern_c = quat.at(4);
    mHeader->hdr.quatern_d = quat.at(5);

    double slope, intercept;
	if ((this->mOutputter->rescale == 0) && 
		(this->mHandler->GetRescaleSlopeAndIntercept(volPair.first, slope, intercept))) {
			mHeader->hdr.scl_slope = slope;
			mHeader->hdr.scl_inter = intercept;
	}

  }


  ///
  /** Adapted from mat44_to_quatern from nifti_io
  */
  template <class T> std::vector<float>
  NiftiConversion<T>::DicomToNiftiQuat(std::vector<double> r)
  {
    std::vector<float> quat;

    // first three items in vector are offsets
    quat.push_back(static_cast<float> (r.at(12)));
    quat.push_back(static_cast<float> (r.at(13)));
    quat.push_back(static_cast<float> (r.at(14)));

    /* compute lengths of each column; these determine grid spacings  */

    double xd = sqrt( r.at(0)*r.at(0) + r.at(1)*r.at(1) + r.at(2)*r.at(2) ) ;
    double yd = sqrt( r.at(4)*r.at(4) + r.at(5)*r.at(5) + r.at(6)*r.at(6) ) ;
    double zd = sqrt( r.at(8)*r.at(8) + r.at(9)*r.at(9) + r.at(10)*r.at(10) ) ;

    /* if a column length is zero, patch the trouble */
    if( xd == 0.0l ){ r.at(0) = 1.0l ; r.at(1) = r.at(2) = 0.0l ; xd = 1.0l ; }
    if( yd == 0.0l ){ r.at(5) = 1.0l ; r.at(4) = r.at(6) = 0.0l ; yd = 1.0l ; }
    if( zd == 0.0l ){ r.at(10) = 1.0l ; r.at(8) = r.at(9) = 0.0l ; zd = 1.0l ; }

    /* normalize the columns */
    r.at(0) /= xd ; r.at(1) /= xd ; r.at(2) /= xd ;
    r.at(4) /= yd ; r.at(5) /= yd ; r.at(6) /= yd ;
    r.at(8) /= zd ; r.at(9) /= zd ; r.at(10) /= zd ;

    zd = r.at(0)*r.at(5)*r.at(10)-r.at(0)*r.at(6)*r.at(9)-r.at(1)*r.at(4)*r.at(10)
    +r.at(1)*r.at(6)*r.at(8)+r.at(2)*r.at(4)*r.at(9)-r.at(2)*r.at(5)*r.at(8) ;  /* should be -1 or 1 */

    if( zd <= 0 ){  
      mHeader->hdr.pixdim[0] = -1;
      r.at(8) = -r.at(8) ; r.at(9) = -r.at(9) ; r.at(10) = -r.at(10) ;
    }
    else {
      mHeader->hdr.pixdim[0] = 1;
    }

    /* now, compute quaternion parameters */
    double a = r.at(0) + r.at(5) + r.at(10) + 1.0l;
    double b,c,d;
    if( a > 0.5l ) {                /* simplest case */
      a = 0.5l * sqrt(a) ;
      b = 0.25l * (r.at(6)-r.at(9)) / a ;
      c = 0.25l * (r.at(8)-r.at(2)) / a ;
      d = 0.25l * (r.at(1)-r.at(4)) / a ;
    } else {                       /* trickier case */
      xd = 1.0 + r.at(0) - (r.at(5)+r.at(10)) ;  /* 4*b*b */
      yd = 1.0 + r.at(5) - (r.at(0)+r.at(10)) ;  /* 4*c*c */
      zd = 1.0 + r.at(10) - (r.at(0)+r.at(5)) ;  /* 4*d*d */
      if( xd > 1.0 ) {
        b = 0.5l * sqrt(xd) ;
        c = 0.25l* (r.at(4)+r.at(1)) / b ;
        d = 0.25l* (r.at(8)+r.at(2)) / b ;
        a = 0.25l* (r.at(6)-r.at(9)) / b ;
      } 
      else {
        if( yd > 1.0 ) {
          c = 0.5l * sqrt(yd) ;
          b = 0.25l* (r.at(4)+r.at(1)) / c ;
          d = 0.25l* (r.at(9)+r.at(6)) / c ;
          a = 0.25l* (r.at(8)-r.at(2)) / c ;
        } 
        else {
          d = 0.5l * sqrt(zd) ;
          b = 0.25l* (r.at(8)+r.at(2)) / d ;
          c = 0.25l* (r.at(9)+r.at(6)) / d ;
          a = 0.25l* (r.at(1)-r.at(4)) / d ;
        }
      }
      if( a < 0.0l )  {
        b=-b ; c=-c ; d=-d; a=-a;
      }
    }

    quat.push_back(static_cast<float>(b));
    quat.push_back(static_cast<float>(c));
    quat.push_back(static_cast<float>(d));

    return quat;
  }
}

#endif
