/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include "itkTractsToDWIImageFilter.h"
#include <boost/progress.hpp>
#include <vtkSmartPointer.h>
#include <vtkPolyData.h>
#include <vtkCellArray.h>
#include <vtkPoints.h>
#include <vtkPolyLine.h>
#include <itkImageRegionIteratorWithIndex.h>
#include <itkResampleImageFilter.h>
#include <itkNearestNeighborInterpolateImageFunction.h>
#include <itkBSplineInterpolateImageFunction.h>
#include <itkCastImageFilter.h>
#include <itkImageFileWriter.h>
#include <itkRescaleIntensityImageFilter.h>
#include <itkWindowedSincInterpolateImageFunction.h>
#include <itkResampleDwiImageFilter.h>
#include <itkKspaceImageFilter.h>
#include <itkDftImageFilter.h>
#include <itkAddImageFilter.h>
#include <itkConstantPadImageFilter.h>
#include <itkCropImageFilter.h>
#include <mitkAstroStickModel.h>
#include <vtkTransform.h>
#include <iostream>
#include <fstream>
#include <exception>
#include <itkImageDuplicator.h>
#include <itksys/SystemTools.hxx>
#include <mitkIOUtil.h>
#include <itkDiffusionTensor3DReconstructionImageFilter.h>
#include <itkDiffusionTensor3D.h>
#include <itkTractDensityImageFilter.h>
#include <boost/lexical_cast.hpp>
#include <itkTractsToVectorImageFilter.h>
#include <itkInvertIntensityImageFilter.h>
#include <itkShiftScaleImageFilter.h>
#include <itkExtractImageFilter.h>
#include <itkResampleDwiImageFilter.h>
#include <boost/algorithm/string/replace.hpp>

namespace itk
{

  template< class PixelType >
  TractsToDWIImageFilter< PixelType >::TractsToDWIImageFilter()
    : m_FiberBundle(NULL)
    , m_StatusText("")
    , m_UseConstantRandSeed(false)
    , m_RandGen(itk::Statistics::MersenneTwisterRandomVariateGenerator::New())
  {
    m_RandGen->SetSeed();
  }

  template< class PixelType >
  TractsToDWIImageFilter< PixelType >::~TractsToDWIImageFilter()
  {

  }

  template< class PixelType >
  TractsToDWIImageFilter< PixelType >::DoubleDwiType::Pointer TractsToDWIImageFilter< PixelType >::SimulateKspaceAcquisition( std::vector< DoubleDwiType::Pointer >& images )
  {
    int numFiberCompartments = m_Parameters.m_FiberModelList.size();
    // create slice object
    ImageRegion<2> sliceRegion;
    sliceRegion.SetSize(0, m_WorkingImageRegion.GetSize()[0]);
    sliceRegion.SetSize(1, m_WorkingImageRegion.GetSize()[1]);
    Vector< double, 2 > sliceSpacing;
    sliceSpacing[0] = m_WorkingSpacing[0];
    sliceSpacing[1] = m_WorkingSpacing[1];

    DoubleDwiType::PixelType nullPix; nullPix.SetSize(images.at(0)->GetVectorLength()); nullPix.Fill(0.0);
    auto magnitudeDwiImage = DoubleDwiType::New();
    magnitudeDwiImage->SetSpacing( m_Parameters.m_SignalGen.m_ImageSpacing );
    magnitudeDwiImage->SetOrigin( m_Parameters.m_SignalGen.m_ImageOrigin );
    magnitudeDwiImage->SetDirection( m_Parameters.m_SignalGen.m_ImageDirection );
    magnitudeDwiImage->SetLargestPossibleRegion( m_Parameters.m_SignalGen.m_CroppedRegion );
    magnitudeDwiImage->SetBufferedRegion( m_Parameters.m_SignalGen.m_CroppedRegion );
    magnitudeDwiImage->SetRequestedRegion( m_Parameters.m_SignalGen.m_CroppedRegion );
    magnitudeDwiImage->SetVectorLength( images.at(0)->GetVectorLength() );
    magnitudeDwiImage->Allocate();
    magnitudeDwiImage->FillBuffer(nullPix);

    m_PhaseImage = DoubleDwiType::New();
    m_PhaseImage->SetSpacing( m_Parameters.m_SignalGen.m_ImageSpacing );
    m_PhaseImage->SetOrigin( m_Parameters.m_SignalGen.m_ImageOrigin );
    m_PhaseImage->SetDirection( m_Parameters.m_SignalGen.m_ImageDirection );
    m_PhaseImage->SetLargestPossibleRegion( m_Parameters.m_SignalGen.m_CroppedRegion );
    m_PhaseImage->SetBufferedRegion( m_Parameters.m_SignalGen.m_CroppedRegion );
    m_PhaseImage->SetRequestedRegion( m_Parameters.m_SignalGen.m_CroppedRegion );
    m_PhaseImage->SetVectorLength( images.at(0)->GetVectorLength() );
    m_PhaseImage->Allocate();
    m_PhaseImage->FillBuffer(nullPix);

    m_KspaceImage = DoubleDwiType::New();
    m_KspaceImage->SetSpacing( m_Parameters.m_SignalGen.m_ImageSpacing );
    m_KspaceImage->SetOrigin( m_Parameters.m_SignalGen.m_ImageOrigin );
    m_KspaceImage->SetDirection( m_Parameters.m_SignalGen.m_ImageDirection );
    m_KspaceImage->SetLargestPossibleRegion( m_Parameters.m_SignalGen.m_CroppedRegion );
    m_KspaceImage->SetBufferedRegion( m_Parameters.m_SignalGen.m_CroppedRegion );
    m_KspaceImage->SetRequestedRegion( m_Parameters.m_SignalGen.m_CroppedRegion );
    m_KspaceImage->SetVectorLength( m_Parameters.m_SignalGen.m_NumberOfCoils );
    m_KspaceImage->Allocate();
    m_KspaceImage->FillBuffer(nullPix);

    std::vector< unsigned int > spikeVolume;
    for (unsigned int i=0; i<m_Parameters.m_SignalGen.m_Spikes; i++)
      spikeVolume.push_back(m_RandGen->GetIntegerVariate()%(images.at(0)->GetVectorLength()));
    std::sort (spikeVolume.begin(), spikeVolume.end());
    std::reverse (spikeVolume.begin(), spikeVolume.end());

    // calculate coil positions
    double a = m_Parameters.m_SignalGen.m_ImageRegion.GetSize(0)*m_Parameters.m_SignalGen.m_ImageSpacing[0];
    double b = m_Parameters.m_SignalGen.m_ImageRegion.GetSize(1)*m_Parameters.m_SignalGen.m_ImageSpacing[1];
    double c = m_Parameters.m_SignalGen.m_ImageRegion.GetSize(2)*m_Parameters.m_SignalGen.m_ImageSpacing[2];
    double diagonal = sqrt(a*a+b*b)/1000;   // image diagonal in m

    m_CoilPointset = mitk::PointSet::New();
    std::vector< itk::Vector<double, 3> > coilPositions;
    itk::Vector<double, 3> pos; pos.Fill(0.0); pos[1] = -diagonal/2;
    itk::Vector<double, 3> center;
    center[0] = a/2-m_Parameters.m_SignalGen.m_ImageSpacing[0]/2;
    center[1] = b/2-m_Parameters.m_SignalGen.m_ImageSpacing[2]/2;
    center[2] = c/2-m_Parameters.m_SignalGen.m_ImageSpacing[1]/2;
    for (int c=0; c<m_Parameters.m_SignalGen.m_NumberOfCoils; c++)
    {
      coilPositions.push_back(pos);
      m_CoilPointset->InsertPoint(c, pos*1000 + m_Parameters.m_SignalGen.m_ImageOrigin.GetVectorFromOrigin() + center );

      double rz = 360.0/m_Parameters.m_SignalGen.m_NumberOfCoils * M_PI/180;
      vnl_matrix_fixed< double, 3, 3 > rotZ; rotZ.set_identity();
      rotZ[0][0] = cos(rz);
      rotZ[1][1] = rotZ[0][0];
      rotZ[0][1] = -sin(rz);
      rotZ[1][0] = -rotZ[0][1];

      pos.SetVnlVector(rotZ*pos.GetVnlVector());
    }

    PrintToLog("0%   10   20   30   40   50   60   70   80   90   100%", false, true, false);
    PrintToLog("|----|----|----|----|----|----|----|----|----|----|\n*", false, false, false);
    unsigned long lastTick = 0;

    boost::progress_display disp(images.at(0)->GetVectorLength()*images.at(0)->GetLargestPossibleRegion().GetSize(2));
    for (unsigned int g=0; g<images.at(0)->GetVectorLength(); g++)
    {
      std::vector< unsigned int > spikeSlice;
      while (!spikeVolume.empty() && spikeVolume.back()==g)
      {
        spikeSlice.push_back(m_RandGen->GetIntegerVariate()%images.at(0)->GetLargestPossibleRegion().GetSize(2));
        spikeVolume.pop_back();
      }
      std::sort (spikeSlice.begin(), spikeSlice.end());
      std::reverse (spikeSlice.begin(), spikeSlice.end());

      for (unsigned int z=0; z<images.at(0)->GetLargestPossibleRegion().GetSize(2); z++)
      {
        std::vector< SliceType::Pointer > compartmentSlices;
        std::vector< double > t2Vector;
        std::vector< double > t1Vector;

        for (unsigned int i=0; i<images.size(); i++)
        {
          DiffusionSignalModel<double>* signalModel;
          if (i<numFiberCompartments)
            signalModel = m_Parameters.m_FiberModelList.at(i);
          else
            signalModel = m_Parameters.m_NonFiberModelList.at(i-numFiberCompartments);

          auto slice = SliceType::New();
          slice->SetLargestPossibleRegion( sliceRegion );
          slice->SetBufferedRegion( sliceRegion );
          slice->SetRequestedRegion( sliceRegion );
          slice->SetSpacing(sliceSpacing);
          slice->Allocate();
          slice->FillBuffer(0.0);

          // extract slice from channel g
          for (unsigned int y=0; y<images.at(0)->GetLargestPossibleRegion().GetSize(1); y++)
            for (unsigned int x=0; x<images.at(0)->GetLargestPossibleRegion().GetSize(0); x++)
            {
              SliceType::IndexType index2D; index2D[0]=x; index2D[1]=y;
              DoubleDwiType::IndexType index3D; index3D[0]=x; index3D[1]=y; index3D[2]=z;

              slice->SetPixel(index2D, images.at(i)->GetPixel(index3D)[g]);
            }

          compartmentSlices.push_back(slice);
          t2Vector.push_back(signalModel->GetT2());
          t1Vector.push_back(signalModel->GetT1());
        }

        int numSpikes = 0;
        while (!spikeSlice.empty() && spikeSlice.back()==z)
        {
          numSpikes++;
          spikeSlice.pop_back();
        }
        int spikeCoil = m_RandGen->GetIntegerVariate()%m_Parameters.m_SignalGen.m_NumberOfCoils;

        if (this->GetAbortGenerateData())
          return NULL;

#pragma omp parallel for
        for (int c=0; c<m_Parameters.m_SignalGen.m_NumberOfCoils; c++)
        {
          // create k-sapce (inverse fourier transform slices)
          auto idft = itk::KspaceImageFilter< SliceType::PixelType >::New();
          idft->SetCompartmentImages(compartmentSlices);
          idft->SetT2(t2Vector);
          idft->SetT1(t1Vector);
          idft->SetUseConstantRandSeed(m_UseConstantRandSeed);
          idft->SetParameters(&m_Parameters);
          idft->SetZ((double)z-(double)(images.at(0)->GetLargestPossibleRegion().GetSize(2)-images.at(0)->GetLargestPossibleRegion().GetSize(2)%2)/2.0);
          idft->SetZidx(z);
          idft->SetCoilPosition(coilPositions.at(c));
          idft->SetFiberBundle(m_FiberBundleWorkingCopy);
          idft->SetTranslation(m_Translations.at(g));
          idft->SetRotation(m_Rotations.at(g));
          idft->SetDiffusionGradientDirection(m_Parameters.m_SignalGen.GetGradientDirection(g));
          if (c==spikeCoil)
            idft->SetSpikesPerSlice(numSpikes);
          idft->Update();

#pragma omp critical
          if (c==spikeCoil && numSpikes>0)
          {
            m_SpikeLog += "Volume " + boost::lexical_cast<std::string>(g) + " Coil " + boost::lexical_cast<std::string>(c) + "\n";
            m_SpikeLog += idft->GetSpikeLog();
          }

          ComplexSliceType::Pointer fSlice;
          fSlice = idft->GetOutput();

          // fourier transform slice
          ComplexSliceType::Pointer newSlice;
          auto dft = itk::DftImageFilter< SliceType::PixelType >::New();
          dft->SetInput(fSlice);
          dft->SetParameters(m_Parameters);
          dft->Update();
          newSlice = dft->GetOutput();

          // put slice back into channel g
          for (unsigned int y=0; y<fSlice->GetLargestPossibleRegion().GetSize(1); y++)
            for (unsigned int x=0; x<fSlice->GetLargestPossibleRegion().GetSize(0); x++)
            {
              DoubleDwiType::IndexType index3D; index3D[0]=x; index3D[1]=y; index3D[2]=z;
              ComplexSliceType::IndexType index2D; index2D[0]=x; index2D[1]=y;

              ComplexSliceType::PixelType cPix = newSlice->GetPixel(index2D);
              double magn = sqrt(cPix.real()*cPix.real()+cPix.imag()*cPix.imag());
              double phase = 0;
              if (cPix.real()!=0)
                phase = atan( cPix.imag()/cPix.real() );

              DoubleDwiType::PixelType dwiPix = magnitudeDwiImage->GetPixel(index3D);
              DoubleDwiType::PixelType phasePix = m_PhaseImage->GetPixel(index3D);

              if (m_Parameters.m_SignalGen.m_NumberOfCoils>1)
              {
                dwiPix[g] += magn*magn;
                phasePix[g] += phase*phase;
              }
              else
              {
                dwiPix[g] = magn;
                phasePix[g] = phase;
              }

#pragma omp critical
              {
                magnitudeDwiImage->SetPixel(index3D, dwiPix);
                m_PhaseImage->SetPixel(index3D, phasePix);

                // k-space image
                if (g==0)
                {
                  DoubleDwiType::PixelType kspacePix = m_KspaceImage->GetPixel(index3D);
                  kspacePix[c] = idft->GetKSpaceImage()->GetPixel(index2D);
                  m_KspaceImage->SetPixel(index3D, kspacePix);
                }
              }
            }
        }

        if (m_Parameters.m_SignalGen.m_NumberOfCoils>1)
        {
#ifdef WIN32
#pragma omp parallel for
#else
#pragma omp parallel for collapse(2)
#endif
          for (int y=0; y<magnitudeDwiImage->GetLargestPossibleRegion().GetSize(1); y++)
            for (int x=0; x<magnitudeDwiImage->GetLargestPossibleRegion().GetSize(0); x++)
            {
              DoubleDwiType::IndexType index3D; index3D[0]=x; index3D[1]=y; index3D[2]=z;
              DoubleDwiType::PixelType magPix = magnitudeDwiImage->GetPixel(index3D);
              magPix[g] = sqrt(magPix[g]/m_Parameters.m_SignalGen.m_NumberOfCoils);

              DoubleDwiType::PixelType phasePix = m_PhaseImage->GetPixel(index3D);
              phasePix[g] = sqrt(phasePix[g]/m_Parameters.m_SignalGen.m_NumberOfCoils);

#pragma omp critical
              {
                magnitudeDwiImage->SetPixel(index3D, magPix);
                m_PhaseImage->SetPixel(index3D, phasePix);
              }
            }
        }

        ++disp;
        unsigned long newTick = 50*disp.count()/disp.expected_count();
        for (unsigned long tick = 0; tick<(newTick-lastTick); tick++)
          PrintToLog("*", false, false, false);
        lastTick = newTick;
      }
    }
    PrintToLog("\n", false);
    return magnitudeDwiImage;
  }

