/**
 * @file  TensorField.cxx
 * @brief Decompose a tensor to different structures: prolate, oblate, sphere, FA, and ADC.
 *
 * Copyright (c) 2008, 2009, 2012, 2013 University of Pennsylvania.
 *
 * This file is part of DTI-DROID.
 *
 * DTI-DROID is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * DTI-DROID is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with DTI-DROID.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Contact: SBIA Group <sbia-software at uphs.upenn.edu>
 */

#include <stdio.h>

#include "mvcdDRTG.h"

#include <dtidroid/RGBAcolorMap.h>
#include <dtidroid/TensorField.h>

//==========================Gaussian kernel=====================================//
void generate1DGaussianKernel(double * filter, double var, int size)
{ 
  double sum = 0.0;
  for (int i=0; i<size; i++)
  {
    double x = i - (double) size / 2.0 + 0.5;
	
	filter[i] = exp(-x*x/(2*var*var));
	
	sum += filter[i];
  }
  
  // normalization
  for (int i=0; i<size; i++)
  {
    filter[i] /= sum;
  }
}

void generate2DGaussianKernel(double ** filter, double var, int size)
{
  double * filter1d = new double [size];
  generate1DGaussianKernel(filter1d, var, size);
  
  for (int i=0; i<size; i++)
    for (int j=0; j<size; j++)
	  filter[i][j] = filter1d[i]*filter1d[j];
	  
  delete [] filter1d;
}

void generate3DGaussianKernel(double *** filter, double var, int size)
{
  double *filter1d = new double [size];
  generate1DGaussianKernel(filter1d, var, size);
  
  for (int i=0; i<size; i++)
    for (int j=0; j<size; j++)
	  for (int k=0; k<size; k++)
	    filter[i][j][k] = filter1d[i]*filter1d[j]*filter1d[k];
		
  delete [] filter1d;
}
//============================= end of Gaussian Kernel========================================//

///@{  Structure fields
void TensorField::initStruct(int nInitStruct)
{
  _nStructInitialized = nInitStruct;
  
  if (nInitStruct & PROLATE_STRUCT)
  {
    _structProlate.init(m_Dims[0],m_Dims[1],m_Dims[2]);
    _structProlate.setVoxelSize(0,m_VoxelSize[0]); 
    _structProlate.setVoxelSize(1,m_VoxelSize[1]); 
    _structProlate.setVoxelSize(2,m_VoxelSize[2]);
  }
  
  if (nInitStruct & OBLATE_STRUCT)
  {
    _structOblate.init(m_Dims[0],m_Dims[1], m_Dims[2]);
    _structOblate.setVoxelSize(0, m_VoxelSize[0]);
    _structOblate.setVoxelSize(1, m_VoxelSize[1]);
    _structOblate.setVoxelSize(2, m_VoxelSize[2]);
  }
  
  if (nInitStruct & SPHERE_STRUCT)
  {
    _structSphere.init(m_Dims[0],m_Dims[1], m_Dims[2]);
    _structSphere.setVoxelSize(0, m_VoxelSize[0]);
    _structSphere.setVoxelSize(1, m_VoxelSize[1]);
    _structSphere.setVoxelSize(2, m_VoxelSize[2]);
  }
  
  if (nInitStruct & FA_STRUCT)
  {
    _structFA.init(m_Dims[0],m_Dims[1], m_Dims[2]);
    _structFA.setVoxelSize(0, m_VoxelSize[0]);
    _structFA.setVoxelSize(1, m_VoxelSize[1]);
    _structFA.setVoxelSize(2, m_VoxelSize[2]);
  }
  
  if (nInitStruct & TRACE_STRUCT)
  {
    _structTrace.init(m_Dims[0],m_Dims[1], m_Dims[2]);
    _structTrace.setVoxelSize(0, m_VoxelSize[0]);
    _structTrace.setVoxelSize(1, m_VoxelSize[1]);
    _structTrace.setVoxelSize(2, m_VoxelSize[2]);
  }
}