  template< class PixelType >
  TractsToDWIImageFilter< PixelType >::ItkDoubleImgType::Pointer TractsToDWIImageFilter< PixelType >::NormalizeInsideMask(ItkDoubleImgType::Pointer image)
  {
    double max = itk::NumericTraits< double >::min();
    double min = itk::NumericTraits< double >::max();

    itk::ImageRegionIterator< ItkDoubleImgType > it(image, image->GetLargestPossibleRegion());
    while(!it.IsAtEnd())
    {
      if (m_Parameters.m_SignalGen.m_MaskImage.IsNotNull() && m_Parameters.m_SignalGen.m_MaskImage->GetPixel(it.GetIndex())<=0)
      {
        it.Set(0.0);
        ++it;
        continue;
      }
      //        if (it.Get()>900)
      //            it.Set(900);

      if (it.Get()>max)
        max = it.Get();
      if (it.Get()<min)
        min = it.Get();
      ++it;
    }
    auto scaler = itk::ShiftScaleImageFilter< ItkDoubleImgType, ItkDoubleImgType >::New();
    scaler->SetInput(image);
    scaler->SetShift(-min);
    scaler->SetScale(1.0/(max-min));
    scaler->Update();
    return scaler->GetOutput();
  }

  template< class PixelType >
  void TractsToDWIImageFilter< PixelType >::CheckVolumeFractionImages()
  {
    m_UseRelativeNonFiberVolumeFractions = false;

    // check for fiber volume fraction maps
    int fibVolImages = 0;
    for (int i=0; i<m_Parameters.m_FiberModelList.size(); i++)
      if (m_Parameters.m_FiberModelList[i]->GetVolumeFractionImage().IsNotNull())
      {
        PrintToLog("Using volume fraction map for fiber compartment " + boost::lexical_cast<std::string>(i+1));
        fibVolImages++;
      }

    // check for non-fiber volume fraction maps
    int nonfibVolImages = 0;
    for (int i=0; i<m_Parameters.m_NonFiberModelList.size(); i++)
      if (m_Parameters.m_NonFiberModelList[i]->GetVolumeFractionImage().IsNotNull())
      {
        PrintToLog("Using volume fraction map for non-fiber compartment " + boost::lexical_cast<std::string>(i+1));
        nonfibVolImages++;
      }

    // not all fiber compartments are using volume fraction maps --> non-fiber volume fractions are assumed to be relative to the non-fiber volume and not absolute voxel-volume fractions.
    // this means if two non-fiber compartments are used but only one of them has an associated volume fraction map, the repesctive other volume fraction map can be determined as inverse (1-val) of the present volume fraction map-
    if ( fibVolImages<m_Parameters.m_FiberModelList.size() && nonfibVolImages==1 && m_Parameters.m_NonFiberModelList.size()==2)
    {
      PrintToLog("Calculating missing non-fiber volume fraction image by inverting the other.\nAssuming non-fiber volume fraction images to contain values relative to the remaining non-fiber volume, not absolute values.");

      auto inverter = itk::InvertIntensityImageFilter< ItkDoubleImgType, ItkDoubleImgType >::New();
      inverter->SetMaximum(1.0);
      if ( m_Parameters.m_NonFiberModelList[0]->GetVolumeFractionImage().IsNull() && m_Parameters.m_NonFiberModelList[1]->GetVolumeFractionImage().IsNotNull() )
      {
        //            m_Parameters.m_NonFiberModelList[1]->SetVolumeFractionImage( NormalizeInsideMask( m_Parameters.m_NonFiberModelList[1]->GetVolumeFractionImage() ) );
        inverter->SetInput( m_Parameters.m_NonFiberModelList[1]->GetVolumeFractionImage() );
        inverter->Update();
        m_Parameters.m_NonFiberModelList[0]->SetVolumeFractionImage(inverter->GetOutput());
      }
      else if ( m_Parameters.m_NonFiberModelList[1]->GetVolumeFractionImage().IsNull() && m_Parameters.m_NonFiberModelList[0]->GetVolumeFractionImage().IsNotNull() )
      {
        //            m_Parameters.m_NonFiberModelList[0]->SetVolumeFractionImage( NormalizeInsideMask( m_Parameters.m_NonFiberModelList[0]->GetVolumeFractionImage() ) );
        inverter->SetInput( m_Parameters.m_NonFiberModelList[0]->GetVolumeFractionImage() );
        inverter->Update();
        m_Parameters.m_NonFiberModelList[1]->SetVolumeFractionImage(inverter->GetOutput());
      }
      else
      {
        itkExceptionMacro("Something went wrong in automatically calculating the missing non-fiber volume fraction image! Did you use two non fiber compartments but only one volume fraction image? Then it should work and this error is really strange.");
      }
      m_UseRelativeNonFiberVolumeFractions = true;

      nonfibVolImages++;
    }

    // Up to two fiber compartments are allowed without volume fraction maps since the volume fractions can then be determined automatically
    if (m_Parameters.m_FiberModelList.size()>2 && fibVolImages!=m_Parameters.m_FiberModelList.size())
      itkExceptionMacro("More than two fiber compartment selected but no corresponding volume fraction maps set!");

    // One non-fiber compartment is allowed without volume fraction map since the volume fraction can then be determined automatically
    if (m_Parameters.m_NonFiberModelList.size()>1 && nonfibVolImages!=m_Parameters.m_NonFiberModelList.size())
      itkExceptionMacro("More than one non-fiber compartment selected but no volume fraction maps set!");

    if (fibVolImages<m_Parameters.m_FiberModelList.size() && nonfibVolImages>0)
    {
      PrintToLog("Not all fiber compartments are using an associated volume fraction image.\nAssuming non-fiber volume fraction images to contain values relative to the remaining non-fiber volume, not absolute values.");
      m_UseRelativeNonFiberVolumeFractions = true;

      //        itk::ImageFileWriter<ItkDoubleImgType>::Pointer wr = itk::ImageFileWriter<ItkDoubleImgType>::New();
      //        wr->SetInput(m_Parameters.m_NonFiberModelList[1]->GetVolumeFractionImage());
      //        wr->SetFileName("/local/volumefraction.nrrd");
      //        wr->Update();
    }

    // initialize the images that store the output volume fraction of each compartment
    m_VolumeFractions.clear();
    for (int i=0; i<m_Parameters.m_FiberModelList.size()+m_Parameters.m_NonFiberModelList.size(); i++)
    {
      auto doubleImg = ItkDoubleImgType::New();
      doubleImg->SetSpacing( m_WorkingSpacing );
      doubleImg->SetOrigin( m_WorkingOrigin );
      doubleImg->SetDirection( m_Parameters.m_SignalGen.m_ImageDirection );
      doubleImg->SetLargestPossibleRegion( m_WorkingImageRegion );
      doubleImg->SetBufferedRegion( m_WorkingImageRegion );
      doubleImg->SetRequestedRegion( m_WorkingImageRegion );
      doubleImg->Allocate();
      doubleImg->FillBuffer(0);
      m_VolumeFractions.push_back(doubleImg);
    }
  }