void TensorField::cleanStruct(int nSelectedStruct)
{
  if ( (nSelectedStruct & PROLATE_STRUCT) && (_nStructInitialized & PROLATE_STRUCT) )
  {
    _structProlate.clear(); _nStructInitialized -= PROLATE_STRUCT;
  }

  if ( (nSelectedStruct & OBLATE_STRUCT) && (_nStructInitialized & OBLATE_STRUCT) )
  {
    _structOblate.clear(); _nStructInitialized -= OBLATE_STRUCT;
  }

  if ( (nSelectedStruct & SPHERE_STRUCT) && (_nStructInitialized & SPHERE_STRUCT) )
  {
    _structSphere.clear(); _nStructInitialized -= SPHERE_STRUCT;
  }

  if ( (nSelectedStruct & FA_STRUCT) && (_nStructInitialized & FA_STRUCT) )
  {
    _structFA.clear(); _nStructInitialized -= FA_STRUCT;
  }

  if ( (nSelectedStruct & TRACE_STRUCT) && (_nStructInitialized & TRACE_STRUCT) )
  {
    _structTrace.clear(); _nStructInitialized -= TRACE_STRUCT;
  }  
}


void TensorField::decomposeStructure()
{
  if (_nStructInitialized == 0) return;
  
  int Xdim= m_Dims[0];
  int Ydim= m_Dims[1];
  int Zdim= m_Dims[2];

  //// due to the normalization problem, trace is processed different from other maps
  float maxTraceVal = 0.0f;
  FloatScalarField trace;
  if (_nStructInitialized & TRACE_STRUCT)
  {
    trace.init(Xdim, Ydim, Zdim);
  }
  
  
  gsl_vector *eval ;
  gsl_matrix *evec ;
  
  gsl_eigen_symmv_workspace  *w ;
  
  eval = gsl_vector_alloc (3);
  evec = gsl_matrix_alloc (3, 3);
  w = gsl_eigen_symmv_alloc (3);
    
  for(int ix=0; ix<Xdim; ix++)
    for(int iy=0; iy<Ydim; iy++)
      for(int iz=0; iz<Zdim; iz++)
	  {
        double M[9];
        double eigVal[3], eigVec[9];

        int ind = iz*Xdim*Ydim + iy*Xdim + ix;

        M[0]= (double) m_Data[ind*6];
        M[4]= (double) m_Data[ind*6+1];
        M[8]= (double) m_Data[ind*6+2];
        M[1]= (double) m_Data[ind*6+3];
        M[2]= (double) m_Data[ind*6+4];
        M[5]= (double) m_Data[ind*6+5];
        M[3]= M[1];
        M[6]= M[2];
        M[7]= M[5];

        if (M[0]+M[1]+M[2]+M[4]+M[5]+M[8] == 0)
        {
          unsigned char *val;
          if (_nStructInitialized & PROLATE_STRUCT)
          {
            val = _structProlate.getAt(ix,iy,iz);
            *val = 0;
          }
          
          if (_nStructInitialized & OBLATE_STRUCT)
          {
            val = _structOblate.getAt(ix,iy,iz);
            *val = 0;
          }
          
          if (_nStructInitialized & SPHERE_STRUCT)
          {
            val = _structSphere.getAt(ix,iy,iz);
            *val = 0;
          }
          
          if (_nStructInitialized & FA_STRUCT)
          {
            val = _structFA.getAt(ix,iy,iz);
            *val = 0;
          }
          
          if (_nStructInitialized & TRACE_STRUCT)
          {
            val = _structTrace.getAt(ix,iy,iz);
            *val = 0;
          }
        
          continue;
        }
        
        gsl_matrix_view m = gsl_matrix_view_array (M, 3, 3);
     
	    gsl_eigen_symmv(&m.matrix, eval, evec, w);
    
	    gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_DESC);
	 
     	for(int i=0; i<3; i++)
	    {
	      eigVal[i] = fabs(gsl_vector_get (eval, i));

	      gsl_vector_view evec_i = gsl_matrix_column (evec, i);
	      for(int j=0; j<3; j++)
		    eigVec[i*3+j] = gsl_vector_get (&evec_i.vector, j);	     
	    }
        
	    unsigned char *val;
        float r;
	  
        //prolate structure
        if (_nStructInitialized & PROLATE_STRUCT)
        {
	      val= _structProlate.getAt(ix,iy,iz);
	      r = (eigVal[0] - eigVal[1])/(eigVal[0]+1e-24);
          *val = (int)(r*255.0f);
        }
        
        //oblate structure
	    if (_nStructInitialized & OBLATE_STRUCT)
        {
          val= _structOblate.getAt(ix,iy,iz);
	      r = (eigVal[1] - eigVal[2])/(eigVal[0]+1e-24);
          *val = (int)(r*255.0f);
        }
    
        // sphere structure
        if (_nStructInitialized & SPHERE_STRUCT)
        {
          val= _structSphere.getAt(ix,iy,iz);
	      r = eigVal[2]/(eigVal[0]+1e-24);
          *val = (int)(r*255.0f);
        }
                
        // trace structure
        if (_nStructInitialized & TRACE_STRUCT)
        {
          float *fval = trace.getAt(ix,iy,iz);
          *fval = eigVal[0]+eigVal[1]+eigVal[2];
          if (*fval > maxTraceVal) maxTraceVal = *fval;
        }
        
        //FA structure
        if (_nStructInitialized & FA_STRUCT)
        {
          val= _structFA.getAt(ix,iy,iz);
          float em = eigVal[0]+eigVal[1]+eigVal[2];
          r = sqrt(3.0f/2.0f*((eigVal[0]-em/3.0f)*(eigVal[0]-em/3.0f) + (eigVal[1]-em/3.0f)*(eigVal[1]-em/3.0f) + 
                (eigVal[2]-em/3.0f)*(eigVal[2]-em/3.0f))/(eigVal[0]*eigVal[0] + eigVal[1]*eigVal[1] + eigVal[2]*eigVal[2]+1e-24));
                       
          *val = (int)(r*255.0f);
        }
	  }	 
  
  if ((_nStructInitialized & TRACE_STRUCT) && (maxTraceVal > 0.0f))
  { /// normalize trace structure
    unsigned char *val = _structTrace.getDataPtr();
    float *fval = trace.getDataPtr();
    
    for(int ix=0; ix<Xdim; ix++)
      for(int iy=0; iy<Ydim; iy++)
        for(int iz=0; iz<Zdim; iz++)    
        {
          int idx = iz*Xdim*Ydim+iy*Xdim+ix;
          val[idx] = (int)(fval[idx]/maxTraceVal*255.0f);
        }
        
    trace.clear();
  }
  
  gsl_eigen_symmv_free (w);
  gsl_vector_free(eval);
  gsl_matrix_free(evec);
}


void TensorField::smoothTensorStructField(int nSelectedStruct, float sigma)
{
  if (nSelectedStruct & PROLATE_STRUCT) 
  {
    if (_nStructInitialized & PROLATE_STRUCT)
      smoothTensorStruct3D(_structProlate, sigma);
    else
      std::cout << "Prolate structure has not been initialized. No smoothing applied." << std::endl;
  }
  
  if (nSelectedStruct & OBLATE_STRUCT)
  {
    if (_nStructInitialized & OBLATE_STRUCT)
      smoothTensorStruct3D(_structOblate, sigma);
    else
      std::cout << "Oblate structure has not been initialized. No smoothing applied." << std::endl;
  }
  
  if (nSelectedStruct & SPHERE_STRUCT)
  {
    if (_nStructInitialized & SPHERE_STRUCT)
      smoothTensorStruct3D(_structSphere, sigma);
    else
      std::cout << "Sphere structure has not been initialized. No smoothing applied." << std::endl;
  }
  
  if (nSelectedStruct & FA_STRUCT)
  {
    if (_nStructInitialized & FA_STRUCT)
      smoothTensorStruct3D(_structFA, sigma);
    else
      std::cout << "FA structure has not been initialized. No smoothing applied." << std::endl;
  }
  
  if (nSelectedStruct & TRACE_STRUCT)
  {
    if (_nStructInitialized & TRACE_STRUCT)
      smoothTensorStruct3D(_structTrace, sigma);
    else
      std::cout << "Trace structure has not been initialized. No smoothing applied." << std::endl;
  }

}


void TensorField::normalizeTensorStructField(int nSelectedStruct)
{
  if (nSelectedStruct & PROLATE_STRUCT)
  {
    if (_nStructInitialized & PROLATE_STRUCT)
      normalizeTensorStruct(_structProlate);
    else
      std::cout << "Prolate structure has not been initialized. No normalization applied." << std::endl;
  }
  
  if (nSelectedStruct & OBLATE_STRUCT)
  {
    if (_nStructInitialized & OBLATE_STRUCT)
      normalizeTensorStruct(_structOblate);
    else
      std::cout << "Oblate structure has not been initialized. No normalization applied." << std::endl;
  }
  
  if (nSelectedStruct & SPHERE_STRUCT) 
  {
    if (_nStructInitialized & SPHERE_STRUCT)
      normalizeTensorStruct(_structSphere);
    else
      std::cout << "Sphere structure has not been initialized. No normalization applied." << std::endl;
  }
  
  if (nSelectedStruct & FA_STRUCT)
  {
    if (_nStructInitialized & FA_STRUCT)
      normalizeTensorStruct(_structFA);
    else
      std::cout << "FA structure has not been initialized. No normalization applied." << std::endl;
  }
  
  if (nSelectedStruct & TRACE_STRUCT)
  {
    std::cout << "No normalization is necessary for trace structure." << std::endl;
    return;
  }
}