  template< class PixelType >
  void TractsToDWIImageFilter< PixelType >::InitializeData()
  {
    m_Rotations.clear();
    m_Translations.clear();
    m_MotionLog = "";
    m_SpikeLog = "";

    // initialize output dwi image
    m_Parameters.m_SignalGen.m_CroppedRegion = m_Parameters.m_SignalGen.m_ImageRegion; m_Parameters.m_SignalGen.m_CroppedRegion.SetSize(1, m_Parameters.m_SignalGen.m_CroppedRegion.GetSize(1)*m_Parameters.m_SignalGen.m_CroppingFactor);
    itk::Point<double,3> shiftedOrigin = m_Parameters.m_SignalGen.m_ImageOrigin; shiftedOrigin[1] += (m_Parameters.m_SignalGen.m_ImageRegion.GetSize(1)-m_Parameters.m_SignalGen.m_CroppedRegion.GetSize(1))*m_Parameters.m_SignalGen.m_ImageSpacing[1]/2;
    m_OutputImage = OutputImageType::New();
    m_OutputImage->SetSpacing( m_Parameters.m_SignalGen.m_ImageSpacing );
    m_OutputImage->SetOrigin( shiftedOrigin );
    m_OutputImage->SetDirection( m_Parameters.m_SignalGen.m_ImageDirection );
    m_OutputImage->SetLargestPossibleRegion( m_Parameters.m_SignalGen.m_CroppedRegion );
    m_OutputImage->SetBufferedRegion( m_Parameters.m_SignalGen.m_CroppedRegion );
    m_OutputImage->SetRequestedRegion( m_Parameters.m_SignalGen.m_CroppedRegion );
    m_OutputImage->SetVectorLength( m_Parameters.m_SignalGen.GetNumVolumes() );
    m_OutputImage->Allocate();
    typename OutputImageType::PixelType temp;
    temp.SetSize(m_Parameters.m_SignalGen.GetNumVolumes());
    temp.Fill(0.0);
    m_OutputImage->FillBuffer(temp);

    // Apply in-plane upsampling for Gibbs ringing artifact
    double upsampling = 1;
    if (m_Parameters.m_SignalGen.m_DoAddGibbsRinging)
      upsampling = 2;
    m_WorkingSpacing = m_Parameters.m_SignalGen.m_ImageSpacing;
    m_WorkingSpacing[0] /= upsampling;
    m_WorkingSpacing[1] /= upsampling;
    m_WorkingImageRegion = m_Parameters.m_SignalGen.m_ImageRegion;
    m_WorkingImageRegion.SetSize(0, m_Parameters.m_SignalGen.m_ImageRegion.GetSize()[0]*upsampling);
    m_WorkingImageRegion.SetSize(1, m_Parameters.m_SignalGen.m_ImageRegion.GetSize()[1]*upsampling);
    m_WorkingOrigin = m_Parameters.m_SignalGen.m_ImageOrigin;
    m_WorkingOrigin[0] -= m_Parameters.m_SignalGen.m_ImageSpacing[0]/2; m_WorkingOrigin[0] += m_WorkingSpacing[0]/2;
    m_WorkingOrigin[1] -= m_Parameters.m_SignalGen.m_ImageSpacing[1]/2; m_WorkingOrigin[1] += m_WorkingSpacing[1]/2;
    m_WorkingOrigin[2] -= m_Parameters.m_SignalGen.m_ImageSpacing[2]/2; m_WorkingOrigin[2] += m_WorkingSpacing[2]/2;
    m_VoxelVolume = m_WorkingSpacing[0]*m_WorkingSpacing[1]*m_WorkingSpacing[2];

    // generate double images to store the individual compartment signals
    m_CompartmentImages.clear();
    int numFiberCompartments = m_Parameters.m_FiberModelList.size();
    int numNonFiberCompartments = m_Parameters.m_NonFiberModelList.size();
    for (int i=0; i<numFiberCompartments+numNonFiberCompartments; i++)
    {
      auto doubleDwi = DoubleDwiType::New();
      doubleDwi->SetSpacing( m_WorkingSpacing );
      doubleDwi->SetOrigin( m_WorkingOrigin );
      doubleDwi->SetDirection( m_Parameters.m_SignalGen.m_ImageDirection );
      doubleDwi->SetLargestPossibleRegion( m_WorkingImageRegion );
      doubleDwi->SetBufferedRegion( m_WorkingImageRegion );
      doubleDwi->SetRequestedRegion( m_WorkingImageRegion );
      doubleDwi->SetVectorLength( m_Parameters.m_SignalGen.GetNumVolumes() );
      doubleDwi->Allocate();
      DoubleDwiType::PixelType pix;
      pix.SetSize(m_Parameters.m_SignalGen.GetNumVolumes());
      pix.Fill(0.0);
      doubleDwi->FillBuffer(pix);
      m_CompartmentImages.push_back(doubleDwi);
    }

    if (m_FiberBundle.IsNull() && m_InputImage.IsNotNull())
    {
      m_CompartmentImages.clear();

      m_Parameters.m_SignalGen.m_DoAddMotion = false;
      m_Parameters.m_SignalGen.m_DoSimulateRelaxation = false;

      PrintToLog("Simulating acquisition for input diffusion-weighted image.", false);
      auto caster = itk::CastImageFilter< OutputImageType, DoubleDwiType >::New();
      caster->SetInput(m_InputImage);
      caster->Update();

      if (m_Parameters.m_SignalGen.m_DoAddGibbsRinging)
      {
        PrintToLog("Upsampling input diffusion-weighted image for Gibbs ringing simulation.", false);

        auto resampler = itk::ResampleDwiImageFilter< double >::New();
        resampler->SetInput(caster->GetOutput());
        itk::Vector< double, 3 > samplingFactor;
        samplingFactor[0] = upsampling;
        samplingFactor[1] = upsampling;
        samplingFactor[2] = 1;
        resampler->SetSamplingFactor(samplingFactor);
        resampler->SetInterpolation(itk::ResampleDwiImageFilter< double >::Interpolate_WindowedSinc);
        resampler->Update();
        m_CompartmentImages.push_back(resampler->GetOutput());
      }
      else
        m_CompartmentImages.push_back(caster->GetOutput());

      for (unsigned int g=0; g<m_Parameters.m_SignalGen.GetNumVolumes(); g++)
      {
        m_Rotation.Fill(0.0);
        m_Translation.Fill(0.0);
        m_Rotations.push_back(m_Rotation);
        m_Translations.push_back(m_Translation);
      }
    }

    // resample mask image and frequency map to fit upsampled geometry
    if (m_Parameters.m_SignalGen.m_DoAddGibbsRinging)
    {
      if (m_Parameters.m_SignalGen.m_MaskImage.IsNotNull())
      {
        // rescale mask image (otherwise there are problems with the resampling)
        auto rescaler = itk::RescaleIntensityImageFilter<ItkUcharImgType,ItkUcharImgType>::New();
        rescaler->SetInput(0,m_Parameters.m_SignalGen.m_MaskImage);
        rescaler->SetOutputMaximum(100);
        rescaler->SetOutputMinimum(0);
        rescaler->Update();

        // resample mask image
        auto resampler = itk::ResampleImageFilter<ItkUcharImgType, ItkUcharImgType>::New();
        resampler->SetInput(rescaler->GetOutput());
        resampler->SetOutputParametersFromImage(m_Parameters.m_SignalGen.m_MaskImage);
        resampler->SetSize(m_WorkingImageRegion.GetSize());
        resampler->SetOutputSpacing(m_WorkingSpacing);
        resampler->SetOutputOrigin(m_WorkingOrigin);
        auto nn_interpolator = itk::NearestNeighborInterpolateImageFunction<ItkUcharImgType, double>::New();
        resampler->SetInterpolator(nn_interpolator);
        resampler->Update();
        m_Parameters.m_SignalGen.m_MaskImage = resampler->GetOutput();
      }
      // resample frequency map
      if (m_Parameters.m_SignalGen.m_FrequencyMap.IsNotNull())
      {
        auto resampler = itk::ResampleImageFilter<ItkDoubleImgType, ItkDoubleImgType>::New();
        resampler->SetInput(m_Parameters.m_SignalGen.m_FrequencyMap);
        resampler->SetOutputParametersFromImage(m_Parameters.m_SignalGen.m_FrequencyMap);
        resampler->SetSize(m_WorkingImageRegion.GetSize());
        resampler->SetOutputSpacing(m_WorkingSpacing);
        resampler->SetOutputOrigin(m_WorkingOrigin);
        auto nn_interpolator = itk::NearestNeighborInterpolateImageFunction<ItkDoubleImgType, double>::New();
        resampler->SetInterpolator(nn_interpolator);
        resampler->Update();
        m_Parameters.m_SignalGen.m_FrequencyMap = resampler->GetOutput();
      }
    }

    m_MaskImageSet = true;
    if (m_Parameters.m_SignalGen.m_MaskImage.IsNull())
    {
      // no input tissue mask is set -> create default
      PrintToLog("No tissue mask set", false);
      m_Parameters.m_SignalGen.m_MaskImage = ItkUcharImgType::New();
      m_Parameters.m_SignalGen.m_MaskImage->SetSpacing( m_WorkingSpacing );
      m_Parameters.m_SignalGen.m_MaskImage->SetOrigin( m_WorkingOrigin );
      m_Parameters.m_SignalGen.m_MaskImage->SetDirection( m_Parameters.m_SignalGen.m_ImageDirection );
      m_Parameters.m_SignalGen.m_MaskImage->SetLargestPossibleRegion( m_WorkingImageRegion );
      m_Parameters.m_SignalGen.m_MaskImage->SetBufferedRegion( m_WorkingImageRegion );
      m_Parameters.m_SignalGen.m_MaskImage->SetRequestedRegion( m_WorkingImageRegion );
      m_Parameters.m_SignalGen.m_MaskImage->Allocate();
      m_Parameters.m_SignalGen.m_MaskImage->FillBuffer(100);
      m_MaskImageSet = false;
    }
    else
    {
      if (m_Parameters.m_SignalGen.m_MaskImage->GetLargestPossibleRegion()!=m_WorkingImageRegion)
        itkExceptionMacro("Mask image and specified DWI geometry are not matching!");

      PrintToLog("Using tissue mask", false);
    }

    if (m_Parameters.m_SignalGen.m_DoAddMotion)
    {
      if (m_Parameters.m_SignalGen.m_DoRandomizeMotion)
      {
        PrintToLog("Random motion artifacts:", false);
        PrintToLog("Maximum rotation: +/-" + boost::lexical_cast<std::string>(m_Parameters.m_SignalGen.m_Rotation) + "°", false);
        PrintToLog("Maximum translation: +/-" + boost::lexical_cast<std::string>(m_Parameters.m_SignalGen.m_Translation) + "mm", false);
      }
      else
      {
        PrintToLog("Linear motion artifacts:", false);
        PrintToLog("Maximum rotation: " + boost::lexical_cast<std::string>(m_Parameters.m_SignalGen.m_Rotation) + "°", false);
        PrintToLog("Maximum translation: " + boost::lexical_cast<std::string>(m_Parameters.m_SignalGen.m_Translation) + "mm", false);
      }
    }
    if ( m_Parameters.m_SignalGen.m_MotionVolumes.empty() )
    {
      // no motion in first volume
      m_Parameters.m_SignalGen.m_MotionVolumes.push_back(false);

      // motion in all other volumes
      while ( m_Parameters.m_SignalGen.m_MotionVolumes.size()<m_Parameters.m_SignalGen.GetNumVolumes() )
        m_Parameters.m_SignalGen.m_MotionVolumes.push_back(true);
    }
    // we need to know for every volume if there is motion. if this information is missing, then set corresponding fal to false
    while ( m_Parameters.m_SignalGen.m_MotionVolumes.size()<m_Parameters.m_SignalGen.GetNumVolumes() )
      m_Parameters.m_SignalGen.m_MotionVolumes.push_back(false);
    m_NumMotionVolumes = 0;
    for (int i=0; i<m_Parameters.m_SignalGen.GetNumVolumes(); i++)
      if (m_Parameters.m_SignalGen.m_MotionVolumes[i])
        m_NumMotionVolumes++;
    m_MotionCounter = 0;

    // creat image to hold transformed mask (motion artifact)
    m_TransformedMaskImage = ItkUcharImgType::New();
    auto duplicator = itk::ImageDuplicator<ItkUcharImgType>::New();
    duplicator->SetInputImage(m_Parameters.m_SignalGen.m_MaskImage);
    duplicator->Update();
    m_TransformedMaskImage = duplicator->GetOutput();

    // second upsampling needed for motion artifacts
    ImageRegion<3>      upsampledImageRegion = m_WorkingImageRegion;
    DoubleVectorType    upsampledSpacing = m_WorkingSpacing;
    upsampledSpacing[0] /= 4;
    upsampledSpacing[1] /= 4;
    upsampledSpacing[2] /= 4;
    upsampledImageRegion.SetSize(0, m_WorkingImageRegion.GetSize()[0]*4);
    upsampledImageRegion.SetSize(1, m_WorkingImageRegion.GetSize()[1]*4);
    upsampledImageRegion.SetSize(2, m_WorkingImageRegion.GetSize()[2]*4);
    itk::Point<double,3> upsampledOrigin = m_WorkingOrigin;
    upsampledOrigin[0] -= m_WorkingSpacing[0]/2; upsampledOrigin[0] += upsampledSpacing[0]/2;
    upsampledOrigin[1] -= m_WorkingSpacing[1]/2; upsampledOrigin[1] += upsampledSpacing[1]/2;
    upsampledOrigin[2] -= m_WorkingSpacing[2]/2; upsampledOrigin[2] += upsampledSpacing[2]/2;
    m_UpsampledMaskImage = ItkUcharImgType::New();
    auto upsampler = itk::ResampleImageFilter<ItkUcharImgType, ItkUcharImgType>::New();
    upsampler->SetInput(m_Parameters.m_SignalGen.m_MaskImage);
    upsampler->SetOutputParametersFromImage(m_Parameters.m_SignalGen.m_MaskImage);
    upsampler->SetSize(upsampledImageRegion.GetSize());
    upsampler->SetOutputSpacing(upsampledSpacing);
    upsampler->SetOutputOrigin(upsampledOrigin);
    auto nn_interpolator = itk::NearestNeighborInterpolateImageFunction<ItkUcharImgType, double>::New();
    upsampler->SetInterpolator(nn_interpolator);
    upsampler->Update();
    m_UpsampledMaskImage = upsampler->GetOutput();
  }

  template< class PixelType >
  void TractsToDWIImageFilter< PixelType >::InitializeFiberData()
  {
    // resample fiber bundle for sufficient voxel coverage
    PrintToLog("Resampling fibers ...");
    m_SegmentVolume = 0.0001;
    float minSpacing = 1;
    if(m_WorkingSpacing[0]<m_WorkingSpacing[1] && m_WorkingSpacing[0]<m_WorkingSpacing[2])
      minSpacing = m_WorkingSpacing[0];
    else if (m_WorkingSpacing[1] < m_WorkingSpacing[2])
      minSpacing = m_WorkingSpacing[1];
    else
      minSpacing = m_WorkingSpacing[2];
    // working copy is needed because we need to resample the fibers but do not want to change the original bundle
    m_FiberBundleWorkingCopy = m_FiberBundle->GetDeepCopy();
    double volumeAccuracy = 10;
    m_FiberBundleWorkingCopy->ResampleSpline(minSpacing/volumeAccuracy);
    m_mmRadius = m_Parameters.m_SignalGen.m_AxonRadius/1000;
    if (m_mmRadius>0)
      m_SegmentVolume = M_PI*m_mmRadius*m_mmRadius*minSpacing/volumeAccuracy;
    // a second fiber bundle is needed to store the transformed version of the m_FiberBundleWorkingCopy
    m_FiberBundleTransformed = m_FiberBundleWorkingCopy;
  }

  template< class PixelType >
  bool TractsToDWIImageFilter< PixelType >::PrepareLogFile()
  {
    assert( ! m_Logfile.is_open() );

    std::string filePath;
    std::string fileName;

    // Get directory name:
    if (m_Parameters.m_Misc.m_OutputPath.size() > 0)
    {
      filePath = m_Parameters.m_Misc.m_OutputPath;
      if( *(--(filePath.cend())) != '/')
      {
        filePath.push_back('/');
      }
    }
    else
    {
      filePath = mitk::IOUtil::GetTempPath() + '/';
    }
    // check if directory exists, else use /tmp/:
    if( itksys::SystemTools::FileIsDirectory( filePath ) )
    {
      while( *(--(filePath.cend())) == '/')
      {
        filePath.pop_back();
      }
      filePath = filePath + '/';
    }
    else
    {
      filePath = mitk::IOUtil::GetTempPath() + '/';
    }

    // Get file name:
    if( ! m_Parameters.m_Misc.m_ResultNode->GetName().empty() )
    {
      fileName = m_Parameters.m_Misc.m_ResultNode->GetName();
    }
    else
    {
      fileName = "";
    }

    if( ! m_Parameters.m_Misc.m_OutputPrefix.empty() )
    {
      fileName = m_Parameters.m_Misc.m_OutputPrefix + fileName;
    }
    else
    {
      fileName = "fiberfox";
    }

    // check if file already exists and DO NOT overwrite existing files:
    std::string NameTest = fileName;
    int c = 0;
    while( itksys::SystemTools::FileExists( filePath + '/' + fileName + ".log" )
           && c <= std::numeric_limits<int>::max() )
    {
      fileName = NameTest + "_" + boost::lexical_cast<std::string>(c);
      ++c;
    }

    try
    {
      m_Logfile.open( ( filePath + '/' + fileName + ".log" ).c_str() );
    }
    catch (const std::ios_base::failure &fail)
    {
      MITK_ERROR << "itkTractsToDWIImageFilter.cpp: Exception " << fail.what()
                 << " while trying to open file" << filePath << '/' << fileName << ".log";
      return false;
    }

    if ( m_Logfile.is_open() )
    {
      PrintToLog( "Logfile: " + filePath + '/' + fileName + ".log", false );
      return true;
    }
    else
    {
      m_StatusText += "Logfile could not be opened!\n";
      MITK_ERROR << "itkTractsToDWIImageFilter.cpp: Logfile could not be opened!";
      return false;
    }
  }

  template< class PixelType >
  void TractsToDWIImageFilter< PixelType >::GenerateData()
  {
    // prepare logfile
    if ( ! PrepareLogFile() )
    {
      this->SetAbortGenerateData( true );
      return;
    }

    m_TimeProbe.Start();

    // check input data
    if (m_FiberBundle.IsNull() && m_InputImage.IsNull())
      itkExceptionMacro("Input fiber bundle and input diffusion-weighted image is NULL!");

    if (m_Parameters.m_FiberModelList.empty() && m_InputImage.IsNull())
      itkExceptionMacro("No diffusion model for fiber compartments defined and input diffusion-weighted"
                        " image is NULL! At least one fiber compartment is necessary to simulate diffusion.");

    if (m_Parameters.m_NonFiberModelList.empty() && m_InputImage.IsNull())
      itkExceptionMacro("No diffusion model for non-fiber compartments defined and input diffusion-weighted"
                        " image is NULL! At least one non-fiber compartment is necessary to simulate diffusion.");

    int baselineIndex = m_Parameters.m_SignalGen.GetFirstBaselineIndex();
    if (baselineIndex<0)
      itkExceptionMacro("No baseline index found!");

    if (!m_Parameters.m_SignalGen.m_SimulateKspaceAcquisition)  // No upsampling of input image needed if no k-space simulation is performed
      m_Parameters.m_SignalGen.m_DoAddGibbsRinging = false;

    if (m_UseConstantRandSeed)  // always generate the same random numbers?
      m_RandGen->SetSeed(0);
    else
      m_RandGen->SetSeed();

    InitializeData();
    if ( m_FiberBundle.IsNotNull() )    // if no fiber bundle is found, we directly proceed to the k-space acquisition simulation
    {
      CheckVolumeFractionImages();
      InitializeFiberData();

      int numFiberCompartments = m_Parameters.m_FiberModelList.size();
      int numNonFiberCompartments = m_Parameters.m_NonFiberModelList.size();

      double maxVolume = 0;
      unsigned long lastTick = 0;
      int signalModelSeed = m_RandGen->GetIntegerVariate();

      PrintToLog("\n", false, false);
      PrintToLog("Generating " + boost::lexical_cast<std::string>(numFiberCompartments+numNonFiberCompartments)
                 + "-compartment diffusion-weighted signal.");

      int numFibers = m_FiberBundleWorkingCopy->GetNumFibers();
      boost::progress_display disp(numFibers*m_Parameters.m_SignalGen.GetNumVolumes());

      PrintToLog("0%   10   20   30   40   50   60   70   80   90   100%", false, true, false);
      PrintToLog("|----|----|----|----|----|----|----|----|----|----|\n*", false, false, false);

      for (unsigned int g=0; g<m_Parameters.m_SignalGen.GetNumVolumes(); g++)
      {
        // move fibers
        SimulateMotion(g);

        // Set signal model random generator seeds to get same configuration in each voxel
        for (int i=0; i<m_Parameters.m_FiberModelList.size(); i++)
          m_Parameters.m_FiberModelList.at(i)->SetSeed(signalModelSeed);
        for (int i=0; i<m_Parameters.m_NonFiberModelList.size(); i++)
          m_Parameters.m_NonFiberModelList.at(i)->SetSeed(signalModelSeed);

        // storing voxel-wise intra-axonal volume in mm³
        auto intraAxonalVolumeImage = ItkDoubleImgType::New();
        intraAxonalVolumeImage->SetSpacing( m_WorkingSpacing );
        intraAxonalVolumeImage->SetOrigin( m_WorkingOrigin );
        intraAxonalVolumeImage->SetDirection( m_Parameters.m_SignalGen.m_ImageDirection );
        intraAxonalVolumeImage->SetLargestPossibleRegion( m_WorkingImageRegion );
        intraAxonalVolumeImage->SetBufferedRegion( m_WorkingImageRegion );
        intraAxonalVolumeImage->SetRequestedRegion( m_WorkingImageRegion );
        intraAxonalVolumeImage->Allocate();
        intraAxonalVolumeImage->FillBuffer(0);
        maxVolume = 0;

        vtkPolyData* fiberPolyData = m_FiberBundleTransformed->GetFiberPolyData();
        // generate fiber signal (if there are any fiber models present)
        if (!m_Parameters.m_FiberModelList.empty())
          for( int i=0; i<numFibers; i++ )
          {
            float fiberWeight = m_FiberBundleTransformed->GetFiberWeight(i);
            vtkCell* cell = fiberPolyData->GetCell(i);
            int numPoints = cell->GetNumberOfPoints();
            vtkPoints* points = cell->GetPoints();

            if (numPoints<2)
              continue;

            for( int j=0; j<numPoints; j++)
            {
              if (this->GetAbortGenerateData())
              {
                PrintToLog("\n", false, false);
                PrintToLog("Simulation aborted");
                return;
              }

              double* temp = points->GetPoint(j);
              itk::Point<float, 3> vertex = GetItkPoint(temp);
              itk::Vector<double> v = GetItkVector(temp);

              itk::Vector<double, 3> dir(3);
              if (j<numPoints-1)
                dir = GetItkVector(points->GetPoint(j+1))-v;
              else
                dir = v-GetItkVector(points->GetPoint(j-1));

              if (dir.GetSquaredNorm()<0.0001 || dir[0]!=dir[0] || dir[1]!=dir[1] || dir[2]!=dir[2])
                continue;

              itk::Index<3> idx;
              itk::ContinuousIndex<float, 3> contIndex;
              m_TransformedMaskImage->TransformPhysicalPointToIndex(vertex, idx);
              m_TransformedMaskImage->TransformPhysicalPointToContinuousIndex(vertex, contIndex);

              if (!m_TransformedMaskImage->GetLargestPossibleRegion().IsInside(idx) || m_TransformedMaskImage->GetPixel(idx)<=0)
                continue;

              // generate signal for each fiber compartment
              for (int k=0; k<numFiberCompartments; k++)
              {
                m_Parameters.m_FiberModelList[k]->SetFiberDirection(dir);
                DoubleDwiType::PixelType pix = m_CompartmentImages.at(k)->GetPixel(idx);
                pix[g] += fiberWeight*m_SegmentVolume*m_Parameters.m_FiberModelList[k]->SimulateMeasurement(g);

                m_CompartmentImages.at(k)->SetPixel(idx, pix);
              }

              // update fiber volume image
              double vol = intraAxonalVolumeImage->GetPixel(idx) + m_SegmentVolume*fiberWeight;
              intraAxonalVolumeImage->SetPixel(idx, vol);
              if (vol>maxVolume)  // we assume that the first volume is always unweighted!
                maxVolume = vol;
            }

            // progress report
            ++disp;
            unsigned long newTick = 50*disp.count()/disp.expected_count();
            for (unsigned int tick = 0; tick<(newTick-lastTick); tick++)
              PrintToLog("*", false, false, false);
            lastTick = newTick;
          }

        // generate non-fiber signal
        ImageRegionIterator<ItkUcharImgType> it3(m_TransformedMaskImage, m_TransformedMaskImage->GetLargestPossibleRegion());
        double fact = 1;    // density correction factor in mm³
        if (m_Parameters.m_SignalGen.m_AxonRadius<0.0001 || maxVolume>m_VoxelVolume)    // the fullest voxel is always completely full
          fact = m_VoxelVolume/maxVolume;
        while(!it3.IsAtEnd())
        {
          if (it3.Get()>0)
          {
            DoubleDwiType::IndexType index = it3.GetIndex();
            itk::Point<double, 3> point;
            m_TransformedMaskImage->TransformIndexToPhysicalPoint(index, point);
            if (m_Parameters.m_SignalGen.m_DoAddMotion && g>=0 && m_Parameters.m_SignalGen.m_MotionVolumes[g])
            {
              if (m_Parameters.m_SignalGen.m_DoRandomizeMotion)
                point = m_FiberBundleWorkingCopy->TransformPoint(point.GetVnlVector(), -m_Rotation[0],-m_Rotation[1],-m_Rotation[2],-m_Translation[0],-m_Translation[1],-m_Translation[2]);
              else
                point = m_FiberBundleWorkingCopy->TransformPoint(point.GetVnlVector(), -m_Rotation[0]*m_MotionCounter,-m_Rotation[1]*m_MotionCounter,-m_Rotation[2]*m_MotionCounter,-m_Translation[0]*m_MotionCounter,-m_Translation[1]*m_MotionCounter,-m_Translation[2]*m_MotionCounter);
            }

            double iAxVolume = intraAxonalVolumeImage->GetPixel(index);

            // if volume fraction image is set use it, otherwise use scaling factor to obtain one full fiber voxel
            double fact2 = fact;
            if (m_Parameters.m_FiberModelList[0]->GetVolumeFractionImage()!=nullptr && iAxVolume>0.0001)
            {
              double val = InterpolateValue(point, m_Parameters.m_FiberModelList[0]->GetVolumeFractionImage());
              if (val>=0)
                fact2 = m_VoxelVolume*val/iAxVolume;
            }

            // adjust intra-axonal image value
            for (int i=0; i<numFiberCompartments; i++)
            {
              DoubleDwiType::PixelType pix = m_CompartmentImages.at(i)->GetPixel(index);
              pix[g] *= fact2;
              m_CompartmentImages.at(i)->SetPixel(index, pix);
            }

            // simulate other compartments
            SimulateExtraAxonalSignal(index, iAxVolume*fact2, g);
          }
          ++it3;
        }
      }

      PrintToLog("\n", false);
      if (this->GetAbortGenerateData())
      {
        PrintToLog("\n", false, false);
        PrintToLog("Simulation aborted");
        return;
      }
    }

    DoubleDwiType::Pointer doubleOutImage;
    double signalScale = m_Parameters.m_SignalGen.m_SignalScale;
    if ( m_Parameters.m_SignalGen.m_SimulateKspaceAcquisition ) // do k-space stuff
    {
      PrintToLog("\n", false, false);
      PrintToLog("Simulating k-space acquisition using "
                 +boost::lexical_cast<std::string>(m_Parameters.m_SignalGen.m_NumberOfCoils)
                 +" coil(s)");

      switch (m_Parameters.m_SignalGen.m_AcquisitionType)
      {
        case SignalGenerationParameters::SingleShotEpi:
          PrintToLog("Acquisition type: single shot EPI", false);
        break;
        case SignalGenerationParameters::SpinEcho:
          PrintToLog("Acquisition type: classic spin echo with cartesian k-space trajectory", false);
        break;
        default:
          PrintToLog("Acquisition type: single shot EPI", false);
      }

      if (m_Parameters.m_SignalGen.m_NoiseVariance>0)
        PrintToLog("Simulating complex Gaussian noise", false);
      if (m_Parameters.m_SignalGen.m_DoSimulateRelaxation)
        PrintToLog("Simulating signal relaxation", false);
      if (m_Parameters.m_SignalGen.m_FrequencyMap.IsNotNull())
        PrintToLog("Simulating distortions", false);
      if (m_Parameters.m_SignalGen.m_DoAddGibbsRinging)
        PrintToLog("Simulating ringing artifacts", false);
      if (m_Parameters.m_SignalGen.m_EddyStrength>0)
        PrintToLog("Simulating eddy currents", false);
      if (m_Parameters.m_SignalGen.m_Spikes>0)
        PrintToLog("Simulating spikes", false);
      if (m_Parameters.m_SignalGen.m_CroppingFactor<1.0)
        PrintToLog("Simulating aliasing artifacts", false);
      if (m_Parameters.m_SignalGen.m_KspaceLineOffset>0)
        PrintToLog("Simulating ghosts", false);

      doubleOutImage = SimulateKspaceAcquisition(m_CompartmentImages);
      signalScale = 1; // already scaled in SimulateKspaceAcquisition()
    }
    else    // don't do k-space stuff, just sum compartments
    {
      PrintToLog("Summing compartments");
      doubleOutImage = m_CompartmentImages.at(0);

      for (unsigned int i=1; i<m_CompartmentImages.size(); i++)
      {
        auto adder = itk::AddImageFilter< DoubleDwiType, DoubleDwiType, DoubleDwiType>::New();
        adder->SetInput1(doubleOutImage);
        adder->SetInput2(m_CompartmentImages.at(i));
        adder->Update();
        doubleOutImage = adder->GetOutput();
      }
    }
    if (this->GetAbortGenerateData())
    {
      PrintToLog("\n", false, false);
      PrintToLog("Simulation aborted");
      return;
    }

    PrintToLog("Finalizing image");
    if (signalScale>1)
      PrintToLog(" Scaling signal", false);
    if (m_Parameters.m_NoiseModel)
      PrintToLog(" Adding noise", false);
    unsigned int window = 0;
    unsigned int min = itk::NumericTraits<unsigned int>::max();
    ImageRegionIterator<OutputImageType> it4 (m_OutputImage, m_OutputImage->GetLargestPossibleRegion());
    DoubleDwiType::PixelType signal; signal.SetSize(m_Parameters.m_SignalGen.GetNumVolumes());
    boost::progress_display disp2(m_OutputImage->GetLargestPossibleRegion().GetNumberOfPixels());

    PrintToLog("0%   10   20   30   40   50   60   70   80   90   100%", false, true, false);
    PrintToLog("|----|----|----|----|----|----|----|----|----|----|\n*", false, false, false);
    int lastTick = 0;

    while(!it4.IsAtEnd())
    {
      if (this->GetAbortGenerateData())
      {
        PrintToLog("\n", false, false);
        PrintToLog("Simulation aborted");
        return;
      }

      ++disp2;
      unsigned long newTick = 50*disp2.count()/disp2.expected_count();
      for (unsigned long tick = 0; tick<(newTick-lastTick); tick++)
        PrintToLog("*", false, false, false);
      lastTick = newTick;

      typename OutputImageType::IndexType index = it4.GetIndex();
      signal = doubleOutImage->GetPixel(index)*signalScale;

      if (m_Parameters.m_NoiseModel)
        m_Parameters.m_NoiseModel->AddNoise(signal);

      for (unsigned int i=0; i<signal.Size(); i++)
      {
        if (signal[i]>0)
          signal[i] = floor(signal[i]+0.5);
        else
          signal[i] = ceil(signal[i]-0.5);

        if ( (!m_Parameters.m_SignalGen.IsBaselineIndex(i) || signal.Size()==1) && signal[i]>window)
          window = signal[i];
        if ( (!m_Parameters.m_SignalGen.IsBaselineIndex(i) || signal.Size()==1) && signal[i]<min)
          min = signal[i];
      }
      it4.Set(signal);
      ++it4;
    }
    window -= min;
    unsigned int level = window/2 + min;
    m_LevelWindow.SetLevelWindow(level, window);
    this->SetNthOutput(0, m_OutputImage);

    PrintToLog("\n", false);
    PrintToLog("Finished simulation");
    m_TimeProbe.Stop();

    if (m_Parameters.m_SignalGen.m_DoAddMotion)
    {
      PrintToLog("\nHead motion log:", false);
      PrintToLog(m_MotionLog, false, false);
    }

    if (m_Parameters.m_SignalGen.m_Spikes>0)
    {
      PrintToLog("\nSpike log:", false);
      PrintToLog(m_SpikeLog, false, false);
    }

    if (m_Logfile.is_open())
      m_Logfile.close();
  } // Heilliger Spaghetti-Code, Batman! todo/mdh