void TensorField::saveTensorStructField(int nSelectedStruct, const std::string& prefix)
{
  if ( nSelectedStruct & PROLATE_STRUCT)
  {
    std::string filename = prefix + "_prolate.img";
    _structProlate.exportRawFieldToFile(filename);
  }
  
  if ( nSelectedStruct & OBLATE_STRUCT)
  {
    std::string filename = prefix + "_oblate.img";
    _structOblate.exportRawFieldToFile(filename);
  }
  
  if ( nSelectedStruct & SPHERE_STRUCT)
  {
    std::string filename = prefix + "_sphere.img";
    _structSphere.exportRawFieldToFile(filename);
  }
  
  if ( nSelectedStruct & FA_STRUCT)
  {
    std::string filename = prefix + "_fa.img";
    _structFA.exportRawFieldToFile(filename);
  }
  
  if ( nSelectedStruct & TRACE_STRUCT)
  {
    std::string filename = prefix + "_trace.img";
    _structTrace.exportRawFieldToFile(filename);
  }
}
///@}

///@{ PD maps
void TensorField::initColorPD(int nInitPD)
{
  _nPDInitialized = nInitPD;
  
  if (nInitPD & PD_FIRST)
  {
    _colorPD_1.init(m_Dims[0],m_Dims[1],m_Dims[2]);
    _colorPD_1.setVoxelSize(0,m_VoxelSize[0]); 
    _colorPD_1.setVoxelSize(1,m_VoxelSize[1]); 
    _colorPD_1.setVoxelSize(2,m_VoxelSize[2]);
  }
  
  if (nInitPD & PD_SECOND)
  {
    _colorPD_2.init(m_Dims[0],m_Dims[1], m_Dims[2]);
    _colorPD_2.setVoxelSize(0, m_VoxelSize[0]);
    _colorPD_2.setVoxelSize(1, m_VoxelSize[1]);
    _colorPD_2.setVoxelSize(2, m_VoxelSize[2]);
  }
  
  if (nInitPD & PD_THIRD)
  {
    _colorPD_3.init(m_Dims[0],m_Dims[1], m_Dims[2]);
    _colorPD_3.setVoxelSize(0, m_VoxelSize[0]);
    _colorPD_3.setVoxelSize(1, m_VoxelSize[1]);
    _colorPD_3.setVoxelSize(2, m_VoxelSize[2]);
  }
}

void TensorField::cleanColorPD(int nSelectedPD)
{
  if ( (nSelectedPD & PD_FIRST) && (_nPDInitialized & PD_FIRST) )
  {
    _colorPD_1.clear(); _nPDInitialized -= PD_FIRST;
  }

  if ( (nSelectedPD & PD_SECOND) && (_nPDInitialized & PD_SECOND) )
  {
    _colorPD_2.clear(); _nPDInitialized -= PD_SECOND;
  }
  
  if ( (nSelectedPD & PD_THIRD) && (_nPDInitialized & PD_THIRD) )
  {
    _colorPD_3.clear(); _nPDInitialized -= PD_THIRD;
  }
}