  template< class PixelType >
  void TractsToDWIImageFilter< PixelType >::PrintToLog(string m, bool addTime, bool linebreak, bool stdOut)
  {
    // timestamp
    if (addTime)
    {
      m_Logfile << this->GetTime() << " > ";
      m_StatusText += this->GetTime() + " > ";
      if (stdOut)
        std::cout << this->GetTime() << " > ";
    }

    // message
    if (m_Logfile.is_open())
      m_Logfile << m;
    m_StatusText += m;
    if (stdOut)
      std::cout << m;

    // new line
    if (linebreak)
    {
      if (m_Logfile.is_open())
        m_Logfile << "\n";
      m_StatusText += "\n";
      if (stdOut)
        std::cout << "\n";
    }
  }

  template< class PixelType >
  void TractsToDWIImageFilter< PixelType >::SimulateMotion(int g)
  {
    // is motion artifact enabled?
    // is the current volume g affected by motion?
    if (m_Parameters.m_SignalGen.m_DoAddMotion && m_Parameters.m_SignalGen.m_MotionVolumes[g] && g<m_Parameters.m_SignalGen.GetNumVolumes())
    {
      if (m_Parameters.m_SignalGen.m_DoRandomizeMotion)
      {
        m_FiberBundleTransformed = m_FiberBundleWorkingCopy->GetDeepCopy(); // either undo last transform or work on fresh copy of untransformed fibers
        m_Rotation[0] = m_RandGen->GetVariateWithClosedRange(m_Parameters.m_SignalGen.m_Rotation[0]*2)-m_Parameters.m_SignalGen.m_Rotation[0];
        m_Rotation[1] = m_RandGen->GetVariateWithClosedRange(m_Parameters.m_SignalGen.m_Rotation[1]*2)-m_Parameters.m_SignalGen.m_Rotation[1];
        m_Rotation[2] = m_RandGen->GetVariateWithClosedRange(m_Parameters.m_SignalGen.m_Rotation[2]*2)-m_Parameters.m_SignalGen.m_Rotation[2];
        m_Translation[0] = m_RandGen->GetVariateWithClosedRange(m_Parameters.m_SignalGen.m_Translation[0]*2)-m_Parameters.m_SignalGen.m_Translation[0];
        m_Translation[1] = m_RandGen->GetVariateWithClosedRange(m_Parameters.m_SignalGen.m_Translation[1]*2)-m_Parameters.m_SignalGen.m_Translation[1];
        m_Translation[2] = m_RandGen->GetVariateWithClosedRange(m_Parameters.m_SignalGen.m_Translation[2]*2)-m_Parameters.m_SignalGen.m_Translation[2];
      }
      else
      {
        m_Rotation = m_Parameters.m_SignalGen.m_Rotation/m_NumMotionVolumes;
        m_Translation = m_Parameters.m_SignalGen.m_Translation/m_NumMotionVolumes;
        m_MotionCounter++;
      }

      // move mask image
      if (m_MaskImageSet)
      {
        ImageRegionIterator<ItkUcharImgType> maskIt(m_UpsampledMaskImage, m_UpsampledMaskImage->GetLargestPossibleRegion());
        m_TransformedMaskImage->FillBuffer(0);

        while(!maskIt.IsAtEnd())
        {
          if (maskIt.Get()<=0)
          {
            ++maskIt;
            continue;
          }

          DoubleDwiType::IndexType index = maskIt.GetIndex();
          itk::Point<double, 3> point;
          m_UpsampledMaskImage->TransformIndexToPhysicalPoint(index, point);
          if (m_Parameters.m_SignalGen.m_DoRandomizeMotion)
            point = m_FiberBundleWorkingCopy->TransformPoint(point.GetVnlVector(), m_Rotation[0],m_Rotation[1],m_Rotation[2],m_Translation[0],m_Translation[1],m_Translation[2]);
          else
            point = m_FiberBundleWorkingCopy->TransformPoint(point.GetVnlVector(), m_Rotation[0]*m_MotionCounter,m_Rotation[1]*m_MotionCounter,m_Rotation[2]*m_MotionCounter,m_Translation[0]*m_MotionCounter,m_Translation[1]*m_MotionCounter,m_Translation[2]*m_MotionCounter);

          m_TransformedMaskImage->TransformPhysicalPointToIndex(point, index);
          if (m_TransformedMaskImage->GetLargestPossibleRegion().IsInside(index))
            m_TransformedMaskImage->SetPixel(index,100);
          ++maskIt;
        }
      }

      if (m_Parameters.m_SignalGen.m_DoRandomizeMotion)
      {
        m_Rotations.push_back(m_Rotation);
        m_Translations.push_back(m_Translation);
        m_MotionLog += boost::lexical_cast<std::string>(g) + " rotation: " + boost::lexical_cast<std::string>(m_Rotation[0]) + "," + boost::lexical_cast<std::string>(m_Rotation[1]) + "," + boost::lexical_cast<std::string>(m_Rotation[2]) + ";";
        m_MotionLog += " translation: " + boost::lexical_cast<std::string>(m_Translation[0]) + "," + boost::lexical_cast<std::string>(m_Translation[1]) + "," + boost::lexical_cast<std::string>(m_Translation[2]) + "\n";
      }
      else
      {
        m_Rotations.push_back(m_Rotation*m_MotionCounter);
        m_Translations.push_back(m_Translation*m_MotionCounter);

        m_MotionLog += boost::lexical_cast<std::string>(g) + " rotation: " + boost::lexical_cast<std::string>(m_Rotation[0]*m_MotionCounter) + "," + boost::lexical_cast<std::string>(m_Rotation[1]*m_MotionCounter) + "," + boost::lexical_cast<std::string>(m_Rotation[2]*m_MotionCounter) + ";";
        m_MotionLog += " translation: " + boost::lexical_cast<std::string>(m_Translation[0]*m_MotionCounter) + "," + boost::lexical_cast<std::string>(m_Translation[1]*m_MotionCounter) + "," + boost::lexical_cast<std::string>(m_Translation[2]*m_MotionCounter) + "\n";
      }

      m_FiberBundleTransformed->TransformFibers(m_Rotation[0],m_Rotation[1],m_Rotation[2],m_Translation[0],m_Translation[1],m_Translation[2]);
    }
    else
    {
      m_Rotation.Fill(0.0);
      m_Translation.Fill(0.0);
      m_Rotations.push_back(m_Rotation);
      m_Translations.push_back(m_Translation);
      m_MotionLog += boost::lexical_cast<std::string>(g) + " rotation: " + boost::lexical_cast<std::string>(m_Rotation[0]) + "," + boost::lexical_cast<std::string>(m_Rotation[1]) + "," + boost::lexical_cast<std::string>(m_Rotation[2]) + ";";
      m_MotionLog += " translation: " + boost::lexical_cast<std::string>(m_Translation[0]) + "," + boost::lexical_cast<std::string>(m_Translation[1]) + "," + boost::lexical_cast<std::string>(m_Translation[2]) + "\n";
    }
  }

  template< class PixelType >
  void TractsToDWIImageFilter< PixelType >::SimulateExtraAxonalSignal(ItkUcharImgType::IndexType index, double intraAxonalVolume, int g)
  {
    int numFiberCompartments = m_Parameters.m_FiberModelList.size();
    int numNonFiberCompartments = m_Parameters.m_NonFiberModelList.size();

    if (intraAxonalVolume>0.0001 && m_Parameters.m_SignalGen.m_DoDisablePartialVolume)  // only fiber in voxel
    {
      DoubleDwiType::PixelType pix = m_CompartmentImages.at(0)->GetPixel(index);
      if (g>=0)
        pix[g] *= m_VoxelVolume/intraAxonalVolume;
      else
        pix *= m_VoxelVolume/intraAxonalVolume;
      m_CompartmentImages.at(0)->SetPixel(index, pix);
      if (g==0)
        m_VolumeFractions.at(0)->SetPixel(index, 1);
      for (int i=1; i<numFiberCompartments; i++)
      {
        DoubleDwiType::PixelType pix = m_CompartmentImages.at(i)->GetPixel(index);
        if (g>=0)
          pix[g] = 0.0;
        else
          pix.Fill(0.0);
        m_CompartmentImages.at(i)->SetPixel(index, pix);
      }
    }
    else
    {
      if (g==0)
        m_VolumeFractions.at(0)->SetPixel(index, intraAxonalVolume/m_VoxelVolume);

      // get non-transformed point (remove headmotion tranformation)
      // this point can then be transformed to each of the original images, regardless of their geometry
      itk::Point<double, 3> point;
      m_TransformedMaskImage->TransformIndexToPhysicalPoint(index, point);
      if (m_Parameters.m_SignalGen.m_DoAddMotion && g>=0 && m_Parameters.m_SignalGen.m_MotionVolumes[g])
      {
        if (m_Parameters.m_SignalGen.m_DoRandomizeMotion)
          point = m_FiberBundleWorkingCopy->TransformPoint(point.GetVnlVector(), -m_Rotation[0],-m_Rotation[1],-m_Rotation[2],-m_Translation[0],-m_Translation[1],-m_Translation[2]);
        else
          point = m_FiberBundleWorkingCopy->TransformPoint(point.GetVnlVector(), -m_Rotation[0]*m_MotionCounter,-m_Rotation[1]*m_MotionCounter,-m_Rotation[2]*m_MotionCounter,-m_Translation[0]*m_MotionCounter,-m_Translation[1]*m_MotionCounter,-m_Translation[2]*m_MotionCounter);
      }

      if (m_Parameters.m_SignalGen.m_DoDisablePartialVolume)
      {
        int maxVolumeIndex = 0;
        double maxWeight = 0;
        for (int i=0; i<numNonFiberCompartments; i++)
        {
          double weight = 0;
          if (numNonFiberCompartments>1)
          {
            double val = InterpolateValue(point, m_Parameters.m_NonFiberModelList[i]->GetVolumeFractionImage());
            if (val<0)
              continue;
            else
              weight = val;
          }

          if (weight>maxWeight)
          {
            maxWeight = weight;
            maxVolumeIndex = i;
          }
        }
        DoubleDwiType::Pointer doubleDwi = m_CompartmentImages.at(maxVolumeIndex+numFiberCompartments);
        DoubleDwiType::PixelType pix = doubleDwi->GetPixel(index);

        if (g>=0)
          pix[g] += m_Parameters.m_NonFiberModelList[maxVolumeIndex]->SimulateMeasurement(g)*m_VoxelVolume;
        else
          pix += m_Parameters.m_NonFiberModelList[maxVolumeIndex]->SimulateMeasurement()*m_VoxelVolume;
        doubleDwi->SetPixel(index, pix);
        if (g==0)
          m_VolumeFractions.at(maxVolumeIndex+numFiberCompartments)->SetPixel(index, 1);
      }
      else
      {
        double extraAxonalVolume = m_VoxelVolume-intraAxonalVolume;    // non-fiber volume
        if (extraAxonalVolume<0)
        {
          MITK_ERROR << "Corrupted intra-axonal signal voxel detected. Fiber volume larger voxel volume!";
          extraAxonalVolume = 0;
        }
        double interAxonalVolume = 0;
        if (numFiberCompartments>1)
          interAxonalVolume = extraAxonalVolume * intraAxonalVolume/m_VoxelVolume;   // inter-axonal fraction of non fiber compartment
        double other = extraAxonalVolume - interAxonalVolume;        // rest of compartment
        if (other<0)
        {
          MITK_ERROR << "Corrupted signal voxel detected. Fiber volume larger voxel volume!";
          other = 0;
        }

        // adjust non-fiber and intra-axonal signal
        for (int i=1; i<numFiberCompartments; i++)
        {
          double weight = interAxonalVolume;
          DoubleDwiType::PixelType pix = m_CompartmentImages.at(i)->GetPixel(index);
          if (intraAxonalVolume>0)    // remove scaling by intra-axonal volume from inter-axonal compartment
          {
            if (g>=0)
              pix[g] /= intraAxonalVolume;
            else
              pix /= intraAxonalVolume;
          }

          if (m_Parameters.m_FiberModelList[i]->GetVolumeFractionImage()!=nullptr)
          {
            double val = InterpolateValue(point, m_Parameters.m_FiberModelList[i]->GetVolumeFractionImage());
            if (val<0)
              continue;
            else
              weight = val*m_VoxelVolume;
          }

          if (g>=0)
            pix[g] *= weight;
          else
            pix *= weight;
          m_CompartmentImages.at(i)->SetPixel(index, pix);
          if (g==0)
            m_VolumeFractions.at(i)->SetPixel(index, weight/m_VoxelVolume);
        }

        for (int i=0; i<numNonFiberCompartments; i++)
        {
          double weight = other;
          DoubleDwiType::PixelType pix = m_CompartmentImages.at(i+numFiberCompartments)->GetPixel(index);
          if (m_Parameters.m_NonFiberModelList[i]->GetVolumeFractionImage()!=nullptr)
          {
            double val = InterpolateValue(point, m_Parameters.m_NonFiberModelList[i]->GetVolumeFractionImage());
            if (val<0)
              continue;
            else
              weight = val*m_VoxelVolume;

            if (m_UseRelativeNonFiberVolumeFractions)
              weight *= other/m_VoxelVolume;
          }

          if (g>=0)
            pix[g] += m_Parameters.m_NonFiberModelList[i]->SimulateMeasurement(g)*weight;
          else
            pix += m_Parameters.m_NonFiberModelList[i]->SimulateMeasurement()*weight;
          m_CompartmentImages.at(i+numFiberCompartments)->SetPixel(index, pix);
          if (g==0)
            m_VolumeFractions.at(i+numFiberCompartments)->SetPixel(index, weight/m_VoxelVolume);
        }
      }
    }
  }

  template< class PixelType >
  double TractsToDWIImageFilter< PixelType >::InterpolateValue(itk::Point<float, 3> itkP, ItkDoubleImgType::Pointer img)
  {
    itk::Index<3> idx;
    itk::ContinuousIndex< double, 3> cIdx;
    img->TransformPhysicalPointToIndex(itkP, idx);
    img->TransformPhysicalPointToContinuousIndex(itkP, cIdx);

    double pix = -1;
    if ( img->GetLargestPossibleRegion().IsInside(idx) )
      pix = img->GetPixel(idx);
    else
      return pix;

    double frac_x = cIdx[0] - idx[0];
    double frac_y = cIdx[1] - idx[1];
    double frac_z = cIdx[2] - idx[2];
    if (frac_x<0)
    {
      idx[0] -= 1;
      frac_x += 1;
    }
    if (frac_y<0)
    {
      idx[1] -= 1;
      frac_y += 1;
    }
    if (frac_z<0)
    {
      idx[2] -= 1;
      frac_z += 1;
    }
    frac_x = 1-frac_x;
    frac_y = 1-frac_y;
    frac_z = 1-frac_z;

    // int coordinates inside image?
    if (idx[0] >= 0 && idx[0] < img->GetLargestPossibleRegion().GetSize(0)-1 &&
        idx[1] >= 0 && idx[1] < img->GetLargestPossibleRegion().GetSize(1)-1 &&
        idx[2] >= 0 && idx[2] < img->GetLargestPossibleRegion().GetSize(2)-1)
    {
      vnl_vector_fixed<double, 8> interpWeights;
      interpWeights[0] = (  frac_x)*(  frac_y)*(  frac_z);
      interpWeights[1] = (1-frac_x)*(  frac_y)*(  frac_z);
      interpWeights[2] = (  frac_x)*(1-frac_y)*(  frac_z);
      interpWeights[3] = (  frac_x)*(  frac_y)*(1-frac_z);
      interpWeights[4] = (1-frac_x)*(1-frac_y)*(  frac_z);
      interpWeights[5] = (  frac_x)*(1-frac_y)*(1-frac_z);
      interpWeights[6] = (1-frac_x)*(  frac_y)*(1-frac_z);
      interpWeights[7] = (1-frac_x)*(1-frac_y)*(1-frac_z);

      pix = img->GetPixel(idx) * interpWeights[0];
      ItkDoubleImgType::IndexType tmpIdx = idx; tmpIdx[0]++;
      pix +=  img->GetPixel(tmpIdx) * interpWeights[1];
      tmpIdx = idx; tmpIdx[1]++;
      pix +=  img->GetPixel(tmpIdx) * interpWeights[2];
      tmpIdx = idx; tmpIdx[2]++;
      pix +=  img->GetPixel(tmpIdx) * interpWeights[3];
      tmpIdx = idx; tmpIdx[0]++; tmpIdx[1]++;
      pix +=  img->GetPixel(tmpIdx) * interpWeights[4];
      tmpIdx = idx; tmpIdx[1]++; tmpIdx[2]++;
      pix +=  img->GetPixel(tmpIdx) * interpWeights[5];
      tmpIdx = idx; tmpIdx[2]++; tmpIdx[0]++;
      pix +=  img->GetPixel(tmpIdx) * interpWeights[6];
      tmpIdx = idx; tmpIdx[0]++; tmpIdx[1]++; tmpIdx[2]++;
      pix +=  img->GetPixel(tmpIdx) * interpWeights[7];
    }

    return pix;
  }

  template< class PixelType >
  itk::Point<float, 3> TractsToDWIImageFilter< PixelType >::GetItkPoint(double point[3])
  {
    itk::Point<float, 3> itkPoint;
    itkPoint[0] = point[0];
    itkPoint[1] = point[1];
    itkPoint[2] = point[2];
    return itkPoint;
  }

  template< class PixelType >
  itk::Vector<double, 3> TractsToDWIImageFilter< PixelType >::GetItkVector(double point[3])
  {
    itk::Vector<double, 3> itkVector;
    itkVector[0] = point[0];
    itkVector[1] = point[1];
    itkVector[2] = point[2];
    return itkVector;
  }

  template< class PixelType >
  vnl_vector_fixed<double, 3> TractsToDWIImageFilter< PixelType >::GetVnlVector(double point[3])
  {
    vnl_vector_fixed<double, 3> vnlVector;
    vnlVector[0] = point[0];
    vnlVector[1] = point[1];
    vnlVector[2] = point[2];
    return vnlVector;
  }

  template< class PixelType >
  vnl_vector_fixed<double, 3> TractsToDWIImageFilter< PixelType >::GetVnlVector(Vector<float,3>& vector)
  {
    vnl_vector_fixed<double, 3> vnlVector;
    vnlVector[0] = vector[0];
    vnlVector[1] = vector[1];
    vnlVector[2] = vector[2];
    return vnlVector;
  }

  template< class PixelType >
  double TractsToDWIImageFilter< PixelType >::RoundToNearest(double num) {
    return (num > 0.0) ? floor(num + 0.5) : ceil(num - 0.5);
  }

  template< class PixelType >
  std::string TractsToDWIImageFilter< PixelType >::GetTime()
  {
    m_TimeProbe.Stop();
    unsigned long total = RoundToNearest(m_TimeProbe.GetTotal());
    unsigned long hours = total/3600;
    unsigned long minutes = (total%3600)/60;
    unsigned long seconds = total%60;
    std::string out = "";
    out.append(boost::lexical_cast<std::string>(hours));
    out.append(":");
    out.append(boost::lexical_cast<std::string>(minutes));
    out.append(":");
    out.append(boost::lexical_cast<std::string>(seconds));
    m_TimeProbe.Start();
    return out;
  }

}


/**
 * @example ublas/test/matrix_test.cc
 */
#include <boost/test/unit_test.hpp>
#include <usml/ublas/matrix_math.h>
#include <usml/ublas/vector_math.h>
#include <usml/ublas/test/matrix_test_support.h>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

using namespace usml::ublas;
using namespace matrix_test ;

BOOST_AUTO_TEST_SUITE(matrix_test)

/**
 * @ingroup ublas_test
 * @{
 */

/**
 * Run a real valued matrix through the standard math routines.  Tests include:
 *
 *  - addition of matrix to itself
 *  - add scalar to matrix
 *  - subtract matrix from scalar
 *  - divide scalar by matrix
 *  - limiting functions: min, max, floor, and ceil
 *  - conversion routines for degrees/radians and latitude/colatitude
 *  - algebraic functions: abs(), abs2(), arg(), sqrt()
 *  - trig functions: cos(), cosh(), sin(), sinh(), tan(), tanh()
 *  - exponential functions: exp(), log(), log10()
 *
 * Warning: Some compilers, at low optimization levels, fail to properly
 * chain multiple ublas operations into a single evaluation.
 * When this happens, the result is equal to the first operation.
 * These tests have been designed to catch this flaw.
 */
BOOST_AUTO_TEST_CASE( real_matrix_test ) {

    matrix<double> rmat(2,7);
    int k = 0;
    for ( size_t n=0; n < rmat.size1(); ++n ) {
        for ( size_t m=0; m < rmat.size2(); ++m ) {
            rmat(n,m) = (double) ((k++)+1);
        }
    }

    matrix<double> result ;
    cout << "=== matrix_test: real_matrix_test ===" << endl;
    cout << "a:         " << rmat << endl;
    cout << "a+a:       " << rmat+rmat << endl;
    USML_MATRIX_REAL_TESTER( rmat+rmat, rmat, add_helper<double> ) ;
    cout << "a+2.1:     " << rmat+2.1 << endl;
    USML_MATRIX_REAL_TESTER( rmat+2.1, rmat, scalar2_add_helper<double> ) ;
    cout << "2.1-a:     " << 2.1-rmat << endl;
    USML_MATRIX_REAL_TESTER( 2.1-rmat, rmat, scalar1_minus_helper<double> ) ;
    cout << "2.1/a:     " << 2.1/rmat << endl;
    USML_MATRIX_REAL_TESTER( 2.1/rmat, rmat, scalar1_divide_helper<double> ) ;
    cout << "a*a:       " << rmat * rmat << endl;
    USML_MATRIX_REAL_TESTER( rmat*rmat, rmat, multiply_helper<double> ) ;
    cout << "a/a:       " << rmat / rmat << endl;
    USML_MATRIX_REAL_TESTER( rmat/rmat, rmat, divide_helper<double> ) ;
    cout << "max(a,3.0):   " << max(rmat,3.0) << endl;
    USML_MATRIX_REAL_TESTER( max(rmat,3.0), rmat, max_helper<double> ) ;
    cout << "min(a,3.0):   " << min(rmat,3.0) << endl;
    USML_MATRIX_REAL_TESTER( min(rmat,3.0), rmat, min_helper<double> ) ;
    cout << "floor(a+2.1): " << floor(rmat+2.1) << endl;
    USML_MATRIX_REAL_TESTER( floor(rmat+2.1), rmat, floor_helper<double> ) ;
    cout << "ceil(a+2.1):  " << ceil(rmat+2.1) << endl;
    USML_MATRIX_REAL_TESTER( ceil(rmat+2.1), rmat, ceil_helper<double> ) ;
    cout << "to_degrees(to_radians(a+2.1)):     " << to_degrees(to_radians(rmat+2.1)) << endl;
    USML_MATRIX_REAL_TESTER( to_degrees(to_radians(rmat+2.1)), rmat, scalar2_add_helper<double>) ;
    cout << "to_latitude(to_colatitude(a+2.1)): " << to_latitude(to_colatitude(rmat+2.1)) << endl;
    USML_MATRIX_REAL_TESTER( to_latitude(to_colatitude(rmat+2.1)), rmat, scalar2_add_helper<double>) ;

    cout << "abs(a):    " << abs(rmat) << endl;
    USML_MATRIX_REAL_TESTER( abs(rmat), rmat, std::abs<double> ) ;
    cout << "abs2(a):   " << abs2(rmat) << endl;
    USML_MATRIX_REAL_TESTER( abs2(rmat), rmat, abs2_helper<double> ) ;
    cout << "arg(a):    " << arg(rmat) << endl;
    USML_MATRIX_REAL_TESTER( arg(rmat), rmat, zero_helper<double> ) ;
    cout << "sqrt(a):   " << sqrt(rmat) << endl;
    USML_MATRIX_REAL_TESTER( sqrt(rmat), rmat, std::sqrt<double> ) ;

    cout << "cos(a):    " << cos(rmat) << endl;
    USML_MATRIX_REAL_TESTER( cos(rmat), rmat, std::cos<double> ) ;
    cout << "cosh(a):   " << cosh(rmat) << endl;
    USML_MATRIX_REAL_TESTER( cosh(rmat), rmat, std::cosh<double> ) ;
    cout << "sin(a):    " << sin(rmat) << endl;
    USML_MATRIX_REAL_TESTER( sin(rmat), rmat, std::sin<double> ) ;
    cout << "sinh(a):   " << sinh(rmat) << endl;
    USML_MATRIX_REAL_TESTER( sinh(rmat), rmat, std::sinh<double> ) ;
    cout << "tan(a):    " << tan(rmat) << endl;
    USML_MATRIX_REAL_TESTER( tan(rmat), rmat, std::tan<double> ) ;
    cout << "tanh(a):   " << tanh(rmat) << endl;
    USML_MATRIX_REAL_TESTER( tanh(rmat), rmat, std::tanh<double> ) ;

    cout << "exp(a):    " << exp(rmat) << endl;
    USML_MATRIX_REAL_TESTER( exp(rmat), rmat, std::exp<double> ) ;
    cout << "log(a):    " << log(rmat) << endl;
    USML_MATRIX_REAL_TESTER( log(rmat), rmat, std::log<double> ) ;
    cout << "log10(a):  " << log10(rmat) << endl;
    USML_MATRIX_REAL_TESTER( log10(rmat), rmat, std::log10<double> ) ;
}

/**
 * Run a complex valued matrix through the standard math routines.  Tests include:
 *
 *  - addition of matrix to itself
 *  - add scalar to matrix
 *  - subtract matrix from scalar
 *  - divide scalar by matrix
 *  - conversion routines for degrees/radians and latitude/colatitude
 *  - algebraic functions: abs(), abs2(), arg(), sqrt()
 *  - trig functions: cos(), cosh(), sin(), sinh(), tan(), tanh()
 *  - exponential functions: exp(), log(), log10()
 */