void TensorField::computeColorPD(bool bFAWeighted)
{

  if (_nPDInitialized == 0) return;

  int Xdim= m_Dims[0];
  int Ydim= m_Dims[1];
  int Zdim= m_Dims[2];


  gsl_vector *eval ;
  gsl_matrix *evec ;
  
  gsl_eigen_symmv_workspace  *w ;
  
  eval = gsl_vector_alloc (3);
  evec = gsl_matrix_alloc (3, 3); 
  w = gsl_eigen_symmv_alloc (3);
    
  for(int ix=0; ix<Xdim; ix++)
    for(int iy=0; iy<Ydim; iy++)
      for(int iz=0; iz<Zdim; iz++)
	  {
        double M[9];
        double eigVal[3], PD[3];
  
	    int ind = iz*Xdim*Ydim + iy*Xdim + ix;

	    M[0]= (double) m_Data[ind*6];
	    M[4]= (double) m_Data[ind*6+1];
	    M[8]= (double) m_Data[ind*6+2];
	    M[1]= (double) m_Data[ind*6+3];
	    M[2]= (double) m_Data[ind*6+4];
	    M[5]= (double) m_Data[ind*6+5];
	    M[3]= M[1];
	    M[6]= M[2];
	    M[7]= M[5];

        if (M[0]+M[1]+M[2]+M[4]+M[5]+M[8] == 0)
        {
          unsigned char *val;
          if (_nPDInitialized & PD_FIRST)
          {
            val = _colorPD_1.getAt(ix,iy,iz);
            val[0] = 0; val[1] = 0; val[2] = 0;
          }
          
          if (_nPDInitialized & PD_SECOND)
          {
            val = _colorPD_2.getAt(ix,iy,iz);
            val[0] = 0; val[1] = 0; val[2] = 0;
          }
          
          if (_nPDInitialized & PD_THIRD)
          {
            val = _colorPD_3.getAt(ix,iy,iz);
            val[0] = 0; val[1] = 0; val[2] = 0;
          }
                  
          continue;
        }
        
	    gsl_matrix_view m = gsl_matrix_view_array (M, 3, 3);
     
	    gsl_eigen_symmv(&m.matrix, eval, evec, w);
    
	    gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_DESC);

	    double trace=0.0;
	    for(int i=0; i<3; i++)
	    {
	      eigVal[i] = gsl_vector_get (eval, i);	      
	      trace += eigVal[i];
	    }
	 
        double fa=1.0;
        if (bFAWeighted)
        {
          fa = sqrt(3.0/2.0*((eigVal[0]-trace/3)*(eigVal[0]-trace/3)+(eigVal[1]-trace/3)*(eigVal[1]-trace/3) 
             + (eigVal[2]-trace/3)*(eigVal[2]-trace/3))/((eigVal[0]*eigVal[0])+(eigVal[1]*eigVal[1]) + (eigVal[2]*eigVal[2])));
             
          if (fa>1.0) fa=1.0;
        }

        unsigned char *val;
        if (_nPDInitialized & PD_FIRST)
        {
          gsl_vector_view evec_i = gsl_matrix_column (evec, 0);
	      
	      for(int j=0; j<3; j++)
		    PD[j] = gsl_vector_get(&evec_i.vector, j);		     
		  
	      if (bFAWeighted) 
          {
            for (int j=0; j<3; j++) PD[j] = fa*PD[j];
          }

	      val=_colorPD_1.getAt(ix,iy,iz);
          val[0]=(int)(fabs(PD[0])*255.0f); 
          val[1]=(int)(fabs(PD[1])*255.0f);
          val[2]=(int)(fabs(PD[2])*255.0f);
	    }
      
        if (_nPDInitialized & PD_SECOND)
        {
          gsl_vector_view evec_i = gsl_matrix_column (evec, 1);
	      
	      for(int j=0; j<3; j++)
		    PD[j] = gsl_vector_get(&evec_i.vector, j);		     
		  
	      if (bFAWeighted) 
          {
            for (int j=0; j<3; j++) PD[j] = fa*PD[j];
          }

	      val=_colorPD_2.getAt(ix,iy,iz);
          val[0]=(int)(fabs(PD[0])*255.0f); 
          val[1]=(int)(fabs(PD[1])*255.0f);
          val[2]=(int)(fabs(PD[2])*255.0f);
	    }
	  
        if (_nPDInitialized & PD_THIRD)
        {
          gsl_vector_view evec_i = gsl_matrix_column (evec, 2);
	      
	      for(int j=0; j<3; j++)
		    PD[j] = gsl_vector_get(&evec_i.vector, j);		     
		  
	      if (bFAWeighted) 
          {
            for (int j=0; j<3; j++) PD[j] = fa*PD[j];
          }

	      val=_colorPD_3.getAt(ix,iy,iz);
          val[0]=(int)(fabs(PD[0])*255.0f); 
          val[1]=(int)(fabs(PD[1])*255.0f);
          val[2]=(int)(fabs(PD[2])*255.0f);
	    }      
	  }
  
  gsl_eigen_symmv_free ( w);
  gsl_vector_free( eval);
  gsl_matrix_free( evec);
  
}

void TensorField::smoothColorPDField(int nSelectedPD, float sigma)
{
  ByteScalarField tmpField;
  tmpField.init(m_Dims[0],m_Dims[1], m_Dims[2]);
  tmpField.setVoxelSize(0, m_VoxelSize[0]);
  tmpField.setVoxelSize(1, m_VoxelSize[1]);
  tmpField.setVoxelSize(2, m_VoxelSize[2]);

  int Xdim= m_Dims[0];
  int Ydim= m_Dims[1];
  int Zdim= m_Dims[2];
  
  if (nSelectedPD & PD_FIRST) 
  {
    if (_nPDInitialized & PD_FIRST)
    {
      unsigned char *sval, *vval;   
      for(int ix=0; ix<Xdim; ix++)
        for(int iy=0; iy<Ydim; iy++)
          for(int iz=0; iz<Zdim; iz++)
	      {
            sval=tmpField.getAt(ix,iy,iz);
            vval=_colorPD_1.getAt(ix,iy,iz);
            *sval=vval[0];
          }
          
      smoothTensorStruct3D(tmpField, sigma);
      
      for(int ix=0; ix<Xdim; ix++)
        for(int iy=0; iy<Ydim; iy++)
          for(int iz=0; iz<Zdim; iz++)
	      {
            sval=tmpField.getAt(ix,iy,iz);
            vval=_colorPD_1.getAt(ix,iy,iz);
            vval[0]=*sval; *sval=vval[1];
          }
       
      smoothTensorStruct3D(tmpField, sigma);
      
      for(int ix=0; ix<Xdim; ix++)
        for(int iy=0; iy<Ydim; iy++)
          for(int iz=0; iz<Zdim; iz++)
	      {
            sval=tmpField.getAt(ix,iy,iz);
            vval=_colorPD_1.getAt(ix,iy,iz);
            vval[1]=*sval; *sval=vval[2];
          }
          
      smoothTensorStruct3D(tmpField, sigma);
      
      for(int ix=0; ix<Xdim; ix++)
        for(int iy=0; iy<Ydim; iy++)
          for(int iz=0; iz<Zdim; iz++)
	      {
            sval=tmpField.getAt(ix,iy,iz);
            vval=_colorPD_1.getAt(ix,iy,iz);
            vval[2]=*sval;
          }
    }
    else
      std::cout << "PD1 has not been initialized. No smoothing applied." << std::endl;
  }
  
  if (nSelectedPD & PD_SECOND) 
  {
    if (_nPDInitialized & PD_SECOND)
    {
      unsigned char *sval, *vval;   
      for(int ix=0; ix<Xdim; ix++)
        for(int iy=0; iy<Ydim; iy++)
          for(int iz=0; iz<Zdim; iz++)
	      {
            sval=tmpField.getAt(ix,iy,iz);
            vval=_colorPD_2.getAt(ix,iy,iz);
            *sval=vval[0];
          }
          
      smoothTensorStruct3D(tmpField, sigma);
      
      for(int ix=0; ix<Xdim; ix++)
        for(int iy=0; iy<Ydim; iy++)
          for(int iz=0; iz<Zdim; iz++)
	      {
            sval=tmpField.getAt(ix,iy,iz);
            vval=_colorPD_2.getAt(ix,iy,iz);
            vval[0]=*sval; *sval=vval[1];
          }
       
      smoothTensorStruct3D(tmpField, sigma);
      
      for(int ix=0; ix<Xdim; ix++)
        for(int iy=0; iy<Ydim; iy++)
          for(int iz=0; iz<Zdim; iz++)
	      {
            sval=tmpField.getAt(ix,iy,iz);
            vval=_colorPD_2.getAt(ix,iy,iz);
            vval[1]=*sval; *sval=vval[2];
          }
          
      smoothTensorStruct3D(tmpField, sigma);
      
      for(int ix=0; ix<Xdim; ix++)
        for(int iy=0; iy<Ydim; iy++)
          for(int iz=0; iz<Zdim; iz++)
	      {
            sval=tmpField.getAt(ix,iy,iz);
            vval=_colorPD_2.getAt(ix,iy,iz);
            vval[2]=*sval;
          }
    }
    else
      std::cout << "PD2 has not been initialized. No smoothing applied." << std::endl;
  }
  
  if (nSelectedPD & PD_THIRD) 
  {
    if (_nPDInitialized & PD_THIRD)
    {
      unsigned char *sval, *vval;   
      for(int ix=0; ix<Xdim; ix++)
        for(int iy=0; iy<Ydim; iy++)
          for(int iz=0; iz<Zdim; iz++)
	      {
            sval=tmpField.getAt(ix,iy,iz);
            vval=_colorPD_3.getAt(ix,iy,iz);
            *sval=vval[0];
          }
          
      smoothTensorStruct3D(tmpField, sigma);
      
      for(int ix=0; ix<Xdim; ix++)
        for(int iy=0; iy<Ydim; iy++)
          for(int iz=0; iz<Zdim; iz++)
	      {
            sval=tmpField.getAt(ix,iy,iz);
            vval=_colorPD_3.getAt(ix,iy,iz);
            vval[0]=*sval; *sval=vval[1];
          }
       
      smoothTensorStruct3D(tmpField, sigma);
      
      for(int ix=0; ix<Xdim; ix++)
        for(int iy=0; iy<Ydim; iy++)
          for(int iz=0; iz<Zdim; iz++)
	      {
            sval=tmpField.getAt(ix,iy,iz);
            vval=_colorPD_3.getAt(ix,iy,iz);
            vval[1]=*sval; *sval=vval[2];
          }
          
      smoothTensorStruct3D(tmpField, sigma);
      
      for(int ix=0; ix<Xdim; ix++)
        for(int iy=0; iy<Ydim; iy++)
          for(int iz=0; iz<Zdim; iz++)
	      {
            sval=tmpField.getAt(ix,iy,iz);
            vval=_colorPD_3.getAt(ix,iy,iz);
            vval[2]=*sval;
          }
    }
    else
      std::cout << "PD3 has not been initialized. No smoothing applied." << std::endl;
  }
  
  tmpField.clear();
}