BOOST_AUTO_TEST_CASE( complex_matrix_test ) {

    matrix< std::complex<double> > cmat(2,3);
    int k = 0;
    for ( size_t n=0; n < cmat.size1(); ++n ) {
        for ( size_t m=0; m < cmat.size2(); ++m ) {
            cmat(n,m) = std::complex<double>((k++)+1,1.0);
        }
    }

    cout << "=== matrix_test: complex_matrix_test ===" << endl;
    cout << "a:         " << cmat << endl;
    cout << "a+a:       " << cmat + cmat << endl;
    USML_MATRIX_COMPLEX_TESTER( cmat+cmat, cmat, add_helper<complex<double> > ) ;
    cout << "a+2.1:     " << cmat+2.1 << endl;
    USML_MATRIX_COMPLEX_TESTER( cmat+2.1, cmat, scalar2_add_helper<complex<double> > ) ;
    cout << "2.1-a:     " << 2.1-cmat << endl;
    USML_MATRIX_COMPLEX_TESTER( 2.1-cmat, cmat, scalar1_minus_helper<complex<double> > ) ;
    cout << "2.1/a:     " << 2.1/cmat << endl;
    USML_MATRIX_COMPLEX_TESTER( 2.1/cmat, cmat, scalar1_divide_helper<complex<double> > ) ;

    cout << "abs(a):    " << abs(cmat) << endl;
    USML_MATRIX_COMPLEX_TESTER( abs(cmat), cmat, abs_helper<complex<double> > ) ;
    cout << "abs2(a):   " << abs2(cmat) << endl;
    USML_MATRIX_COMPLEX_TESTER( abs2(cmat), cmat, abs2_helper<complex<double> > ) ;
    cout << "arg(a):    " << arg(cmat) << endl;
    USML_MATRIX_COMPLEX_TESTER( arg(cmat), cmat, arg_helper<double> ) ;
    cout << "sqrt(a):   " << sqrt(cmat) << endl;
    USML_MATRIX_COMPLEX_TESTER( sqrt(cmat), cmat, std::sqrt<double> ) ;

    cout << "cos(a):    " << cos(cmat) << endl;
    USML_MATRIX_COMPLEX_TESTER( cos(cmat), cmat, std::cos<double> ) ;
    cout << "cosh(a):   " << cosh(cmat) << endl;
    USML_MATRIX_COMPLEX_TESTER( cosh(cmat), cmat, std::cosh<double> ) ;
    cout << "sin(a):    " << sin(cmat) << endl;
    USML_MATRIX_COMPLEX_TESTER( sin(cmat), cmat, std::sin<double> ) ;
    cout << "sinh(a):   " << sinh(cmat) << endl;
    USML_MATRIX_COMPLEX_TESTER( sinh(cmat), cmat, std::sinh<double> ) ;
    cout << "tan(a):    " << tan(cmat) << endl;
    USML_MATRIX_COMPLEX_TESTER( tan(cmat), cmat, std::tan<double> ) ;
    cout << "tanh(a):   " << tanh(cmat) << endl;
    USML_MATRIX_COMPLEX_TESTER( tanh(cmat), cmat, std::tanh<double> ) ;

    cout << "exp(a):    " << exp(cmat) << endl;
    USML_MATRIX_COMPLEX_TESTER( exp(cmat), cmat, std::exp<double> ) ;
    cout << "log(a):    " << log(cmat) << endl;
    USML_MATRIX_COMPLEX_TESTER( log(cmat), cmat, std::log<double> ) ;
    cout << "log10(a):  " << log10(cmat) << endl;
    USML_MATRIX_COMPLEX_TESTER( log10(cmat), cmat, std::log10<double> ) ;
}

/**
 * Test all of the real and complex combinations of the pow() function.
 */
BOOST_AUTO_TEST_CASE( pow_matrix_test ) {

    matrix<double> rmat(2,3);
    matrix< std::complex<double> > cmat(2,3);
    std::complex<double> cmplx(2.5,3.5);

    int k = 0;
    for ( size_t n=0; n < cmat.size1(); ++n ) {
        for ( size_t m=0; m < cmat.size2(); ++m ) {
            rmat(n,m) = (double) (k+1);
            cmat(n,m) = std::complex<double>(k+1,1.0);
            ++k;
        }
    }

    cout << "=== matrix_test: pow_matrix_test ===" << endl;
    cout << "N:      " << rmat << endl;
    cout << "M:      " << cmat << endl;
    cout << "C:      " << cmplx << endl;

    cout << "N^3:    " << pow(rmat,3) << endl;
    USML_MATRIX_POW_SCALAR2_TESTER( pow(rmat,3), rmat, 3 ) ;
    cout << "N^2.5:  " << pow(rmat,2.5) << endl;
    USML_MATRIX_POW_SCALAR2_TESTER( pow(rmat,2.5), rmat, 2.5 ) ;
    cout << "2^N:    " << pow( 2.0, rmat ) << endl;
    USML_MATRIX_POW_SCALAR1_TESTER( pow(2.0,rmat), 2.0, rmat ) ;

    cout << "M^3:    " << pow(cmat,3.0) << endl;
    USML_MATRIX_POW_SCALAR2_TESTER( pow(cmat,3.0), cmat, 3.0 ) ;
    cout << "M^2.5:  " << pow(cmat,2.5) << endl;
    USML_MATRIX_POW_SCALAR2_TESTER( pow(cmat,2.5), cmat, 2.5 ) ;
    cout << "2^M:    " << pow(2.0,cmat) << endl;
    USML_MATRIX_POW_SCALAR1_TESTER( pow(2.0,cmat), 2.0, cmat ) ;

    cout << "N^N:    " << pow(rmat,rmat) << endl;
    USML_MATRIX_POW_TESTER( pow(rmat,rmat), rmat, rmat ) ;
    cout << "M^M:    " << pow(cmat,cmat) << endl;
    USML_MATRIX_POW_TESTER( pow(cmat,cmat), cmat, cmat ) ;
    cout << "N^M:    " << pow(rmat,cmat) << endl;
    USML_MATRIX_POW_TESTER( pow(rmat,cmat), rmat, cmat ) ;
    cout << "M^N:    " << pow(cmat,rmat) << endl;
    USML_MATRIX_POW_TESTER( pow(cmat,rmat), cmat, rmat ) ;
}

/**
 * Test the generation of real valued and analytic signals from
 * a matrix of arguments.
 *
 * Assume that testers will visually inspect the results.
 */
BOOST_AUTO_TEST_CASE( signal_matrix_test ) {

    matrix<double> rmat(2,3);
    int k = 0;
    for ( size_t n=0; n < rmat.size1(); ++n ) {
        for ( size_t m=0; m < rmat.size2(); ++m ) {
            rmat(n,m) = (double) ((k++)+1);
        }
    }

    cout << "=== matrix_test: signal_matrix_test ===" << endl;
    cout << "input:  " << rmat << endl;
    cout << "signal: " << signal(rmat) << endl;
    USML_MATRIX_REAL_TESTER( signal(rmat), rmat, signal_helper<double> ) ;
    cout << "signal: " << asignal(rmat) << endl;
    USML_MATRIX_CR_TESTER( asignal(rmat), rmat, asignal_helper<double> ) ;
}

/**
 * Run real valued trig routines forward and backward to check algorithms.
 */
BOOST_AUTO_TEST_CASE( realInverse_matrix_test ) {

    matrix<double> rmat(2,3);
    int k = 0;
    for ( size_t n=0; n < rmat.size1(); ++n ) {
        for ( size_t m=0; m < rmat.size2(); ++m ) {
            rmat(n,m) = 0.1 * (double) ((k++)+1);
        }
    }

    cout << "=== matrix_test: realInverse_matrix_test ===" << endl;
    cout << "acos(cos(t)):    " << acos(cos(rmat)) << endl;
    USML_MATRIX_REAL_TESTER( acos(cos(rmat)), rmat, identity<double> ) ;
    cout << "acosh(cosh(t)):  " << acosh(cosh(rmat)) << endl;
    USML_MATRIX_REAL_TESTER( acosh(cosh(rmat)), rmat, identity<double> ) ;
    cout << "asin(sin(t)):    " << asin(sin(rmat)) << endl;
    USML_MATRIX_REAL_TESTER( asin(sin(rmat)), rmat, identity<double> ) ;
    cout << "asinh(sinh(t)):  " << asinh(sinh(rmat)) << endl;
    USML_MATRIX_REAL_TESTER( asinh(sinh(rmat)), rmat, identity<double> ) ;
    cout << "atan(tan(t)):    " << atan(tan(rmat)) << endl;
    USML_MATRIX_REAL_TESTER( atan(tan(rmat)), rmat, identity<double> ) ;
    cout << "atanh(tanh(t)):  " << atanh(tanh(rmat)) << endl;
    USML_MATRIX_REAL_TESTER( atanh(tanh(rmat)), rmat, identity<double> ) ;
}

/**
 * Run complex trig routines forward and backward to check algorithms.
 */
BOOST_AUTO_TEST_CASE( complexInverse_matrix_test ) {

    matrix< std::complex<double> > cmat(2,3);
    int k = 0;
    for ( size_t n=0; n < cmat.size1(); ++n ) {
        for ( size_t m=0; m < cmat.size2(); ++m ) {
            cmat(n,m) = 0.1 * std::complex<double>((k++)+1,1.0);
        }
    }

    cout << "=== matrix_test: complexInverse_matrix_test ===" << endl;
    cout << "acos(cos(t)):    " << acos(cos(cmat)) << endl;
    USML_MATRIX_COMPLEX_TESTER( acos(cos(cmat)), cmat, identity<complex<double> > ) ;
    cout << "acosh(cosh(t)):  " << acosh(cosh(cmat)) << endl;
    USML_MATRIX_COMPLEX_TESTER( acosh(cosh(cmat)), cmat, identity<complex<double> > ) ;
    cout << "asin(sin(t)):    " << asin(sin(cmat)) << endl;
    USML_MATRIX_COMPLEX_TESTER( asin(sin(cmat)), cmat, identity<complex<double> > ) ;
    cout << "asinh(sinh(t)):  " << asinh(sinh(cmat)) << endl;
    USML_MATRIX_COMPLEX_TESTER( asinh(sinh(cmat)), cmat, identity<complex<double> > ) ;
    cout << "atan(tan(t)):    " << atan(tan(cmat)) << endl;
    USML_MATRIX_COMPLEX_TESTER( atan(tan(cmat)), cmat, identity<complex<double> > ) ;
    cout << "atanh(tanh(t)):  " << atanh(tanh(cmat)) << endl;
    USML_MATRIX_COMPLEX_TESTER( atanh(tanh(cmat)), cmat, identity<complex<double> > ) ;
}

/**
* Test the ability to use uBLAS matrix proxies.
* If successful the first digit of each entry should be 1,
* the second digit should be the row number, and
* the third digit should be the column number.
*/
BOOST_AUTO_TEST_CASE(ublas_proxy_test) {
	cout << "=== matrix_test: ublas_proxy_test ===" << endl;

	// fill a vector wit column numbers

	vector<double> v(3);
	for (unsigned i = 0; i < v.size(); ++i) {
		v(i) = i + 1;
	}

	// initialize matrix to all values of 100.0

	matrix<double> m(3, 3);
	m = matrix<double>(3, 3, 100.0);

	// add column and row number to existing matrix data

	for (unsigned i = 0; i < m.size1(); ++i) {
		matrix_row< matrix<double> > mr(m, i);
		mr += v + 10 * (i + 1);
	}
	std::cout << m << std::endl;
	BOOST_CHECK_CLOSE(m(2,2), 133, 1e-10);
}

/// @}

BOOST_AUTO_TEST_SUITE_END()


#pragma once
#ifndef EIGEN_HPP
#define EIGEN_HPP

#define EIGEN_NO_DEBUG

//c++ libraries
#include <iosfwd>
//eigen
#include <Eigen/Dense>
//ann serialize
#include "src/mem/serialize.hpp"

namespace eigen{

struct LIN_SOLVER{
	enum type{
		LLT=0,//cholesky decomposition
		LDLT=1,//cholesky-variant
		PPLU=2,//LU - partial pivoting
		FPLU=3,//LU - full pivoting
		HQR=4,//Householder QR
		CPHQR=5,//Householder QR - column pivoting
		UNKNOWN=-1
	};
	static LIN_SOLVER::type read(const char* str);
};
std::ostream& operator<<(std::ostream& out, const LIN_SOLVER::type& t);

}

namespace serialize{

//**********************************************
// byte measures
//**********************************************

template <> int nbytes(const Eigen::Vector3d& obj);
template <> int nbytes(const Eigen::VectorXd& obj);
template <> int nbytes(const Eigen::Matrix3d& obj);
template <> int nbytes(const Eigen::MatrixXd& obj);
template <> int nbytes(const std::vector<Eigen::Vector3d>& obj);
template <> int nbytes(const std::vector<Eigen::VectorXd>& obj);

//**********************************************
// packing
//**********************************************

template <> int pack(const Eigen::Vector3d& obj, char* arr);
template <> int pack(const Eigen::VectorXd& obj, char* arr);
template <> int pack(const Eigen::Matrix3d& obj, char* arr);
template <> int pack(const Eigen::MatrixXd& obj, char* arr);
template <> int pack(const std::vector<Eigen::Vector3d>& obj, char* arr);
template <> int pack(const std::vector<Eigen::VectorXd>& obj, char* arr);

//**********************************************
// unpacking
//**********************************************

template <> int unpack(Eigen::Vector3d& obj, const char* arr);
template <> int unpack(Eigen::VectorXd& obj, const char* arr);
template <> int unpack(Eigen::Matrix3d& obj, const char* arr);
template <> int unpack(Eigen::MatrixXd& obj, const char* arr);
template <> int unpack(std::vector<Eigen::Vector3d>& obj, const char* arr);
template <> int unpack(std::vector<Eigen::VectorXd>& obj, const char* arr);

}

#endif


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN set_intersection tests

#include <boost/test/unit_test.hpp>

#include <algorithm>
#include <cmath>
#include <functional>
#include <iterator>
#include <list>
#include <vector>

BOOST_AUTO_TEST_SUITE( set_intersection_test_suite )

BOOST_AUTO_TEST_CASE( empty_primary_input_default_compare )
{
    const std::vector<int> primary;
    const std::list<int> secondary{1, 2, 3};
    std::vector<int> output;

    std::set_intersection(std::begin(primary), std::end(primary), std::begin(secondary), std::end(secondary), std::back_inserter(output));

    const std::vector<int> expected;
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(output), std::end(output), std::begin(expected), std::end(expected));
}

BOOST_AUTO_TEST_CASE( empty_primary_input_custom_compare )
{
    const std::vector<int> primary;
    const std::list<int> secondary{3, 2, 1};
    std::vector<int> output;

    std::set_intersection(std::begin(primary), std::end(primary), std::begin(secondary), std::end(secondary), std::back_inserter(output), std::greater<int>());

    const std::vector<int> expected;
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(output), std::end(output), std::begin(expected), std::end(expected));
}

BOOST_AUTO_TEST_CASE( empty_primary_input_conversion_compare )
{
    const std::vector<int> primary;
    const std::list<int> secondary{1, 2, 3};
    std::vector<int> output;

    std::set_intersection(std::begin(primary), std::end(primary), std::begin(secondary), std::end(secondary), std::back_inserter(output), std::less<double>());

    const std::vector<int> expected;
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(output), std::end(output), std::begin(expected), std::end(expected));
}