void TensorField::saveColorPDField(int nSelectedPD, const std::string& prefix)
{
  int Xdim= m_Dims[0];
  int Ydim= m_Dims[1];
  int Zdim= m_Dims[2];
  
  RGBAcolorMap *vRGBA= new RGBAcolorMap(Zdim, Xdim, Ydim);
  struct byteRGBA aMap;
  
  if ( nSelectedPD & PD_FIRST)
  {
    for(int iz=0; iz<Zdim; iz++)      
      for(int ix=0; ix<Xdim; ix++)
	    for(int iy=0; iy<Ydim; iy++)
	    {
          unsigned char * pd_val = _colorPD_1.getAt(ix,iy,iz);
          unsigned char d = aMap.rgba[3] = pd_val[0];
          aMap.rgba[2] = pd_val[1]; if (d < pd_val[1]) d = pd_val[1];
          aMap.rgba[1] = pd_val[2]; if (d < pd_val[2]) d = pd_val[2];
          if (d > 0) aMap.rgba[0] = 255; else aMap.rgba[0] = 0;

          vRGBA->setVoxel(ix, iy, iz, aMap);
        }
        
    std::string filename = prefix + "_pd1.cmp";
    vRGBA->saveVolume(filename.c_str());
  }
  
  if ( nSelectedPD & PD_SECOND)
  {
    for(int iz=0; iz<Zdim; iz++)      
      for(int ix=0; ix<Xdim; ix++)
	    for(int iy=0; iy<Ydim; iy++)
	    {
          unsigned char * pd_val = _colorPD_2.getAt(ix,iy,iz);
          unsigned char d = aMap.rgba[3] = pd_val[0];
          aMap.rgba[2] = pd_val[1]; if (d < pd_val[1]) d = pd_val[1];
          aMap.rgba[1] = pd_val[2]; if (d < pd_val[2]) d = pd_val[2];
          if (d > 0) aMap.rgba[0] = 255; else aMap.rgba[0] = 0;

          vRGBA->setVoxel(ix, iy, iz, aMap);
        }
        
    std::string filename = prefix + "_pd2.cmp";
    vRGBA->saveVolume(filename.c_str());
  }
  
  if ( nSelectedPD & PD_THIRD)
  {
    for(int iz=0; iz<Zdim; iz++)      
      for(int ix=0; ix<Xdim; ix++)
	    for(int iy=0; iy<Ydim; iy++)
	    {
          unsigned char * pd_val = _colorPD_3.getAt(ix,iy,iz);
          unsigned char d = aMap.rgba[3] = pd_val[0];
          aMap.rgba[2] = pd_val[1]; if (d < pd_val[1]) d = pd_val[1];
          aMap.rgba[1] = pd_val[2]; if (d < pd_val[2]) d = pd_val[2];
          if (d > 0) aMap.rgba[0] = 255; else aMap.rgba[0] = 0;

          vRGBA->setVoxel(ix, iy, iz, aMap);
        }
        
    std::string filename = prefix + "_pd3.cmp";
    vRGBA->saveVolume(filename.c_str());
  }
  
  delete vRGBA;
}
///@}