BOOST_AUTO_TEST_CASE( same_type_inputs_default_compare )
{
    const std::vector<int> primary{1, 3, 4, 5, 6};
    const std::list<int> secondary{1, 2, 3};
    std::vector<int> output;

    std::set_intersection(std::begin(primary), std::end(primary), std::begin(secondary), std::end(secondary), std::back_inserter(output));

    const std::vector<int> expected{1, 3};
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(output), std::end(output), std::begin(expected), std::end(expected));
}

BOOST_AUTO_TEST_CASE( same_type_inputs_custom_compare )
{
    const std::vector<int> primary{1, -3, -4, 5, -6};
    const std::list<int> secondary{1, -2, 3};
    std::vector<int> output;

    auto comp = [](int lhs, int rhs){ return std::less<int>()(std::abs(lhs), std::abs(rhs)); };
    std::set_intersection(std::begin(primary), std::end(primary), std::begin(secondary), std::end(secondary), std::back_inserter(output), comp);

    const std::vector<int> expected{1, -3};
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(output), std::end(output), std::begin(expected), std::end(expected));
}

BOOST_AUTO_TEST_CASE( same_type_inputs_conversion_compare )
{
    const std::vector<int> primary{1, -3, -4, 5, -6};
    const std::list<int> secondary{1, -2, 3};
    std::vector<int> output;

    auto comp = [](double lhs, double rhs){ return std::less<double>()(std::abs(lhs), std::abs(rhs)); };
    std::set_intersection(std::begin(primary), std::end(primary), std::begin(secondary), std::end(secondary), std::back_inserter(output), comp);

    const std::vector<int> expected{1, -3};
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(output), std::end(output), std::begin(expected), std::end(expected));
}

BOOST_AUTO_TEST_CASE( different_type_inputs_default_compare )
{
    const std::vector<int> primary{1, 3, 4, 5, 6};
    const std::list<double> secondary{1.1, 2, 4.0, 4.6};
    std::vector<int> output;

    /*! For the default compare between an int and a double, the int will be
     * promoted to double first, so for example 1 will be considered different
     * to 1.1, in which case none of them will be reported to the output
     * iterator. */
    std::set_intersection(std::begin(primary), std::end(primary), std::begin(secondary), std::end(secondary), std::back_inserter(output));

    const std::vector<int> expected{4};
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(output), std::end(output), std::begin(expected), std::end(expected));
}

BOOST_AUTO_TEST_CASE( different_type_inputs_custom_compare )
{
    const std::vector<int> primary{1, 3, 4, 5, 6};
    const std::list<double> secondary{1.1, 2, 3.9, 4.6};
    std::vector<int> output;

    auto comp = [](int lhs, double rhs){ return std::less<double>()(static_cast<double>(lhs), std::floor(rhs)); };
    std::set_intersection(std::begin(primary), std::end(primary), std::begin(secondary), std::end(secondary), std::back_inserter(output), comp);

    const std::vector<int> expected{1, 3, 4};
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(output), std::end(output), std::begin(expected), std::end(expected));
}

BOOST_AUTO_TEST_CASE( repeated_elements_default_compare )
{
    const std::vector<int> primary{1, 1, 1, 1, 2, 3, 3, 3, 3, 4, 4, 5, 6};
    const std::list<int> secondary{1, 1, 2, 3};
    std::vector<int> output;

    std::set_intersection(std::begin(primary), std::end(primary), std::begin(secondary), std::end(secondary), std::back_inserter(output));

    const std::vector<int> expected{1, 1, 2, 3};
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(output), std::end(output), std::begin(expected), std::end(expected));
}

BOOST_AUTO_TEST_CASE( different_output_type_default_compare )
{
    const std::vector<double> primary{1, 3, 4.6, 5, 6};
    const std::list<double> secondary{1.1, 2, 3.9, 4.6};
    std::vector<int> output;

    std::set_intersection(std::begin(primary), std::end(primary), std::begin(secondary), std::end(secondary), std::back_inserter(output));

    const std::vector<int> expected{4};
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(output), std::end(output), std::begin(expected), std::end(expected));
}

BOOST_AUTO_TEST_CASE( different_output_type_custom_compare )
{
    const std::vector<double> primary{-1.1, 1, -1, 3, 4.6, 5, 6};
    const std::list<double> secondary{1.1, -2, 3.9, -4.6};
    std::vector<int> output;

    auto comp = [](double lhs, double rhs){ return std::less<double>()(std::abs(lhs), std::abs(rhs)); };
    std::set_intersection(std::begin(primary), std::end(primary), std::begin(secondary), std::end(secondary), std::back_inserter(output), comp);

    /*! Here is the interesting thing: -1.1 will be compared equal to 1.1 and
     * reported to an output iterator that accepts ints. As the algorithm
     * reports the element from the first collection, -1.1 will be cast to int,
     * so the number in the output collection will be negative as well. */
    const std::vector<int> expected{-1, 4};
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(output), std::end(output), std::begin(expected), std::end(expected));
}

BOOST_AUTO_TEST_SUITE_END( /* set_intersection_test_suite */ )


#pragma once

#include <Eigen/Core>

#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

///////////////////////////////////////////////////////////////////////////////
// Point - Point

template <typename DerivedDP0, typename DerivedDP1>
inline auto point_point_relative_displacement(
    const Eigen::MatrixBase<DerivedDP0>& dp0,
    const Eigen::MatrixBase<DerivedDP1>& dp1)
{
    return dp0 - dp1;
}

template <
    typename DerivedDisp,
    typename DerivedBasis,
    typename T = typename DerivedDisp::Scalar>
inline VectorMax6<T> point_point_relative_mesh_displacements(
    const Eigen::MatrixBase<DerivedDisp>& tangent_relative_displacement,
    const Eigen::MatrixBase<DerivedBasis>& basis)
{
    int dim = basis.rows();
    VectorMax6<T> mesh_displacements(2 * dim);
    mesh_displacements.head(dim) = basis * tangent_relative_displacement;
    mesh_displacements.tail(dim) = -mesh_displacements.head(dim);
    return mesh_displacements;
}

template <typename DerivedBasis>
inline MatrixMax<typename DerivedBasis::Scalar, 2, 6>
point_point_TT(const Eigen::MatrixBase<DerivedBasis>& basis)
{
    MatrixMax<typename DerivedBasis::Scalar, 2, 6> TT(
        basis.cols(), 2 * basis.rows());
    TT.leftCols(basis.rows()) = basis.transpose();
    TT.rightCols(basis.rows()) = -basis.transpose();
    return TT;
}

///////////////////////////////////////////////////////////////////////////////
// Point - Edge

template <
    typename DerivedDP,
    typename DerivedDE0,
    typename DerivedDE1,
    typename T>
inline auto point_edge_relative_displacement(
    const Eigen::MatrixBase<DerivedDP>& dp,
    const Eigen::MatrixBase<DerivedDE0>& de0,
    const Eigen::MatrixBase<DerivedDE1>& de1,
    const T& alpha)
{
    return dp - ((de1 - de0) * alpha + de0);
}

template <typename DerivedDisp, typename DerivedBasis, typename T>
inline VectorMax9<T> point_edge_relative_mesh_displacements(
    const Eigen::MatrixBase<DerivedDisp>& tangent_relative_displacement,
    const Eigen::MatrixBase<DerivedBasis>& basis,
    const T& alpha)
{
    int dim = basis.rows();
    VectorMax9<T> mesh_displacements(3 * dim);
    mesh_displacements.head(dim) = basis * tangent_relative_displacement;
    mesh_displacements.segment(1 * dim, dim) =
        (alpha - 1.0) * mesh_displacements.head(dim);
    mesh_displacements.tail(dim) = -alpha * mesh_displacements.head(dim);
    return mesh_displacements;
}

template <typename DerivedBasis, typename T>
inline MatrixMax<typename DerivedBasis::Scalar, 2, 9>
point_edge_TT(const Eigen::MatrixBase<DerivedBasis>& basis, const T& alpha)
{
    MatrixMax<typename DerivedBasis::Scalar, 2, 9> TT(
        basis.cols(), 3 * basis.rows());
    TT.leftCols(basis.rows()) = basis.transpose();
    TT.middleCols(basis.rows(), basis.rows()) =
        (alpha - 1.0) * basis.transpose();
    TT.rightCols(basis.rows()) = -alpha * basis.transpose();
    return TT;
}

///////////////////////////////////////////////////////////////////////////////
// Edge - Edge

/// Compute the relative displacement of the edges.
template <
    typename DerivedDEA0,
    typename DerivedDEA1,
    typename DerivedDEB0,
    typename DerivedDEB1,
    typename DerivedBarycentricCoordinates>
inline auto edge_edge_relative_displacement(
    const Eigen::MatrixBase<DerivedDEA0>& dea0,
    const Eigen::MatrixBase<DerivedDEA1>& dea1,
    const Eigen::MatrixBase<DerivedDEB0>& deb0,
    const Eigen::MatrixBase<DerivedDEB1>& deb1,
    const Eigen::MatrixBase<DerivedBarycentricCoordinates>&
        barrycentric_coordinates)
{
    // closest_point_a_displacement - closest_point_b_displacement
    return ((dea1 - dea0) * barrycentric_coordinates[0] + dea0)
        - ((deb1 - deb0) * barrycentric_coordinates[1] + deb0);
}

template <
    typename DerivedDisp,
    typename DerivedBasis,
    typename DerivedBeta,
    typename T = typename DerivedDisp::Scalar>
inline VectorMax12<T> edge_edge_relative_mesh_displacements(
    const Eigen::MatrixBase<DerivedDisp>& tangent_relative_displacement,
    const Eigen::MatrixBase<DerivedBasis>& basis,
    const Eigen::MatrixBase<DerivedBeta>& gamma)
{
    VectorMax3<T> rel_disp = basis * tangent_relative_displacement;
    int dim = rel_disp.size();
    VectorMax12<T> mesh_displacements(4 * dim);
    mesh_displacements.head(dim) = (1.0 - gamma[0]) * rel_disp; // dea0
    mesh_displacements.segment(dim, dim) = gamma[0] * rel_disp; // dea1
    mesh_displacements.segment(2 * dim, dim) =
        (gamma[1] - 1.0) * rel_disp;                     // deb0
    mesh_displacements.tail(dim) = -gamma[1] * rel_disp; // deb1
    return mesh_displacements;
}

template <typename DerivedBasis, typename DerivedGamma>
inline MatrixMax<typename DerivedBasis::Scalar, 2, 12> edge_edge_TT(
    const Eigen::MatrixBase<DerivedBasis>& basis,
    const Eigen::MatrixBase<DerivedGamma>& gamma)
{
    MatrixMax<typename DerivedBasis::Scalar, 2, 12> TT(
        basis.cols(), 4 * basis.rows());
    TT.leftCols(basis.rows()) = (1.0 - gamma[0]) * basis.transpose();
    TT.middleCols(basis.rows(), basis.rows()) = gamma[0] * basis.transpose();
    TT.middleCols(2 * basis.rows(), basis.rows()) =
        (gamma[1] - 1.0) * basis.transpose();
    TT.rightCols(basis.rows()) = -gamma[1] * basis.transpose();
    return TT;
}

///////////////////////////////////////////////////////////////////////////////
// Point - Triangle

/// Compute the relative displacement of the point to the triangle.
template <
    typename DerivedDP,
    typename DerivedDT0,
    typename DerivedDT1,
    typename DerivedDT2,
    typename DerivedBarycentricCoordinates>
inline auto point_triangle_relative_displacement(
    const Eigen::MatrixBase<DerivedDP>& dp,
    const Eigen::MatrixBase<DerivedDT0>& dt0,
    const Eigen::MatrixBase<DerivedDT1>& dt1,
    const Eigen::MatrixBase<DerivedDT2>& dt2,
    const Eigen::MatrixBase<DerivedBarycentricCoordinates>&
        barycentric_coordinates)
{
    // Compute the displacement of the closest point and subtract it from the
    // points displacement.
    return dp
        - (dt0 + barycentric_coordinates[0] * (dt1 - dt0)
           + barycentric_coordinates[1] * (dt2 - dt0));
}

template <
    typename DerivedDisp,
    typename DerivedBasis,
    typename DerivedBeta,
    typename T = typename DerivedDisp::Scalar>
inline VectorMax12<T> point_triangle_relative_mesh_displacements(
    const Eigen::MatrixBase<DerivedDisp>& tangent_relative_displacement,
    const Eigen::MatrixBase<DerivedBasis>& basis,
    const Eigen::MatrixBase<DerivedBeta>& beta)
{
    int dim = basis.rows();
    VectorMax12<T> mesh_displacements(4 * dim);
    mesh_displacements.head(dim) = basis * tangent_relative_displacement;
    mesh_displacements.segment(dim, dim) =
        (-1 + beta[0] + beta[1]) * mesh_displacements.head(dim);
    mesh_displacements.segment(2 * dim, dim) =
        -beta[0] * mesh_displacements.head(dim);
    mesh_displacements.tail(dim) = -beta[1] * mesh_displacements.head(dim);
    return mesh_displacements;
}

template <typename DerivedBasis, typename DerivedBeta>
inline MatrixMax<typename DerivedBasis::Scalar, 2, 12> point_triangle_TT(
    const Eigen::MatrixBase<DerivedBasis>& basis,
    const Eigen::MatrixBase<DerivedBeta>& beta)
{
    MatrixMax<typename DerivedBasis::Scalar, 2, 12> TT(
        basis.cols(), 4 * basis.rows());
    TT.leftCols(basis.rows()) = basis.transpose();
    TT.middleCols(basis.rows(), basis.rows()) =
        (-1 + beta[0] + beta[1]) * basis.transpose();
    TT.middleCols(2 * basis.rows(), basis.rows()) =
        -beta[0] * basis.transpose();
    TT.rightCols(basis.rows()) = -beta[1] * basis.transpose();
    return TT;
}

} // namespace ipc