void TensorField::smoothTensorStruct2D(ByteScalarField& tensorStruct, float sigma)
{
  // obtain gaussian filter
  double **filter = Dalloc2d(5,5);
  generate2DGaussianKernel(filter, sigma, 5);
  
  // 2D smoothing
  for (int z=0; z<m_Dims[2]; z++)
  {
    for (int y=0; y<m_Dims[1]; y++)
      for (int x=0; x<m_Dims[0]; x++)
      {
        int ind = z*m_Dims[0]*m_Dims[1] + y*m_Dims[0] + x;
        if (m_Data[ind*6]+m_Data[ind*6+1]+m_Data[ind*6+2]+m_Data[ind*6+3]
            +m_Data[ind*6+4]+m_Data[ind*6+5]==0) continue;
            
        unsigned char *val; 
        float sVal = 0.0, weight = 0.0;
        for (int m=y-2; m<=y+2; m++)
		  for (int n=x-2; n<=x+2; n++)
          {
            if ( m<0 || m>=m_Dims[1] || n<0 || n>=m_Dims[0]) continue;
            
            int lm = m-y+2;
            int ln = n-x+2;
            val = tensorStruct.getAt(n,m,z);

            sVal += (float)(*val)*(float)filter[lm][ln];
            weight += (float)filter[lm][ln];
          }
        
        if (weight > 0.0f) sVal = sVal/weight;
        if (sVal > 255.0f) sVal = 255.0f;
        val = tensorStruct.getAt(x,y,z);
        *val = (int)(sVal+0.5f);
      }
  }
  
  Dfree2d(filter, 5);
}

void TensorField::smoothTensorStruct3D(ByteScalarField& tensorStruct, float sigma)
{
  // obtain gaussian filter
  double ***filter = Dalloc3d(5,5,5);
  generate3DGaussianKernel(filter, sigma, 5);
  
  // 2D smoothing
  for (int z=0; z<m_Dims[2]; z++)
    for (int y=0; y<m_Dims[1]; y++)
      for (int x=0; x<m_Dims[0]; x++)
      {
        int ind = z*m_Dims[0]*m_Dims[1] + y*m_Dims[0] + x;
        if (m_Data[ind*6]+m_Data[ind*6+1]+m_Data[ind*6+2]+m_Data[ind*6+3]
            +m_Data[ind*6+4]+m_Data[ind*6+5]==0) continue;
            
        unsigned char *val; 
        float sVal = 0.0, weight = 0.0;
        for (int l=z-2; l<=z+2; l++)
          for (int m=y-2; m<=y+2; m++)
		    for (int n=x-2; n<=x+2; n++)
            {
              if ( l<0 || l>=m_Dims[2] || m<0 || m>=m_Dims[1] || n<0 || n>=m_Dims[0]) continue;
            
              int ll = l-z+2;
              int lm = m-y+2;
              int ln = n-x+2;
              val = tensorStruct.getAt(n,m,l);
            
              sVal += (float)(*val)*(float)filter[ll][lm][ln];
              weight += (float)filter[ll][lm][ln];
          }
        
        if (weight > 0.0f) sVal = sVal/weight;
        if (sVal > 255.0f) sVal = 255.0f;
        val = tensorStruct.getAt(x,y,z);
        *val = (int)(sVal+0.5f);
      }
  
  Dfree3d(filter, 5, 5);
}

void TensorField::normalizeTensorStruct(ByteScalarField& tensorStruct)
{
  unsigned char *val;
  
  int val_min = 255, val_max = 0;
  for (int z=0; z<m_Dims[2]; z++)
    for (int y=0; y<m_Dims[1]; y++)
      for (int x=0; x<m_Dims[0]; x++)
      {
        val = tensorStruct.getAt(x,y,z);
        if ( *val > val_max ) val_max = *val;
        if ( *val < val_min ) val_min = *val;
      }
      
  if (val_max == val_min) return;
  
  for (int z=0; z<m_Dims[2]; z++)
    for (int y=0; y<m_Dims[1]; y++)
      for (int x=0; x<m_Dims[0]; x++)
      {
        val = tensorStruct.getAt(x,y,z);
        *val = (int) (((double)(*val) - val_min)/(val_max-val_min)*255.0);
      }
}
