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

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.

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

#define RAPIDXML_NO_EXCEPTIONS
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include <itkImageFileWriter.h>
#include <itkImageFileReader.h>
#include <mitkLog.h>
#include <algorithm>
#include <string>

template< class ScalarType >
mitk::FiberfoxParameters< ScalarType >::FiberfoxParameters()
  : m_NoiseModel(nullptr)
{

}

template< class ScalarType >
mitk::FiberfoxParameters< ScalarType >::~FiberfoxParameters()
{
  //    if (m_NoiseModel!=nullptr)
  //        delete m_NoiseModel;
}

void mitk::SignalGenerationParameters::GenerateGradientHalfShell()
{
  int NPoints = 2*m_NumGradients;
  m_GradientDirections.clear();

  m_NumBaseline = NPoints/20;
  if (m_NumBaseline==0)
    m_NumBaseline=1;

  GradientType g;
  g.Fill(0.0);
  for (unsigned int i=0; i<m_NumBaseline; i++)
    m_GradientDirections.push_back(g);

  if (NPoints==0)
    return;

  vnl_vector<double> theta; theta.set_size(NPoints);
  vnl_vector<double> phi; phi.set_size(NPoints);
  double C = sqrt(4*M_PI);
  phi(0) = 0.0;
  phi(NPoints-1) = 0.0;
  for(int i=0; i<NPoints; i++)
  {
    theta(i) = acos(-1.0+2.0*i/(NPoints-1.0)) - M_PI / 2.0;
    if( i>0 && i<NPoints-1)
    {
      phi(i) = (phi(i-1) + C /
                sqrt(NPoints*(1-(-1.0+2.0*i/(NPoints-1.0))*(-1.0+2.0*i/(NPoints-1.0)))));
    }
  }

  for(int i=0; i<NPoints; i++)
  {
    g[2] = sin(theta(i));
    if (g[2]<0)
      continue;
    g[0] = cos(theta(i)) * cos(phi(i));
    g[1] = cos(theta(i)) * sin(phi(i));
    m_GradientDirections.push_back(g);
  }
}

std::vector< int > mitk::SignalGenerationParameters::GetBaselineIndices()
{
  std::vector< int > result;
  for( unsigned int i=0; i<this->m_GradientDirections.size(); i++)
    if (m_GradientDirections.at(i).GetNorm()<0.0001)
      result.push_back(i);
  return result;
}

unsigned int mitk::SignalGenerationParameters::GetFirstBaselineIndex()
{
  for( unsigned int i=0; i<this->m_GradientDirections.size(); i++)
    if (m_GradientDirections.at(i).GetNorm()<0.0001)
      return i;
  return -1;
}

bool mitk::SignalGenerationParameters::IsBaselineIndex(unsigned int idx)
{
  if (m_GradientDirections.size()>idx && m_GradientDirections.at(idx).GetNorm()<0.0001)
    return true;
  return false;
}

unsigned int mitk::SignalGenerationParameters::GetNumWeightedVolumes()
{
  return m_NumGradients;
}

unsigned int mitk::SignalGenerationParameters::GetNumBaselineVolumes()
{
  return m_NumBaseline;
}

unsigned int mitk::SignalGenerationParameters::GetNumVolumes()
{
  return m_GradientDirections.size();
}

mitk::SignalGenerationParameters::GradientListType mitk::SignalGenerationParameters::GetGradientDirections()
{
  return m_GradientDirections;
}

mitk::SignalGenerationParameters::GradientType mitk::SignalGenerationParameters::GetGradientDirection(unsigned int i)
{
  return m_GradientDirections.at(i);
}

void mitk::SignalGenerationParameters::SetNumWeightedVolumes(int numGradients)
{
  m_NumGradients = numGradients;
  GenerateGradientHalfShell();
}

std::vector< int > mitk::SignalGenerationParameters::GetBvalues()
{
  std::vector< int > bVals;
  for( GradientType g : m_GradientDirections)
  {
    float norm = g.GetNorm();
    int bVal = std::round(norm*norm*m_Bvalue);
    if ( std::find(bVals.begin(), bVals.end(), bVal) == bVals.end() )
      bVals.push_back(bVal);
  }
  return bVals;
}

void mitk::SignalGenerationParameters::SetGradienDirections(GradientListType gradientList)
{
  m_GradientDirections = gradientList;
  m_NumGradients = 0;
  m_NumBaseline = 0;

  for( unsigned int i=0; i<this->m_GradientDirections.size(); i++)
  {
    float norm = m_GradientDirections.at(i).GetNorm();
    if (norm>0.0001)
      m_NumGradients++;
    else
      m_NumBaseline++;
  }
}

void mitk::SignalGenerationParameters::SetGradienDirections(mitk::DiffusionPropertyHelper::GradientDirectionsContainerType::Pointer gradientList)
{
  m_NumGradients = 0;
  m_NumBaseline = 0;
  m_GradientDirections.clear();

  for( unsigned int i=0; i<gradientList->Size(); i++)
  {
    GradientType g;
    g[0] = gradientList->at(i)[0];
    g[1] = gradientList->at(i)[1];
    g[2] = gradientList->at(i)[2];
    m_GradientDirections.push_back(g);

    float norm = m_GradientDirections.at(i).GetNorm();
    if (norm>0.0001)
      m_NumGradients++;
    else
      m_NumBaseline++;
  }
}

template< class ScalarType >
void mitk::FiberfoxParameters< ScalarType >::SaveParameters(std::string filename)
{
  if(filename.empty())
    return;
  if(".ffp"!=filename.substr(filename.size()-4, 4))
    filename += ".ffp";

  const std::string& locale = "C";
  const std::string& currLocale = setlocale( LC_ALL, nullptr );

  if ( locale.compare(currLocale)!=0 )
  {
    try
    {
      setlocale(LC_ALL, locale.c_str());
    }
    catch(...)
    {
      MITK_INFO << "Could not set locale " << locale;
    }
  }

  boost::property_tree::ptree parameters;

  // fiber generation parameters
  parameters.put("fiberfox.fibers.distribution", m_FiberGen.m_Distribution);
  parameters.put("fiberfox.fibers.variance", m_FiberGen.m_Variance);
  parameters.put("fiberfox.fibers.density", m_FiberGen.m_Density);
  parameters.put("fiberfox.fibers.spline.sampling", m_FiberGen.m_Sampling);
  parameters.put("fiberfox.fibers.spline.tension", m_FiberGen.m_Tension);
  parameters.put("fiberfox.fibers.spline.continuity", m_FiberGen.m_Continuity);
  parameters.put("fiberfox.fibers.spline.bias", m_FiberGen.m_Bias);
  parameters.put("fiberfox.fibers.rotation.x", m_FiberGen.m_Rotation[0]);
  parameters.put("fiberfox.fibers.rotation.y", m_FiberGen.m_Rotation[1]);
  parameters.put("fiberfox.fibers.rotation.z", m_FiberGen.m_Rotation[2]);
  parameters.put("fiberfox.fibers.translation.x", m_FiberGen.m_Translation[0]);
  parameters.put("fiberfox.fibers.translation.y", m_FiberGen.m_Translation[1]);
  parameters.put("fiberfox.fibers.translation.z", m_FiberGen.m_Translation[2]);
  parameters.put("fiberfox.fibers.scale.x", m_FiberGen.m_Scale[0]);
  parameters.put("fiberfox.fibers.scale.y", m_FiberGen.m_Scale[1]);
  parameters.put("fiberfox.fibers.scale.z", m_FiberGen.m_Scale[2]);

  // image generation parameters
  parameters.put("fiberfox.image.basic.size.x", m_SignalGen.m_ImageRegion.GetSize(0));
  parameters.put("fiberfox.image.basic.size.y", m_SignalGen.m_ImageRegion.GetSize(1));
  parameters.put("fiberfox.image.basic.size.z", m_SignalGen.m_ImageRegion.GetSize(2));
  parameters.put("fiberfox.image.basic.spacing.x", m_SignalGen.m_ImageSpacing[0]);
  parameters.put("fiberfox.image.basic.spacing.y", m_SignalGen.m_ImageSpacing[1]);
  parameters.put("fiberfox.image.basic.spacing.z", m_SignalGen.m_ImageSpacing[2]);
  parameters.put("fiberfox.image.basic.origin.x", m_SignalGen.m_ImageOrigin[0]);
  parameters.put("fiberfox.image.basic.origin.y", m_SignalGen.m_ImageOrigin[1]);
  parameters.put("fiberfox.image.basic.origin.z", m_SignalGen.m_ImageOrigin[2]);
  parameters.put("fiberfox.image.basic.direction.1", m_SignalGen.m_ImageDirection[0][0]);
  parameters.put("fiberfox.image.basic.direction.2", m_SignalGen.m_ImageDirection[0][1]);
  parameters.put("fiberfox.image.basic.direction.3", m_SignalGen.m_ImageDirection[0][2]);
  parameters.put("fiberfox.image.basic.direction.4", m_SignalGen.m_ImageDirection[1][0]);
  parameters.put("fiberfox.image.basic.direction.5", m_SignalGen.m_ImageDirection[1][1]);
  parameters.put("fiberfox.image.basic.direction.6", m_SignalGen.m_ImageDirection[1][2]);
  parameters.put("fiberfox.image.basic.direction.7", m_SignalGen.m_ImageDirection[2][0]);
  parameters.put("fiberfox.image.basic.direction.8", m_SignalGen.m_ImageDirection[2][1]);
  parameters.put("fiberfox.image.basic.direction.9", m_SignalGen.m_ImageDirection[2][2]);
  parameters.put("fiberfox.image.basic.numgradients", m_SignalGen.GetNumWeightedVolumes());
  for( unsigned int i=0; i<this->m_SignalGen.GetNumVolumes(); i++)
  {
    parameters.put("fiberfox.image.gradients."+boost::lexical_cast<std::string>(i)+".x", m_SignalGen.GetGradientDirection(i)[0]);
    parameters.put("fiberfox.image.gradients."+boost::lexical_cast<std::string>(i)+".y", m_SignalGen.GetGradientDirection(i)[1]);
    parameters.put("fiberfox.image.gradients."+boost::lexical_cast<std::string>(i)+".z", m_SignalGen.GetGradientDirection(i)[2]);
  }

  parameters.put("fiberfox.image.acquisitiontype", m_SignalGen.m_AcquisitionType);
  parameters.put("fiberfox.image.coilsensitivityprofile", m_SignalGen.m_CoilSensitivityProfile);
  parameters.put("fiberfox.image.numberofcoils", m_SignalGen.m_NumberOfCoils);
  parameters.put("fiberfox.image.reversephase", m_SignalGen.m_ReversePhase);
  parameters.put("fiberfox.image.partialfourier", m_SignalGen.m_PartialFourier);
  parameters.put("fiberfox.image.noisevariance", m_SignalGen.m_NoiseVariance);
  parameters.put("fiberfox.image.trep", m_SignalGen.m_tRep);
  parameters.put("fiberfox.image.signalScale", m_SignalGen.m_SignalScale);
  parameters.put("fiberfox.image.tEcho", m_SignalGen.m_tEcho);
  parameters.put("fiberfox.image.tLine", m_SignalGen.m_tLine);
  parameters.put("fiberfox.image.tInhom", m_SignalGen.m_tInhom);
  parameters.put("fiberfox.image.bvalue", m_SignalGen.m_Bvalue);
  parameters.put("fiberfox.image.simulatekspace", m_SignalGen.m_SimulateKspaceAcquisition);
  parameters.put("fiberfox.image.axonRadius", m_SignalGen.m_AxonRadius);
  parameters.put("fiberfox.image.doSimulateRelaxation", m_SignalGen.m_DoSimulateRelaxation);
  parameters.put("fiberfox.image.doDisablePartialVolume", m_SignalGen.m_DoDisablePartialVolume);
  parameters.put("fiberfox.image.artifacts.spikesnum", m_SignalGen.m_Spikes);
  parameters.put("fiberfox.image.artifacts.spikesscale", m_SignalGen.m_SpikeAmplitude);
  parameters.put("fiberfox.image.artifacts.kspaceLineOffset", m_SignalGen.m_KspaceLineOffset);
  parameters.put("fiberfox.image.artifacts.eddyStrength", m_SignalGen.m_EddyStrength);
  parameters.put("fiberfox.image.artifacts.eddyTau", m_SignalGen.m_Tau);
  parameters.put("fiberfox.image.artifacts.aliasingfactor", m_SignalGen.m_CroppingFactor);
  parameters.put("fiberfox.image.artifacts.addringing", m_SignalGen.m_DoAddGibbsRinging);
  parameters.put("fiberfox.image.artifacts.doAddMotion", m_SignalGen.m_DoAddMotion);
  parameters.put("fiberfox.image.artifacts.randomMotion", m_SignalGen.m_DoRandomizeMotion);
  parameters.put("fiberfox.image.artifacts.translation0", m_SignalGen.m_Translation[0]);
  parameters.put("fiberfox.image.artifacts.translation1", m_SignalGen.m_Translation[1]);
  parameters.put("fiberfox.image.artifacts.translation2", m_SignalGen.m_Translation[2]);
  parameters.put("fiberfox.image.artifacts.rotation0", m_SignalGen.m_Rotation[0]);
  parameters.put("fiberfox.image.artifacts.rotation1", m_SignalGen.m_Rotation[1]);
  parameters.put("fiberfox.image.artifacts.rotation2", m_SignalGen.m_Rotation[2]);
  parameters.put("fiberfox.image.artifacts.motionvolumes", m_Misc.m_MotionVolumesBox);
  parameters.put("fiberfox.image.artifacts.addnoise", m_Misc.m_CheckAddNoiseBox);
  parameters.put("fiberfox.image.artifacts.addghosts", m_Misc.m_CheckAddGhostsBox);
  parameters.put("fiberfox.image.artifacts.addaliasing", m_Misc.m_CheckAddAliasingBox);
  parameters.put("fiberfox.image.artifacts.addspikes", m_Misc.m_CheckAddSpikesBox);
  parameters.put("fiberfox.image.artifacts.addeddycurrents", m_Misc.m_CheckAddEddyCurrentsBox);
  parameters.put("fiberfox.image.artifacts.doAddDistortions", m_Misc.m_CheckAddDistortionsBox);
  parameters.put("fiberfox.image.outputvolumefractions", m_Misc.m_CheckOutputVolumeFractionsBox);
  parameters.put("fiberfox.image.showadvanced", m_Misc.m_CheckAdvancedSignalOptionsBox);
  parameters.put("fiberfox.image.signalmodelstring", m_Misc.m_SignalModelString);
  parameters.put("fiberfox.image.artifactmodelstring", m_Misc.m_ArtifactModelString);
  parameters.put("fiberfox.image.outpath", m_Misc.m_OutputPath);
  parameters.put("fiberfox.fibers.realtime", m_Misc.m_CheckRealTimeFibersBox);
  parameters.put("fiberfox.fibers.showadvanced", m_Misc.m_CheckAdvancedFiberOptionsBox);
  parameters.put("fiberfox.fibers.constantradius", m_Misc.m_CheckConstantRadiusBox);
  parameters.put("fiberfox.fibers.includeFiducials", m_Misc.m_CheckIncludeFiducialsBox);

  if (m_NoiseModel!=nullptr)
  {
    parameters.put("fiberfox.image.artifacts.noisevariance", m_NoiseModel->GetNoiseVariance());
    if (dynamic_cast<mitk::RicianNoiseModel<ScalarType>*>(m_NoiseModel.get()))
      parameters.put("fiberfox.image.artifacts.noisetype", "rice");
    else if (dynamic_cast<mitk::ChiSquareNoiseModel<ScalarType>*>(m_NoiseModel.get()))
      parameters.put("fiberfox.image.artifacts.noisetype", "chisquare");
  }

  for (std::size_t i=0; i<m_FiberModelList.size()+m_NonFiberModelList.size(); i++)
  {
    mitk::DiffusionSignalModel<ScalarType>* signalModel = nullptr;
    if (i<m_FiberModelList.size())
    {
      signalModel = m_FiberModelList.at(i);
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".type", "fiber");
    }
    else
    {
      signalModel = m_NonFiberModelList.at(i-m_FiberModelList.size());
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".type", "non-fiber");
    }

    if (dynamic_cast<mitk::StickModel<ScalarType>*>(signalModel))
    {
      mitk::StickModel<ScalarType>* model = dynamic_cast<mitk::StickModel<ScalarType>*>(signalModel);
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".model", "stick");
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".d", model->GetDiffusivity());
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".t2", model->GetT2());
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".t1", model->GetT1());
    }
    else  if (dynamic_cast<mitk::TensorModel<ScalarType>*>(signalModel))
    {
      mitk::TensorModel<ScalarType>* model = dynamic_cast<mitk::TensorModel<ScalarType>*>(signalModel);
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".model", "tensor");
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".d1", model->GetDiffusivity1());
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".d2", model->GetDiffusivity2());
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".d3", model->GetDiffusivity3());
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".t2", model->GetT2());
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".t1", model->GetT1());
    }
    else  if (dynamic_cast<mitk::RawShModel<ScalarType>*>(signalModel))
    {
      mitk::RawShModel<ScalarType>* model = dynamic_cast<mitk::RawShModel<ScalarType>*>(signalModel);
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".model", "prototype");
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".minFA", model->GetFaRange().first);
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".maxFA", model->GetFaRange().second);
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".minADC", model->GetAdcRange().first);
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".maxADC", model->GetAdcRange().second);
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".maxNumSamples", model->GetMaxNumKernels());
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".numSamples", model->GetNumberOfKernels());
      int shOrder = model->GetShOrder();
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".numCoeffs", (shOrder*shOrder + shOrder + 2)/2 + shOrder);

      for (unsigned int j=0; j<model->GetNumberOfKernels(); j++)
      {
        vnl_vector< double > coeffs = model->GetCoefficients(j);
        for (unsigned int k=0; k<coeffs.size(); k++)
          parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".kernels."+boost::lexical_cast<std::string>(j)+".coeffs."+boost::lexical_cast<std::string>(k), coeffs[k]);
        parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".kernels."+boost::lexical_cast<std::string>(j)+".B0", model->GetBaselineSignal(j));
      }
    }
    else  if (dynamic_cast<mitk::BallModel<ScalarType>*>(signalModel))
    {
      mitk::BallModel<ScalarType>* model = dynamic_cast<mitk::BallModel<ScalarType>*>(signalModel);
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".model", "ball");
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".d", model->GetDiffusivity());
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".t2", model->GetT2());
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".t1", model->GetT1());
    }
    else  if (dynamic_cast<mitk::AstroStickModel<ScalarType>*>(signalModel))
    {
      mitk::AstroStickModel<ScalarType>* model = dynamic_cast<mitk::AstroStickModel<ScalarType>*>(signalModel);
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".model", "astrosticks");
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".d", model->GetDiffusivity());
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".t2", model->GetT2());
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".t1", model->GetT1());
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".randomize", model->GetRandomizeSticks());
    }
    else  if (dynamic_cast<mitk::DotModel<ScalarType>*>(signalModel))
    {
      mitk::DotModel<ScalarType>* model = dynamic_cast<mitk::DotModel<ScalarType>*>(signalModel);
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".model", "dot");
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".t2", model->GetT2());
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".t1", model->GetT1());
    }

    if (signalModel!=nullptr)
    {
      parameters.put("fiberfox.image.compartments."+boost::lexical_cast<std::string>(i)+".ID", signalModel->m_CompartmentId);

      if (signalModel->GetVolumeFractionImage().IsNotNull())
      {
        try{
          itk::ImageFileWriter<ItkDoubleImgType>::Pointer writer = itk::ImageFileWriter<ItkDoubleImgType>::New();
          writer->SetFileName(filename+"_VOLUME"+boost::lexical_cast<std::string>(signalModel->m_CompartmentId)+".nrrd");
          writer->SetInput(signalModel->GetVolumeFractionImage());
          writer->Update();
          MITK_INFO << "Volume fraction image for compartment "+boost::lexical_cast<std::string>(signalModel->m_CompartmentId)+" saved.";
        }
        catch(...)
        {
        }
      }
    }
  }

  boost::property_tree::xml_writer_settings<std::string> writerSettings(' ', 2);
  boost::property_tree::xml_parser::write_xml(filename, parameters, std::locale(), writerSettings);

  try{
    itk::ImageFileWriter<ItkDoubleImgType>::Pointer writer = itk::ImageFileWriter<ItkDoubleImgType>::New();
    writer->SetFileName(filename+"_FMAP.nrrd");
    writer->SetInput(m_SignalGen.m_FrequencyMap);
    writer->Update();
  }
  catch(...)
  {
    MITK_INFO << "No frequency map saved.";
  }
  try{
    itk::ImageFileWriter<ItkUcharImgType>::Pointer writer = itk::ImageFileWriter<ItkUcharImgType>::New();
    writer->SetFileName(filename+"_MASK.nrrd");
    writer->SetInput(m_SignalGen.m_MaskImage);
    writer->Update();
  }
  catch(...)
  {
    MITK_INFO << "No mask image saved.";
  }

  setlocale(LC_ALL, currLocale.c_str());
}

template< class ScalarType >
template< class ParameterType >
ParameterType mitk::FiberfoxParameters< ScalarType >::ReadVal(boost::property_tree::ptree::value_type const& v, std::string tag, ParameterType defaultValue, bool essential)
{
  try
  {
    return v.second.get<ParameterType>(tag);
  }
  catch (...)
  {
    if (essential)
    {
      mitkThrow() << "Parameter file corrupted. Essential tag is missing: '" << tag << "'";
    }
    if (tag!="artifacts.noisetype")
    {
      MITK_INFO << "Tag '" << tag << "' not found. Using default value '" << defaultValue << "'.";
      m_MissingTags += "\n- ";
      m_MissingTags += tag;
    }
    return defaultValue;
  }
}

template< class ScalarType >
void mitk::FiberfoxParameters< ScalarType >::LoadParameters(std::string filename)
{
  m_MissingTags = "";
  if(filename.empty()) { return; }

  const std::string& locale = "C";
  const std::string& currLocale = setlocale( LC_ALL, nullptr );

  if ( locale.compare(currLocale)!=0 )
  {
    try
    {
      setlocale(LC_ALL, locale.c_str());
    }
    catch(...)
    {
      MITK_INFO << "Could not set locale " << locale;
    }
  }

  boost::property_tree::ptree parameterTree;
  boost::property_tree::xml_parser::read_xml( filename, parameterTree );

  m_FiberModelList.clear();
  m_NonFiberModelList.clear();
  if (m_NoiseModel) { m_NoiseModel = nullptr; }

  BOOST_FOREACH( boost::property_tree::ptree::value_type const& v1, parameterTree.get_child("fiberfox") )
  {
    if( v1.first == "fibers" )
    {
      m_Misc.m_CheckRealTimeFibersBox = ReadVal<bool>(v1,"realtime", m_Misc.m_CheckRealTimeFibersBox);
      m_Misc.m_CheckAdvancedFiberOptionsBox = ReadVal<bool>(v1,"showadvanced", m_Misc.m_CheckAdvancedFiberOptionsBox);
      m_Misc.m_CheckConstantRadiusBox = ReadVal<bool>(v1,"constantradius", m_Misc.m_CheckConstantRadiusBox);
      m_Misc.m_CheckIncludeFiducialsBox = ReadVal<bool>(v1,"includeFiducials", m_Misc.m_CheckIncludeFiducialsBox);

      switch (ReadVal<unsigned int>(v1,"distribution", 0))
      {
        case 0:
          m_FiberGen.m_Distribution = FiberGenerationParameters::DISTRIBUTE_UNIFORM;
        break;
        case 1:
          m_FiberGen.m_Distribution = FiberGenerationParameters::DISTRIBUTE_GAUSSIAN;
        break;
        default:
          m_FiberGen.m_Distribution = FiberGenerationParameters::DISTRIBUTE_UNIFORM;
      }
      m_FiberGen.m_Variance = ReadVal<double>(v1,"variance", m_FiberGen.m_Variance);
      m_FiberGen.m_Density = ReadVal<unsigned int>(v1,"density", m_FiberGen.m_Density);
      m_FiberGen.m_Sampling = ReadVal<double>(v1,"spline.sampling", m_FiberGen.m_Sampling);
      m_FiberGen.m_Tension = ReadVal<double>(v1,"spline.tension", m_FiberGen.m_Tension);
      m_FiberGen.m_Continuity = ReadVal<double>(v1,"spline.continuity", m_FiberGen.m_Continuity);
      m_FiberGen.m_Bias = ReadVal<double>(v1,"spline.bias", m_FiberGen.m_Bias);
      m_FiberGen.m_Rotation[0] = ReadVal<double>(v1,"rotation.x", m_FiberGen.m_Rotation[0]);
      m_FiberGen.m_Rotation[1] = ReadVal<double>(v1,"rotation.y", m_FiberGen.m_Rotation[1]);
      m_FiberGen.m_Rotation[2] = ReadVal<double>(v1,"rotation.z", m_FiberGen.m_Rotation[2]);
      m_FiberGen.m_Translation[0] = ReadVal<double>(v1,"translation.x", m_FiberGen.m_Translation[0]);
      m_FiberGen.m_Translation[1] = ReadVal<double>(v1,"translation.y", m_FiberGen.m_Translation[1]);
      m_FiberGen.m_Translation[2] = ReadVal<double>(v1,"translation.z", m_FiberGen.m_Translation[2]);
      m_FiberGen.m_Scale[0] = ReadVal<double>(v1,"scale.x", m_FiberGen.m_Scale[0]);
      m_FiberGen.m_Scale[1] = ReadVal<double>(v1,"scale.y", m_FiberGen.m_Scale[1]);
      m_FiberGen.m_Scale[2] = ReadVal<double>(v1,"scale.z", m_FiberGen.m_Scale[2]);
    }
    else if ( v1.first == "image" )
    {
      m_Misc.m_SignalModelString = ReadVal<std::string>(v1,"signalmodelstring", m_Misc.m_SignalModelString);
      m_Misc.m_ArtifactModelString = ReadVal<std::string>(v1,"artifactmodelstring", m_Misc.m_ArtifactModelString);
      m_Misc.m_OutputPath = ReadVal<std::string>(v1,"outpath", m_Misc.m_OutputPath);
      m_Misc.m_CheckOutputVolumeFractionsBox = ReadVal<bool>(v1,"outputvolumefractions", m_Misc.m_CheckOutputVolumeFractionsBox);
      m_Misc.m_CheckAdvancedSignalOptionsBox = ReadVal<bool>(v1,"showadvanced", m_Misc.m_CheckAdvancedSignalOptionsBox);
      m_Misc.m_CheckAddDistortionsBox = ReadVal<bool>(v1,"artifacts.doAddDistortions", m_Misc.m_CheckAddDistortionsBox);
      m_Misc.m_CheckAddNoiseBox = ReadVal<bool>(v1,"artifacts.addnoise", m_Misc.m_CheckAddNoiseBox);
      m_Misc.m_CheckAddGhostsBox = ReadVal<bool>(v1,"artifacts.addghosts", m_Misc.m_CheckAddGhostsBox);
      m_Misc.m_CheckAddAliasingBox = ReadVal<bool>(v1,"artifacts.addaliasing", m_Misc.m_CheckAddAliasingBox);
      m_Misc.m_CheckAddSpikesBox = ReadVal<bool>(v1,"artifacts.addspikes", m_Misc.m_CheckAddSpikesBox);
      m_Misc.m_CheckAddEddyCurrentsBox = ReadVal<bool>(v1,"artifacts.addeddycurrents", m_Misc.m_CheckAddEddyCurrentsBox);

      m_SignalGen.m_ImageRegion.SetSize(0, ReadVal<int>(v1,"basic.size.x",m_SignalGen.m_ImageRegion.GetSize(0)));
      m_SignalGen.m_ImageRegion.SetSize(1, ReadVal<int>(v1,"basic.size.y",m_SignalGen.m_ImageRegion.GetSize(1)));
      m_SignalGen.m_ImageRegion.SetSize(2, ReadVal<int>(v1,"basic.size.z",m_SignalGen.m_ImageRegion.GetSize(2)));
      m_SignalGen.m_ImageSpacing[0] = ReadVal<double>(v1,"basic.spacing.x",m_SignalGen.m_ImageSpacing[0]);
      m_SignalGen.m_ImageSpacing[1] = ReadVal<double>(v1,"basic.spacing.y",m_SignalGen.m_ImageSpacing[1]);
      m_SignalGen.m_ImageSpacing[2] = ReadVal<double>(v1,"basic.spacing.z",m_SignalGen.m_ImageSpacing[2]);
      m_SignalGen.m_ImageOrigin[0] = ReadVal<double>(v1,"basic.origin.x",m_SignalGen.m_ImageOrigin[0]);
      m_SignalGen.m_ImageOrigin[1] = ReadVal<double>(v1,"basic.origin.y",m_SignalGen.m_ImageOrigin[1]);
      m_SignalGen.m_ImageOrigin[2] = ReadVal<double>(v1,"basic.origin.z",m_SignalGen.m_ImageOrigin[2]);
      m_SignalGen.m_ImageDirection[0][0] = ReadVal<double>(v1,"basic.direction.1",m_SignalGen.m_ImageDirection[0][0]);
      m_SignalGen.m_ImageDirection[0][1] = ReadVal<double>(v1,"basic.direction.2",m_SignalGen.m_ImageDirection[0][1]);
      m_SignalGen.m_ImageDirection[0][2] = ReadVal<double>(v1,"basic.direction.3",m_SignalGen.m_ImageDirection[0][2]);
      m_SignalGen.m_ImageDirection[1][0] = ReadVal<double>(v1,"basic.direction.4",m_SignalGen.m_ImageDirection[1][0]);
      m_SignalGen.m_ImageDirection[1][1] = ReadVal<double>(v1,"basic.direction.5",m_SignalGen.m_ImageDirection[1][1]);
      m_SignalGen.m_ImageDirection[1][2] = ReadVal<double>(v1,"basic.direction.6",m_SignalGen.m_ImageDirection[1][2]);
      m_SignalGen.m_ImageDirection[2][0] = ReadVal<double>(v1,"basic.direction.7",m_SignalGen.m_ImageDirection[2][0]);
      m_SignalGen.m_ImageDirection[2][1] = ReadVal<double>(v1,"basic.direction.8",m_SignalGen.m_ImageDirection[2][1]);
      m_SignalGen.m_ImageDirection[2][2] = ReadVal<double>(v1,"basic.direction.9",m_SignalGen.m_ImageDirection[2][2]);

      m_SignalGen.m_AcquisitionType = (SignalGenerationParameters::AcquisitionType)
                                      ReadVal<int>(v1,"acquisitiontype", m_SignalGen.m_AcquisitionType);
      m_SignalGen.m_CoilSensitivityProfile = (SignalGenerationParameters::CoilSensitivityProfile)
                                             ReadVal<int>(v1,"coilsensitivityprofile", m_SignalGen.m_CoilSensitivityProfile);
      m_SignalGen.m_NumberOfCoils = ReadVal<int>(v1,"numberofcoils", m_SignalGen.m_NumberOfCoils);
      m_SignalGen.m_ReversePhase = ReadVal<bool>(v1,"reversephase", m_SignalGen.m_ReversePhase);
      m_SignalGen.m_PartialFourier = ReadVal<double>(v1,"partialfourier", m_SignalGen.m_PartialFourier);
      m_SignalGen.m_NoiseVariance = ReadVal<double>(v1,"noisevariance", m_SignalGen.m_NoiseVariance);
      m_SignalGen.m_tRep = ReadVal<double>(v1,"trep", m_SignalGen.m_tRep);
      m_SignalGen.m_SignalScale = ReadVal<double>(v1,"signalScale", m_SignalGen.m_SignalScale);
      m_SignalGen.m_tEcho = ReadVal<double>(v1,"tEcho", m_SignalGen.m_tEcho);
      m_SignalGen.m_tLine = ReadVal<double>(v1,"tLine", m_SignalGen.m_tLine);
      m_SignalGen.m_tInhom = ReadVal<double>(v1,"tInhom", m_SignalGen.m_tInhom);
      m_SignalGen.m_Bvalue = ReadVal<double>(v1,"bvalue", m_SignalGen.m_Bvalue);
      m_SignalGen.m_SimulateKspaceAcquisition = ReadVal<bool>(v1,"simulatekspace", m_SignalGen.m_SimulateKspaceAcquisition);

      m_SignalGen.m_AxonRadius = ReadVal<double>(v1,"axonRadius", m_SignalGen.m_AxonRadius);
      m_SignalGen.m_Spikes = ReadVal<unsigned int>(v1,"artifacts.spikesnum", m_SignalGen.m_Spikes);
      m_SignalGen.m_SpikeAmplitude = ReadVal<double>(v1,"artifacts.spikesscale", m_SignalGen.m_SpikeAmplitude);
      m_SignalGen.m_KspaceLineOffset = ReadVal<double>(v1,"artifacts.kspaceLineOffset", m_SignalGen.m_KspaceLineOffset);
      m_SignalGen.m_EddyStrength = ReadVal<double>(v1,"artifacts.eddyStrength", m_SignalGen.m_EddyStrength);
      m_SignalGen.m_Tau = ReadVal<double>(v1,"artifacts.eddyTau", m_SignalGen.m_Tau);
      m_SignalGen.m_CroppingFactor = ReadVal<double>(v1,"artifacts.aliasingfactor", m_SignalGen.m_CroppingFactor);
      m_SignalGen.m_DoAddGibbsRinging = ReadVal<bool>(v1,"artifacts.addringing", m_SignalGen.m_DoAddGibbsRinging);
      m_SignalGen.m_DoSimulateRelaxation = ReadVal<bool>(v1,"doSimulateRelaxation", m_SignalGen.m_DoSimulateRelaxation);
      m_SignalGen.m_DoDisablePartialVolume = ReadVal<bool>(v1,"doDisablePartialVolume", m_SignalGen.m_DoDisablePartialVolume);
      m_SignalGen.m_DoAddMotion = ReadVal<bool>(v1,"artifacts.doAddMotion", m_SignalGen.m_DoAddMotion);
      m_SignalGen.m_DoRandomizeMotion = ReadVal<bool>(v1,"artifacts.randomMotion", m_SignalGen.m_DoRandomizeMotion);
      m_SignalGen.m_Translation[0] = ReadVal<double>(v1,"artifacts.translation0", m_SignalGen.m_Translation[0]);
      m_SignalGen.m_Translation[1] = ReadVal<double>(v1,"artifacts.translation1", m_SignalGen.m_Translation[1]);
      m_SignalGen.m_Translation[2] = ReadVal<double>(v1,"artifacts.translation2", m_SignalGen.m_Translation[2]);
      m_SignalGen.m_Rotation[0] = ReadVal<double>(v1,"artifacts.rotation0", m_SignalGen.m_Rotation[0]);
      m_SignalGen.m_Rotation[1] = ReadVal<double>(v1,"artifacts.rotation1", m_SignalGen.m_Rotation[1]);
      m_SignalGen.m_Rotation[2] = ReadVal<double>(v1,"artifacts.rotation2", m_SignalGen.m_Rotation[2]);

      // m_SignalGen.SetNumWeightedVolumes(ReadVal<unsigned int>(v1,"numgradients", m_SignalGen.GetNumWeightedVolumes()));
      SignalGenerationParameters::GradientListType gradients;
      BOOST_FOREACH( boost::property_tree::ptree::value_type const& v2, v1.second.get_child("gradients") )
      {
        SignalGenerationParameters::GradientType g;
        g[0] = ReadVal<double>(v2,"x",0);
        g[1] = ReadVal<double>(v2,"y",0);
        g[2] = ReadVal<double>(v2,"z",0);
        gradients.push_back(g);
      }
      m_SignalGen.SetGradienDirections(gradients);

      m_Misc.m_MotionVolumesBox = ReadVal<std::string>(v1,"artifacts.motionvolumes", m_Misc.m_MotionVolumesBox);
      m_SignalGen.m_MotionVolumes.clear();

      if ( m_Misc.m_MotionVolumesBox == "random" )
      {
        for ( size_t i=0; i < m_SignalGen.GetNumVolumes(); ++i )
        {
          m_SignalGen.m_MotionVolumes.push_back( bool( rand()%2 ) );
        }
        MITK_DEBUG << "mitkFiberfoxParameters.cpp: Case m_Misc.m_MotionVolumesBox == \"random\".";
      }
      else if ( ! m_Misc.m_MotionVolumesBox.empty() )
      {
        std::stringstream stream( m_Misc.m_MotionVolumesBox );
        std::vector<int> numbers;
        int nummer = std::numeric_limits<int>::max();
        while( stream >> nummer )
        {
          if( nummer < std::numeric_limits<int>::max() )
            {
              numbers.push_back( nummer );
            }
        }
        // If a list of negative numbers is given:
        if( *(std::min_element( numbers.begin(), numbers.end() )) < 0
            && *(std::max_element( numbers.begin(), numbers.end() )) <= 0 ) // cave: -0 == +0
        {
          for ( size_t i=0; i<m_SignalGen.GetNumVolumes(); ++i )
          {
            m_SignalGen.m_MotionVolumes.push_back( true );
          }
          // set all true except those given.
          for (auto number : numbers)
          {
            if (-number < static_cast<int>(m_SignalGen.GetNumVolumes()) && -number >= 0 )
              m_SignalGen.m_MotionVolumes.at(-number) = false;
          }
          MITK_DEBUG << "mitkFiberfoxParameters.cpp: Case list of negative numbers.";
        }
        // If a list of positive numbers is given:
        else if( *(std::min_element( numbers.begin(), numbers.end() )) >= 0
                 && *(std::max_element( numbers.begin(), numbers.end() )) >= 0 )
        {
          for ( size_t i=0; i<m_SignalGen.GetNumVolumes(); ++i )
          {
            m_SignalGen.m_MotionVolumes.push_back( false );
          }
          // set all false except those given.
          for (auto number : numbers)
          {
            if (number < static_cast<int>(m_SignalGen.GetNumVolumes()) && number >= 0)
              m_SignalGen.m_MotionVolumes.at(number) = true;
          }
          MITK_DEBUG << "mitkFiberfoxParameters.cpp: Case list of positive numbers.";
        }
        else
        {
          MITK_WARN << "mitkFiberfoxParameters.cpp: Inconsistent list of numbers in m_MotionVolumesBox.";
          break;
        }
      }
      else
      {
        MITK_WARN << "mitkFiberfoxParameters.cpp: Cannot make sense of string in m_MotionVolumesBox.";
        break;
      }

      try
      {
        if (ReadVal<std::string>(v1,"artifacts.noisetype","")=="rice")
        {
          m_NoiseModel = std::make_shared< mitk::RicianNoiseModel<ScalarType> >();
          m_NoiseModel->SetNoiseVariance(ReadVal<double>(v1,"artifacts.noisevariance",m_NoiseModel->GetNoiseVariance()));
        }
      }
      catch(...)
      {
        MITK_DEBUG << "mitkFiberfoxParameters.cpp: caught some error while trying m_NoiseModel->SetNoiseVariance()";
        // throw;
      }

      try
      {
        if (ReadVal<std::string>(v1,"artifacts.noisetype","")=="chisquare")
        {
          m_NoiseModel = std::make_shared< mitk::ChiSquareNoiseModel<ScalarType> >();
          m_NoiseModel->SetNoiseVariance(ReadVal<double>(v1,"artifacts.noisevariance",m_NoiseModel->GetNoiseVariance()));
        }
      }
      catch(...)
      {
        MITK_DEBUG << "mitkFiberfoxParameters.cpp: caught some error while trying m_NoiseModel->SetNoiseVariance()";
        // throw;
      }

      BOOST_FOREACH( boost::property_tree::ptree::value_type const& v2, v1.second.get_child("compartments") )
      {
        mitk::DiffusionSignalModel<ScalarType>* signalModel = nullptr;

        std::string model = ReadVal<std::string>(v2,"model","",true);
        if (model=="stick")
        {
          mitk::StickModel<ScalarType>* model = new mitk::StickModel<ScalarType>();
          model->SetDiffusivity(ReadVal<double>(v2,"d",model->GetDiffusivity()));
          model->SetT2(ReadVal<double>(v2,"t2",model->GetT2()));
          model->SetT1(ReadVal<double>(v2,"t1",model->GetT1()));
          model->SetBvalue(m_SignalGen.m_Bvalue);
          model->m_CompartmentId = ReadVal<unsigned int>(v2,"ID",0,true);
          if (ReadVal<std::string>(v2,"type","",true)=="fiber")
            m_FiberModelList.push_back(model);
          else if (ReadVal<std::string>(v2,"type","",true)=="non-fiber")
            m_NonFiberModelList.push_back(model);
          signalModel = model;
        }
        else if (model=="tensor")
        {
          mitk::TensorModel<ScalarType>* model = new mitk::TensorModel<ScalarType>();
          model->SetDiffusivity1(ReadVal<double>(v2,"d1",model->GetDiffusivity1()));
          model->SetDiffusivity2(ReadVal<double>(v2,"d2",model->GetDiffusivity2()));
          model->SetDiffusivity3(ReadVal<double>(v2,"d3",model->GetDiffusivity3()));
          model->SetT2(ReadVal<double>(v2,"t2",model->GetT2()));
          model->SetT1(ReadVal<double>(v2,"t1",model->GetT1()));
          model->SetBvalue(m_SignalGen.m_Bvalue);
          model->m_CompartmentId = ReadVal<unsigned int>(v2,"ID",0,true);
          if (ReadVal<std::string>(v2,"type","",true)=="fiber")
            m_FiberModelList.push_back(model);
          else if (ReadVal<std::string>(v2,"type","",true)=="non-fiber")
            m_NonFiberModelList.push_back(model);
          signalModel = model;
        }
        else if (model=="ball")
        {
          mitk::BallModel<ScalarType>* model = new mitk::BallModel<ScalarType>();
          model->SetDiffusivity(ReadVal<double>(v2,"d",model->GetDiffusivity()));
          model->SetT2(ReadVal<double>(v2,"t2",model->GetT2()));
          model->SetT1(ReadVal<double>(v2,"t1",model->GetT1()));
          model->SetBvalue(m_SignalGen.m_Bvalue);
          model->m_CompartmentId = ReadVal<unsigned int>(v2,"ID",0,true);
          if (ReadVal<std::string>(v2,"type","",true)=="fiber")
            m_FiberModelList.push_back(model);
          else if (ReadVal<std::string>(v2,"type","",true)=="non-fiber")
            m_NonFiberModelList.push_back(model);
          signalModel = model;
        }
        else if (model=="astrosticks")
        {
          mitk::AstroStickModel<ScalarType>* model = new AstroStickModel<ScalarType>();
          model->SetDiffusivity(ReadVal<double>(v2,"d",model->GetDiffusivity()));
          model->SetT2(ReadVal<double>(v2,"t2",model->GetT2()));
          model->SetT1(ReadVal<double>(v2,"t1",model->GetT1()));
          model->SetBvalue(m_SignalGen.m_Bvalue);
          model->SetRandomizeSticks(ReadVal<bool>(v2,"randomize",model->GetRandomizeSticks()));
          model->m_CompartmentId = ReadVal<unsigned int>(v2,"ID",0,true);
          if (ReadVal<std::string>(v2,"type","",true)=="fiber")
            m_FiberModelList.push_back(model);
          else if (ReadVal<std::string>(v2,"type","",true)=="non-fiber")
            m_NonFiberModelList.push_back(model);
          signalModel = model;
        }
        else if (model=="dot")
        {
          mitk::DotModel<ScalarType>* model = new mitk::DotModel<ScalarType>();
          model->SetT2(ReadVal<double>(v2,"t2",model->GetT2()));
          model->SetT1(ReadVal<double>(v2,"t1",model->GetT1()));
          model->m_CompartmentId = ReadVal<unsigned int>(v2,"ID",0,true);
          if (ReadVal<std::string>(v2,"type","",true)=="fiber")
            m_FiberModelList.push_back(model);
          else if (ReadVal<std::string>(v2,"type","",true)=="non-fiber")
            m_NonFiberModelList.push_back(model);
          signalModel = model;
        }
        else if (model=="prototype")
        {
          mitk::RawShModel<ScalarType>* model = new mitk::RawShModel<ScalarType>();
          model->SetMaxNumKernels(ReadVal<unsigned int>(v2,"maxNumSamples",model->GetMaxNumKernels()));
          model->SetFaRange(ReadVal<double>(v2,"minFA",model->GetFaRange().first), ReadVal<double>(v2,"maxFA",model->GetFaRange().second));
          model->SetAdcRange(ReadVal<double>(v2,"minADC",model->GetAdcRange().first), ReadVal<double>(v2,"maxADC",model->GetAdcRange().second));
          model->m_CompartmentId = ReadVal<unsigned int>(v2,"ID",0,true);
          unsigned int numCoeffs = ReadVal<unsigned int>(v2,"numCoeffs",0,true);
          unsigned int numSamples = ReadVal<unsigned int>(v2,"numSamples",0,true);

          for (unsigned int j=0; j<numSamples; j++)
          {
            vnl_vector< double > coeffs(numCoeffs);
            for (unsigned int k=0; k<numCoeffs; k++)
            {
              coeffs[k] = ReadVal<double>(v2,"kernels."+boost::lexical_cast<std::string>(j)+".coeffs."+boost::lexical_cast<std::string>(k),0,true);
            }
            model->SetShCoefficients( coeffs, ReadVal<double>(v2,"kernels."+boost::lexical_cast<std::string>(j)+".B0",0,true) );
          }

          if (ReadVal<std::string>(v2,"type","",true)=="fiber")
          { m_FiberModelList.push_back(model); }
          else if (ReadVal<std::string>(v2,"type","",true)=="non-fiber")
          { m_NonFiberModelList.push_back(model); }
          // else ?
          signalModel = model;
        }

        if (signalModel!=nullptr)
        {
          signalModel->SetGradientList(gradients);
          try
          {
            itk::ImageFileReader<ItkDoubleImgType>::Pointer reader = itk::ImageFileReader<ItkDoubleImgType>::New();
            reader->SetFileName(filename+"_VOLUME"+ReadVal<std::string>(v2,"ID","")+".nrrd");
            reader->Update();
            signalModel->SetVolumeFractionImage(reader->GetOutput());
            MITK_INFO << "Volume fraction image loaded for compartment " << signalModel->m_CompartmentId;
          }
          catch(...)
          {
            MITK_INFO << "No volume fraction image found for compartment " << signalModel->m_CompartmentId;
          }
        }
      }
    }
    else
    {

    }
  }

  try
  {
    itk::ImageFileReader<ItkDoubleImgType>::Pointer reader = itk::ImageFileReader<ItkDoubleImgType>::New();
    reader->SetFileName(filename+"_FMAP.nrrd");
    reader->Update();
    m_SignalGen.m_FrequencyMap = reader->GetOutput();
    MITK_INFO << "Frequency map loaded.";
  }
  catch(...)
  {
    MITK_INFO << "No frequency map found.";
  }

  try
  {
    itk::ImageFileReader<ItkUcharImgType>::Pointer reader = itk::ImageFileReader<ItkUcharImgType>::New();
    reader->SetFileName(filename+"_MASK.nrrd");
    reader->Update();
    m_SignalGen.m_MaskImage = reader->GetOutput();
    MITK_INFO << "Mask image loaded.";
  }
  catch(...)
  {
    MITK_INFO << "No mask image found.";
  }

  setlocale(LC_ALL, currLocale.c_str());
}

template< class ScalarType >
void mitk::FiberfoxParameters< ScalarType >::PrintSelf()
{
  MITK_INFO << "Not implemented :(";
}


//
// Created by markus on 23.04.21.
//

#include <random>
#include <boost/math/distributions/exponential.hpp>
#include "ParticleFilter.h"
#include "../Simulator/world/world.h"
#include "../Simulator/interactive_objects/DistanceSensor.h"
#include "helpers.h"
#include "DummyRobot.h"

ParticleFilter::ParticleFilter(RobotControlInterface *robot, std::string map_filename, int initialN, int targetN) {
    this->particles_world = new World(map_filename, "Particle filter");
    this->particles_world->addMapObject(this->mapLine);
    this->robot = robot;

    this->N = initialN;
    this->TARGET_N = targetN;

    // create initial particles distribution and show the simulation map
    this->particles = ParticleFilter::create_uniform_particles(this->particles_world->get_map_bounds().x,
                                                               this->particles_world->get_map_bounds().y,
                                                               this->N);
    this->estimatedRobot = new MapRobot(robot->get_radius());
    this->estimatedRobot->setColor(CV_RGB(255, 255, 255));
    this->particles_world->addMapObject(this->estimatedRobot);
    ParticleFilter::updateParticleSimulation(this->particles, SHOW_WHATS_GOING_ON);
}

std::vector<std::array<double, 3>> ParticleFilter::create_uniform_particles(double x_range, double y_range, int N) {
    std::vector<std::array<double, 3>> particles;
    std::default_random_engine generator;
    generator.seed(std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count());

    std::uniform_real_distribution<double> x_distribution(0, x_range);
    std::uniform_real_distribution<double> y_distribution(0, y_range);
    std::uniform_real_distribution<double> angle_distribution(-M_PI, M_PI);

    for (int i = 0; i < N; i++) {
        particles.push_back(std::array<double, 3>({x_distribution(generator),
                                                   y_distribution(generator),
                                                   angle_distribution(generator)}));
    }
    return particles;
}

void ParticleFilter::updateParticleSimulation(std::vector<std::array<double, 3>> particles, bool showMap, std::vector<double> weights) {
    // get sensor config to recreate (clone) the same sensors as on the robot
    std::vector<std::array<double, 2>> sensor_config;
    for (DistanceSensor *sensor : DistanceSensor::filter_for_distance_sensor(this->robot->get_sensors())) {
        sensor_config.push_back(std::array<double, 2>({sensor->get_sensor_angle(), sensor->get_sensor_max_distance()}));
    }

    // If weights are provided they will be visualized on the map. For the color scaling we have to know what the maximum probability of one particle is
    double maxWeight = 0;
    bool useCustomWeightColors = false;
    if (!weights.empty()) {
        useCustomWeightColors = true;
        for (double weight : weights) {
            maxWeight = maxWeight > weight ? maxWeight : weight;
        }
    }

    // add or delete robots from the world if the amount does not match
    if (this->particles_world->getRobotsList().size() > particles.size()) {
        // there are too many particles -> remove some
        this->particles_world->deleteRobotByIndex(particles.size() - 1, this->particles_world->getRobotsList().size() - particles.size() + 1, true);
    } else if (this->particles_world->getRobotsList().size() < particles.size()) {
        // there arent enough particles -> add some
        for (long i = this->particles_world->getRobotsList().size(); i<particles.size(); ++i) {
            // create robot
            auto* dummyRobot = new DummyRobot("particle", this->robot->get_radius(), this->particles_world);
            // set default visualization options
            dummyRobot->setDrawOptions(CV_RGB(255, 0, 0), true, 1);
            // add robot to world
            this->particles_world->addRobot(dummyRobot);

            // create sensors
            for (std::array<double, 2> s_config : sensor_config) {
                DistanceSensor *sensor = new DistanceSensor(this->particles_world, dummyRobot, s_config[0], s_config[1], true);
                dummyRobot->add_sensor(sensor);
            }
        }
    }

    // update robots
    std::vector<Robot*> dummyRobotsList = this->particles_world->getRobotsList();
    for (int i = 0; i < particles.size(); ++i) {
        std::array<double, 3> particle = particles[i];
        DummyRobot* dummyRobot = (DummyRobot*) dummyRobotsList[i];
        dummyRobot->reposition(particle[0], particle[1], particle[2]);

        // set custom visualization options
        if (useCustomWeightColors) {
            dummyRobot->setDrawOptions(getColor(weights[i] / maxWeight), true, 1);
        }

        for (SensorInterface* sensor : dummyRobot->get_sensors()) {
            sensor->update_sensor_data(true);
        }
    }

    // show map if enabled
    if (showMap) {
        this->particles_world->show_map(true);
    }
}

std::vector<std::array<double, 3>> ParticleFilter::particles_predict(std::vector<std::array<double, 3>> *oldParticles,
                                                                     double move_distance,
                                                                     double move_angle,
                                                                     double standard_deviation) {
    std::vector<std::array<double, 3>> updated_particles;

    // init normal_distribution
    std::random_device rd{};
    std::mt19937 gen{rd()};
    std::normal_distribution<> d{0, standard_deviation};

    // update oldParticles
    for (std::array<double, 3> particle : *oldParticles) {
        // update moved distance and angle with a norm distribution value (x * ((100 + <norm_dist>) / 100))
        double distance_with_distribution = move_distance * ((100 + d(gen)) / 100);
        double angle_with_distribution = move_angle * ((100 + d(gen)) / 100);

        double newRobotAngle = angle_with_distribution + particle[2];

        // update oldParticles with distance and angle
        updated_particles.push_back(std::array<double, 3>(
                {
                        particle[0] + cos(newRobotAngle) * distance_with_distribution,
                        particle[1] + sin(newRobotAngle) * distance_with_distribution,
                        newRobotAngle
                }));
    }

    return updated_particles;
}

std::vector<double> ParticleFilter::particles_update(std::vector<double> robotSensorValues, std::vector<Robot *> simRobots, double lambda) {
    std::vector<double> updated_weights = std::vector<double>(simRobots.size(), 1.0);
    auto d = boost::math::exponential_distribution<>{lambda};

    double sumOfWeights = 0;

    for (int i = 0; i < updated_weights.size(); ++i) {
        // collect sensor values of simulated particles
        std::vector<DistanceSensor *> sensors = DistanceSensor::filter_for_distance_sensor(simRobots.at(i)->get_sensors());
        int j = 0;
        for (DistanceSensor *sensor : sensors) {
            // calculate correlation
            updated_weights[i] *= boost::math::pdf(d, abs(sensor->get_simplified_sensor_value() - robotSensorValues[j]));
            updated_weights[i] += 1.e-300; // prevent rounding to 0
            j++;
        }

        sumOfWeights += updated_weights[i];
    }

    // normalize weights: sum of weights should equal 1
    for (double &updated_weight : updated_weights) {
        updated_weight /= sumOfWeights;
    }

    return updated_weights;
}

std::tuple<std::vector<std::array<double, 3>>, std::vector<double>> ParticleFilter::particles_resample(
        std::vector<std::array<double, 3>> *oldParticles,
        std::vector<double> *weights,
        int N,
        bool enableRandomParticles,
        double noise,
        cv::Point2d mapBounds) {
    // grouping algorithm for resampling is explained here: https://robotics.stackexchange.com/a/481
    // creates an index list of entries to keep
    std::vector<double> positions = {};
    for (int i = 0; i < N; ++i) {
        positions.push_back((i + ((double) rand() / RAND_MAX)) / N);
    }

    std::vector<int> indexes(N, 0);
    std::vector<double> cumulativeSum = {};
    cumulativeSum.push_back(weights->at(0));
    for (int i = 1; i < N; ++i) {
        cumulativeSum.push_back(cumulativeSum.at(i - 1) + weights->at(i));
    }

    int i = 0;
    int j = 0;
    while (i < N && j < N) {
        if (positions[i] < cumulativeSum[j]) {
            indexes[i] = j;
            i += 1;
        } else {
            j += 1;
        }
    }

    // create a new particles list. It will be based on the previous one. Based on the index list created above some values will be kept, others
    // will be deleted. There will be many duplicates, which is expected. Each value will be modified by a noise value.
    std::default_random_engine generator;
    std::uniform_real_distribution<double> uniformNoise(-noise, noise);

    std::vector<std::array<double, 3>> updatedParticles = {};
    std::vector<double> updatedWeights = {};
    double weightSum = 0;
    for (int i = 0; i < N; ++i) {
        updatedParticles.push_back(std::array<double, 3>({oldParticles->at(indexes[i])[0] + uniformNoise(generator),
                                                          oldParticles->at(indexes[i])[1] + uniformNoise(generator),
                                                          oldParticles->at(indexes[i])[2] + uniformNoise(generator) / 10
                                                         }));
        updatedWeights.push_back(weights->at(indexes[i]));
        weightSum += weights->at(indexes[i]);
    }

    // normalize weights
    for (int i = 0; i < N; ++i) {
        weights->at(indexes[i]) /= weightSum;
    }

    return std::tuple<std::vector<std::array<double, 3>>, std::vector<double>>(updatedParticles, updatedWeights);
}

ParticleEvaluationData ParticleFilter::update() {
    return this->update(this->robot->get_last_tick_movement_distance(), this->robot->get_last_tick_movement_angle());
}

ParticleEvaluationData ParticleFilter::update(double distance, double angle) {
    // check if in location finding phase
    int removeElementsCount = N * INITIAL_PHASE_REPLACE_SHARE;;
    if (!this->initialLocationFinished && this->iterationsCounter > 0) {
        // check if location is found
        if (this->locationCertaintyEstimation > CERTAINTY_ESTIMATION_THRESHOLD) {
            // check uncertainty metric if location seems to be already successful
            std::cout << "Found location" << std::endl;
            this->initialLocationFinished = true;
            this->N = TARGET_N;
            this->useRandomParticles = false;
        }

        // if location not yet found: select the worst rated particles and replace them with random
        if (!this->initialLocationFinished) {
            // bubble sort
            // could be replaced with a more efficient algorithm if its computationally too expensive
            bool finished = false;
            for (int i = 0; !finished && i < this->particles.size(); ++i) {
                finished = true;
                for (int j = i + 1; j < this->particles.size(); ++j) {
                    if (this->weights[i] > this->weights[j]) {
                        finished = false;
                        double tmpWeight = this->weights[i];
                        std::array<double, 3> tmpParticle = this->particles[i];
                        this->weights[i] = this->weights[j];
                        this->weights[j] = tmpWeight;
                        this->particles[i] = this->particles[j];
                        this->particles[j] = tmpParticle;
                    }
                }
            }

            // drop the first particles (lowest probability)
            this->weights.erase(this->weights.begin(), this->weights.begin() + removeElementsCount);
            this->particles.erase(this->particles.begin(), this->particles.begin() + removeElementsCount);

            // generate new particles to replace the removed ones
            std::vector<std::array<double, 3>> randomParticles = create_uniform_particles(this->particles_world->get_map_bounds().x,
                                                                                this->particles_world->get_map_bounds().y,
                                                                                removeElementsCount);
            this->particles.insert(this->particles.end(), randomParticles.begin(), randomParticles.end());
        }
    }
    this->iterationsCounter++;

    // Begin of actual particle filter steps

    // Particle Filter Step 1) particle prediction: apply estimated movement distance / angle (currently in this simulator there is no noise for those values)
    std::vector<std::array<double, 3>> updated_particles = particles_predict(&this->particles,
                                                                             distance,
                                                                             angle,
                                                                             4);

    // Particle Filter Step 2) update weights: update the weight of each particle based on how much the simulated sensor values correspond to the ones of our "real" robot
    // generate vector containing the current sensor values of the "real" robot
    std::vector<double> robotSensorValues = {};
    for (DistanceSensor *sensor : DistanceSensor::filter_for_distance_sensor(this->robot->get_sensors())) {
        robotSensorValues.push_back(sensor->get_simplified_sensor_value());
    }

    // this allows to read the sensor values of all simulated particles in particles_update()
    this->updateParticleSimulation(updated_particles, false);

    // this will do the actual weights update
    std::vector<double> updated_weights = this->particles_update(robotSensorValues, this->particles_world->getRobotsList(), 0.01);

    // Particle Filter Step 3) resample: create new particles based on the weights. They will be mostly around the most previous particles with the highest weight.
    std::tuple<std::vector<std::array<double, 3>>, std::vector<double>> resampledTuple = this->particles_resample(&updated_particles,
                                                                                &updated_weights,
                                                                                this->N,
                                                                                this->useRandomParticles,
                                                                                5,
                                                                                this->particles_world->get_map_bounds());

    // END of actual particle filter steps

    // calculate estimated position
    std::array<double, 3> estimatedParticle{};
    if (!this->initialLocationFinished) {
        // if in the initial phase: only the first x particles are relevant, the others are random
        int relevantParticlesCount = N - removeElementsCount;
        estimatedParticle = weightedAverageParticle(std::vector<std::array<double,3>>(updated_particles.begin(), updated_particles.begin()+relevantParticlesCount), std::vector<double>(updated_weights.begin(), updated_weights.begin()+relevantParticlesCount));
    } else {
        estimatedParticle = weightedAverageParticle(updated_particles, updated_weights);
    }

    // estimate whether the prediction is probably already accurate and if yes, add to history, if not: clear history
    // also visualize it on the map as a line
    if (!this->estimationHistory.empty()) {
        // TODO: replace this algorithm with a better one looking further in the past and calculate an uncertainty value
        double maxAcceptableMovementDistance = pow(this->robot->get_max_move_speed() / GAME_TPS * 2.5, 2); // theoretically maximum possible movement distance * X as square to prevent requirement of sqrt
        double distanceFromLastEstimation = pow(this->estimationHistory.back()[0] - estimatedParticle[0], 2) + pow(this->estimationHistory.back()[1] - estimatedParticle[1], 2);
        if (distanceFromLastEstimation >= maxAcceptableMovementDistance) {
            this->estimationHistory.clear();
            this->locationCertaintyEstimation = this->locationCertaintyEstimation > 0 ? this->locationCertaintyEstimation - 5 / GAME_TPS : 0;
            if (SHOW_WHATS_GOING_ON && !this->initialLocationFinished) this->mapLine->clearPoints();
            std::cout << "Estimated location jumped too far, this is an indication for a wrong location estimation" << std::endl;
        } else {
            this->locationCertaintyEstimation = this->locationCertaintyEstimation < 10 ? this->locationCertaintyEstimation + 1 / GAME_TPS : 10;
        }
    }
    if (SHOW_WHATS_GOING_ON) this->estimationHistory.push_back(estimatedParticle);
    this->mapLine->addPoint(cv::Point2i((int) estimatedParticle[0], (int) estimatedParticle[1]));

    // show current particles with their weights and the estimated robot location
    // nice for visualization but will impact performance!
    if (SHOW_WHATS_GOING_ON) {
        this->updateParticleSimulation(updated_particles, false, updated_weights);
        this->estimatedRobot->reposition(cv::Point2d(estimatedParticle[0], estimatedParticle[1]), estimatedParticle[2]);
        this->particles_world->show_map(true);
    }

    // save particles and weights
    this->particles = std::get<0>(resampledTuple);
    this->weights = std::get<1>(resampledTuple);

    return ParticleEvaluationData({cv::Point2d(estimatedParticle[0], estimatedParticle[1]), estimatedParticle[2], locationCertaintyEstimation >= CERTAINTY_ESTIMATION_THRESHOLD});
}


/**
 * \ file GainExpanderFilter.cpp
 */

#include <ATK/Dynamic/GainExpanderFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/math/constants/constants.hpp>

constexpr gsl::index PROCESSSIZE = 64;

BOOST_AUTO_TEST_CASE( GainExpanderFilter_softness_test )
{
  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter;
  filter.set_softness(0.5);
  BOOST_CHECK_EQUAL(filter.get_softness(), 0.5);
}

BOOST_AUTO_TEST_CASE( GainExpanderFilter_softness_range_test )
{
  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter;
  BOOST_CHECK_THROW(filter.set_softness(-0.000001), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( GainExpanderFilter_const_1_test )
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(1, outdata[i], 0.1);
  }
}

BOOST_AUTO_TEST_CASE( GainExpanderFilter_const_0_test )
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 0;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(0.1);
  filter.set_ratio(2);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_SMALL(outdata[i], 0.001);
  }
}

BOOST_AUTO_TEST_CASE( GainExpanderFilter_const_1_threshold_2_ratio_2_test )
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(2);
  filter.set_softness(1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(0.700553358, outdata[i], 0.1);
  }
}

BOOST_AUTO_TEST_CASE(GainExpanderFilter_const_1_threshold_2_ratio_2_test_LUT)
{
  std::array<double, PROCESSSIZE> data;
  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(2);
  filter.set_softness(1);
  filter.wait_for_LUT_completion();

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(0.700553358, outdata[i], 0.1);
  }
}

BOOST_AUTO_TEST_CASE(GainExpanderFilter_const_1_threshold_2_ratio_4_test)
{
  std::array<double, PROCESSSIZE> data;
  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(4);
  filter.set_softness(1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(0.343814075, outdata[i], 0.1);
  }
}

BOOST_AUTO_TEST_CASE(GainExpanderFilter_const_1_threshold_2_ratio_4_test_LUT)
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(4);
  filter.set_softness(1);
  filter.wait_for_LUT_completion();

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(0.343814075, outdata[i], 0.1);
  }
}

BOOST_AUTO_TEST_CASE( GainExpanderFilter_const_1_threshold_2_ratio_4_test_twice )
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(4);
  filter.set_softness(1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(0.343814075, outdata[i], 0.1);
  }

  generator.set_pointer(data.data(), PROCESSSIZE);
  output.set_pointer(outdata.data(), PROCESSSIZE);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(0.343814075, outdata[i], 0.1);
  }
}

BOOST_AUTO_TEST_CASE(GainExpanderFilter_const_1_threshold_2_ratio_4_test_twice_LUT)
{
  std::array<double, PROCESSSIZE> data;
  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainExpanderFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(2);
  filter.set_ratio(4);
  filter.set_softness(1);
  filter.wait_for_LUT_completion();

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(0.343814075, outdata[i], 0.1);
  }

  generator.set_pointer(data.data(), PROCESSSIZE);
  output.set_pointer(outdata.data(), PROCESSSIZE);

  output.process(PROCESSSIZE);

  for (gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(0.343814075, outdata[i], 0.1);
  }
}


//    Copyright 2019 Jij Inc.

//    Licensed under the Apache License, Version 2.0 (the "License");
//    you may not use this file except in compliance with the License.
//    You may obtain a copy of the License at

//        http://www.apache.org/licenses/LICENSE-2.0

//    Unless required by applicable law or agreed to in writing, software
//    distributed under the License is distributed on an "AS IS" BASIS,
//    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//    See the License for the specific language governing permissions and
//    limitations under the License.

#ifndef OPENJIJ_UTILITY_EIGEN_HPP__
#define OPENJIJ_UTILITY_EIGEN_HPP__

#include <graph/all.hpp>
#include <Eigen/Dense>
#include <Eigen/Sparse>

namespace openjij {
    namespace utility {

        /**
         * @brief get Eigen Matrix type from Graph Type
         *
         * @tparam GraphType
         * @tparam Options Eigen Options (RowMajor or ColMajor)
         */
        template<typename GraphType, int Options=Eigen::ColMajor>
            struct get_eigen_matrix_type{};

        /**
         * @brief get Eigen Matrix type from Graph Type
         *
         * @tparam GraphType
         * @tparam Options Eigen Options (RowMajor or ColMajor)
         */
        template<typename FloatType, int Options>
            struct get_eigen_matrix_type<graph::Dense<FloatType>, Options>{
                using type = Eigen::Matrix<FloatType, Eigen::Dynamic, Eigen::Dynamic, Options>;
            };

        /**
         * @brief get Eigen Matrix type from Graph Type
         *
         * @tparam GraphType
         * @tparam Options Eigen Options (RowMajor or ColMajor)
         */
        template<typename FloatType, int Options>
            struct get_eigen_matrix_type<graph::Sparse<FloatType>, Options>{
                using type = Eigen::SparseMatrix<FloatType, Options>;
            };

        /**
         * @brief generate Eigen Vector from std::vector
         *
         * @tparam FloatType
         * @tparam Options Eigen Options (RowMajor or ColMajor)
         * @param init_spin
         *
         * @return generated Eigen Vector (init_spin.size()+1 x 1)
         */
        template<typename FloatType, int Options=Eigen::ColMajor>
            inline static Eigen::Matrix<FloatType, Eigen::Dynamic, 1, Options>
            gen_vector_from_std_vector(const graph::Spins& init_spin){
                Eigen::Matrix<FloatType, Eigen::Dynamic, 1, Options> ret_vec(init_spin.size()+1);

                //initialize spin
                for(size_t i=0; i<init_spin.size(); i++){
                    ret_vec(i) = init_spin[i];
                }

                //for local field
                ret_vec[init_spin.size()] = 1;

                return ret_vec;
            }

        /**
         * @brief generate Eigen Matrix from TrotterSpins
         *
         * @tparam FloatType
         * @tparam Options Eigen Options (RowMajor or ColMajor)
         * @param trotter_spins
         *
         * @return generated Eigen Matrix (trotter_spins[0].size()+1 x trotter_spins.size())
         */
        template<typename FloatType, int Options=Eigen::ColMajor>
            inline static Eigen::Matrix<FloatType, Eigen::Dynamic, Eigen::Dynamic, Options>
            gen_matrix_from_trotter_spins(const std::vector<graph::Spins>& trotter_spins){
                Eigen::Matrix<FloatType, Eigen::Dynamic, Eigen::Dynamic, Options> ret_mat(trotter_spins[0].size()+1, trotter_spins.size());

                //initialize spin
                for(size_t j=0; j<trotter_spins.size(); j++){
                    for(size_t i=0; i<trotter_spins[j].size(); i++){
                        ret_mat(i,j) = trotter_spins[j][i];
                    }
                }

                //dummy spins
                for(size_t j=0; j<trotter_spins.size(); j++){
                    ret_mat(trotter_spins[0].size(),j) = 1;
                }

                return ret_mat;
            }

        /**
         * @brief generate Eigen Dense Matrix from Dense graph
         *
         * @tparam Options Eigen Options (RowMajor or ColMajor)
         * @tparam FloatType
         * @param graph
         *
         * @return generated Eigen Dense Matrix (graph.get_num_spins()+1 x graph.get_num_spins()+1)
         */
        template<int Options=Eigen::ColMajor, typename FloatType>
            inline static Eigen::Matrix<FloatType, Eigen::Dynamic, Eigen::Dynamic, Options>
            gen_matrix_from_graph(const graph::Dense<FloatType>& graph){
                //initialize interaction
                Eigen::Matrix<FloatType, Eigen::Dynamic, Eigen::Dynamic, Options> ret_mat(graph.get_num_spins()+1, graph.get_num_spins()+1);

                ret_mat.setZero();

                for(size_t i=0; i<graph.get_num_spins(); i++){
                    for(size_t j=i+1; j<graph.get_num_spins(); j++){
                        ret_mat(i,j) = graph.J(i,j);
                        ret_mat(j,i) = graph.J(i,j);
                    }
                }

                //for local field
                for(size_t i=0; i<graph.get_num_spins(); i++){
                    ret_mat(i,graph.get_num_spins()) = graph.h(i);
                    ret_mat(graph.get_num_spins(),i) = graph.h(i);
                }

                //for local field
                ret_mat(graph.get_num_spins(),graph.get_num_spins()) = 1;

                return ret_mat;
            }

        /**
         * @brief generate Eigen Sparse Matrix from Sparse graph
         *
         * @tparam Options Eigen Options (RowMajor or ColMajor)
         * @tparam FloatType
         * @param graph
         *
         * @return generated Eigen Sparse Matrix (graph.get_num_spins()+1 x graph.get_num_spins()+1)
         */
        template<int Options=Eigen::ColMajor, typename FloatType>
            inline static Eigen::SparseMatrix<FloatType, Options>
            gen_matrix_from_graph(const graph::Sparse<FloatType>& graph){
                //initialize interaction
                Eigen::SparseMatrix<FloatType, Options> ret_mat(graph.get_num_spins()+1, graph.get_num_spins()+1);

                ret_mat.setZero();

                //make triplet list
                using T = std::vector<Eigen::Triplet<FloatType>>;
                T t_list;

                for(size_t ind=0; ind<graph.get_num_spins(); ind++){
                    for(size_t adj_ind : graph.adj_nodes(ind)){
                        if(ind != adj_ind){
                            t_list.emplace_back(ind, adj_ind, graph.J(ind, adj_ind));
                        }
                        else{
                            t_list.emplace_back(ind, graph.get_num_spins(), graph.h(ind));
                            t_list.emplace_back(graph.get_num_spins(), ind, graph.h(ind));
                        }
                    }
                }

                t_list.emplace_back(graph.get_num_spins(), graph.get_num_spins(), 1);

                ret_mat.setFromTriplets(t_list.begin(), t_list.end());

                return ret_mat;
            }

    } // namespace utility
} // namespace openjij

#endif


#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <Eigen/Dense>
#include <fstream>
#include <iostream>
#include <pcl/search/impl/kdtree.hpp>
#include <vector>

using namespace std;
using namespace Eigen;

ros::Publisher pub_cloud;

sensor_msgs::PointCloud2 local_map_pcl;
sensor_msgs::PointCloud2 local_depth_pcl;

ros::Subscriber odom_sub;
ros::Subscriber global_map_sub, local_map_sub;

ros::Timer local_sensing_timer;

bool has_global_map(false);
bool has_local_map(false);
bool has_odom(false);

nav_msgs::Odometry _odom;

double sensing_horizon, sensing_rate, estimation_rate;
double _x_size, _y_size, _z_size;
double _gl_xl, _gl_yl, _gl_zl;
double _resolution, _inv_resolution;
int _GLX_SIZE, _GLY_SIZE, _GLZ_SIZE;

ros::Time last_odom_stamp = ros::TIME_MAX;

inline Eigen::Vector3d gridIndex2coord(const Eigen::Vector3i& index) {
  Eigen::Vector3d pt;
  pt(0) = ((double)index(0) + 0.5) * _resolution + _gl_xl;
  pt(1) = ((double)index(1) + 0.5) * _resolution + _gl_yl;
  pt(2) = ((double)index(2) + 0.5) * _resolution + _gl_zl;

  return pt;
};

inline Eigen::Vector3i coord2gridIndex(const Eigen::Vector3d& pt) {
  Eigen::Vector3i idx;
  idx(0) = std::min(std::max(int((pt(0) - _gl_xl) * _inv_resolution), 0),
                    _GLX_SIZE - 1);
  idx(1) = std::min(std::max(int((pt(1) - _gl_yl) * _inv_resolution), 0),
                    _GLY_SIZE - 1);
  idx(2) = std::min(std::max(int((pt(2) - _gl_zl) * _inv_resolution), 0),
                    _GLZ_SIZE - 1);

  return idx;
};

void rcvOdometryCallbck(const nav_msgs::Odometry& odom) {
  /*if(!has_global_map)
    return;*/
  has_odom = true;
  _odom = odom;
}

pcl::PointCloud<pcl::PointXYZ> _cloud_all_map, _local_map;
pcl::VoxelGrid<pcl::PointXYZ> _voxel_sampler;
sensor_msgs::PointCloud2 _local_map_pcd;

pcl::search::KdTree<pcl::PointXYZ> _kdtreeLocalMap;
vector<int> _pointIdxRadiusSearch;
vector<float> _pointRadiusSquaredDistance;

void rcvGlobalPointCloudCallBack(
    const sensor_msgs::PointCloud2& pointcloud_map) {
  if (has_global_map) return;

  ROS_WARN("Global Pointcloud received..");

  pcl::PointCloud<pcl::PointXYZ> cloud_input;
  pcl::fromROSMsg(pointcloud_map, cloud_input);

  _voxel_sampler.setLeafSize(0.1f, 0.1f, 0.1f);
  _voxel_sampler.setInputCloud(cloud_input.makeShared());
  _voxel_sampler.filter(_cloud_all_map);

  _kdtreeLocalMap.setInputCloud(_cloud_all_map.makeShared());

  has_global_map = true;
}

void renderSensedPoints(const ros::TimerEvent& event) {
  if (!has_global_map || !has_odom) return;

  Eigen::Quaterniond q;
  q.x() = _odom.pose.pose.orientation.x;
  q.y() = _odom.pose.pose.orientation.y;
  q.z() = _odom.pose.pose.orientation.z;
  q.w() = _odom.pose.pose.orientation.w;

  Eigen::Matrix3d rot;
  rot = q;
  Eigen::Vector3d yaw_vec = rot.col(0);

  _local_map.points.clear();
  pcl::PointXYZ searchPoint(_odom.pose.pose.position.x,
                            _odom.pose.pose.position.y,
                            _odom.pose.pose.position.z);
  _pointIdxRadiusSearch.clear();
  _pointRadiusSquaredDistance.clear();

  pcl::PointXYZ pt;
  if (_kdtreeLocalMap.radiusSearch(searchPoint, sensing_horizon,
                                   _pointIdxRadiusSearch,
                                   _pointRadiusSquaredDistance) > 0) {
    for (size_t i = 0; i < _pointIdxRadiusSearch.size(); ++i) {
      pt = _cloud_all_map.points[_pointIdxRadiusSearch[i]];

      // if ((fabs(pt.z - _odom.pose.pose.position.z) / (pt.x - _odom.pose.pose.position.x)) >
      //     tan(M_PI / 12.0))
      //   continue;
      if ((fabs(pt.z - _odom.pose.pose.position.z) / sensing_horizon) >
          tan(M_PI / 6.0))
        continue;

      Vector3d pt_vec(pt.x - _odom.pose.pose.position.x,
                      pt.y - _odom.pose.pose.position.y,
                      pt.z - _odom.pose.pose.position.z);

      if (pt_vec.normalized().dot(yaw_vec) < 0.5) continue;

      _local_map.points.push_back(pt);
    }
  } else {
    return;
  }

  _local_map.width = _local_map.points.size();
  _local_map.height = 1;
  _local_map.is_dense = true;

  pcl::toROSMsg(_local_map, _local_map_pcd);
  _local_map_pcd.header.frame_id = "map";

  pub_cloud.publish(_local_map_pcd);
}

void rcvLocalPointCloudCallBack(
    const sensor_msgs::PointCloud2& pointcloud_map) {
  // do nothing, fix later
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "pcl_render");
  ros::NodeHandle nh("~");

  nh.getParam("sensing_horizon", sensing_horizon);
  nh.getParam("sensing_rate", sensing_rate);
  nh.getParam("estimation_rate", estimation_rate);

  nh.getParam("map/x_size", _x_size);
  nh.getParam("map/y_size", _y_size);
  nh.getParam("map/z_size", _z_size);

  // subscribe point cloud
  global_map_sub = nh.subscribe("global_map", 1, rcvGlobalPointCloudCallBack);
  local_map_sub = nh.subscribe("local_map", 1, rcvLocalPointCloudCallBack);
  odom_sub = nh.subscribe("odometry", 50, rcvOdometryCallbck);

  // publisher depth image and color image
  pub_cloud =
      nh.advertise<sensor_msgs::PointCloud2>("/pcl_render_node/cloud", 10);

  double sensing_duration = 1.0 / sensing_rate * 2.5;

  local_sensing_timer =
      nh.createTimer(ros::Duration(sensing_duration), renderSensedPoints);

  _inv_resolution = 1.0 / _resolution;

  _gl_xl = -_x_size / 2.0;
  _gl_yl = -_y_size / 2.0;
  _gl_zl = 0.0;

  _GLX_SIZE = (int)(_x_size * _inv_resolution);
  _GLY_SIZE = (int)(_y_size * _inv_resolution);
  _GLZ_SIZE = (int)(_z_size * _inv_resolution);

  ros::Rate rate(100);
  bool status = ros::ok();
  while (status) {
    ros::spinOnce();
    status = ros::ok();
    rate.sleep();
  }
}


#include <stdlib.h>
#include <gtk/gtk.h>
#include <libintl.h>
#include <fstream>
#include <boost/lexical_cast.hpp>
//#include <boost/random.hpp>
//#include "generators.h"
using namespace std;
using namespace boost;
//using namespace boost::random;
#define _(x) gettext(x)

///Global variables
GtkWidget *win;
GtkWidget *rootHbox; //"More root" than rootVbox
 GtkWidget *labelVbox;
 GtkWidget *rootVbox;
GtkWidget *hbox;
GtkWidget *label;
GtkWidget *algorithmComboBox;
GtkWidget *distributionComboBox;
GtkWidget *seedEntry;
unsigned long seed;
#ifdef __linux__
  GtkWidget *hqSeedCheckButton; //High quality seed, uses /dev/random instead of /dev/urandom
#endif
GtkWidget *generateSeedButton;
GtkWidget *lowerLimitEntry;
GtkWidget *upperLimitEntry;
GtkWidget *amountSpinner;
GtkWidget *fileEntry;

static void generateSeed(GtkWidget *wid, gpointer data)
{

    ///Read from /dev/(u)random if we are using linux or use time(0) on windows
    #ifdef __linux__
        ifstream f;
        ///Dependent on the state of highEntropyCheckbox open either /dev/random or /dev/urandom for read
        if(gtk_toggle_button_get_active(GTK_TOGGLE_BUTTON(hqSeedCheckButton)))
            {f.open("/dev/random", ifstream::in);}
        else {f.open("/dev/urandom", ifstream::in);}
        f.read(reinterpret_cast<char*>(&seed), sizeof(seed));
        f.close();
    #else ///Seed from time(0)
        mt19937 mersenne(static_cast<unsigned long>(time(0)));
        uniform_int<unsigned int> dist(0, ULONG_MAX);
        variate_generator<mt19937&, uniform_int<unsigned int>> dist(mersenne, evenDist);
        seed = mersenne();
    #endif
        gtk_entry_set_text(GTK_ENTRY(seedEntry), lexical_cast<string>(seed).c_str());
}

int main (int argc, char *argv[])
{

  /* Initialize GTK+ */
  g_log_set_handler ("Gtk", G_LOG_LEVEL_WARNING, (GLogFunc) gtk_false, NULL);
  gtk_init (&argc, &argv);
  g_log_set_handler ("Gtk", G_LOG_LEVEL_WARNING, g_log_default_handler, NULL);

  /* Create the main window */
  win = gtk_window_new (GTK_WINDOW_TOPLEVEL);
  gtk_container_set_border_width (GTK_CONTAINER (win), 8);
  gtk_window_set_title (GTK_WINDOW (win), _("MultiRNG"));
  gtk_window_set_position (GTK_WINDOW (win), GTK_WIN_POS_CENTER);
  gtk_widget_realize (win);
  g_signal_connect (win, "destroy", gtk_main_quit, NULL);

  /// Create the main horizontal/vertical boxes and fill it.
  rootHbox = gtk_hbox_new(false,5);
   labelVbox = gtk_vbox_new(false,5);
   rootVbox = gtk_vbox_new (false, 5);
  //Create new hbox
  label = gtk_label_new(_("Library"));
   gtk_box_pack_start_defaults(GTK_BOX(labelVbox), label);
  ///Init algorithm widgets
  hbox = gtk_hbox_new(false, 5);
  label = gtk_label_new(_("Algorithms:"));
    gtk_box_pack_start_defaults(GTK_BOX(labelVbox), label);
  algorithmComboBox = gtk_combo_box_new_text();
    gtk_combo_box_append_text(GTK_COMBO_BOX(algorithmComboBox), _("MT 19937"));
    gtk_combo_box_append_text(GTK_COMBO_BOX(algorithmComboBox), _("Mersenne Twister 191937"));  ///1
    gtk_combo_box_append_text(GTK_COMBO_BOX(algorithmComboBox), _("MInstdRand (LC Variant)"));  ///2
    gtk_combo_box_append_text(GTK_COMBO_BOX(algorithmComboBox), _("Ecuyer 1988"));              ///3
    gtk_combo_box_append_text(GTK_COMBO_BOX(algorithmComboBox), _("Kreutzer 1986"));            ///4
    gtk_combo_box_append_text(GTK_COMBO_BOX(algorithmComboBox), _("Hellekalek 1995"));          ///5
    gtk_combo_box_append_text(GTK_COMBO_BOX(algorithmComboBox), _("Mersenne Twister 11213b"));  ///6
    gtk_combo_box_append_text(GTK_COMBO_BOX(algorithmComboBox), _("Lagged Fibonacci 607"));     ///7
    gtk_combo_box_append_text(GTK_COMBO_BOX(algorithmComboBox), _("Lagged Fibonacci 1279"));    ///8
    gtk_combo_box_append_text(GTK_COMBO_BOX(algorithmComboBox), _("Lagged Fibonacci 2281"));    ///9
    gtk_combo_box_append_text(GTK_COMBO_BOX(algorithmComboBox), _("Lagged Fibonacci 3217"));    ///10
    gtk_combo_box_append_text(GTK_COMBO_BOX(algorithmComboBox), _("Lagged Fibonacci 4423"));    ///11
    gtk_combo_box_append_text(GTK_COMBO_BOX(algorithmComboBox), _("Lagged Fibonacci 9689"));    ///12
    gtk_combo_box_append_text(GTK_COMBO_BOX(algorithmComboBox), _("Lagged Fibonacci 19937"));   ///13
    gtk_combo_box_append_text(GTK_COMBO_BOX(algorithmComboBox), _("Lagged Fibonacci 23209"));   ///14
    gtk_combo_box_append_text(GTK_COMBO_BOX(algorithmComboBox), _("Lagged Fibonacci 44497"));   ///15
    gtk_combo_box_set_active(GTK_COMBO_BOX(algorithmComboBox), 0);
    gtk_box_pack_start_defaults(GTK_BOX(hbox),algorithmComboBox);
   gtk_box_pack_start_defaults(GTK_BOX(rootVbox), hbox);
  ///Init distribution widgets
  hbox = gtk_hbox_new(false, 5);
  label = gtk_label_new(_("Distributions:"));
    gtk_box_pack_start_defaults(GTK_BOX(labelVbox), label);
  distributionComboBox = gtk_combo_box_new_text();
    gtk_combo_box_append_text(GTK_COMBO_BOX(distributionComboBox), _("Uniform Integer"));
    gtk_combo_box_append_text(GTK_COMBO_BOX(distributionComboBox), _("Uniform Real"));
    gtk_combo_box_append_text(GTK_COMBO_BOX(distributionComboBox), _("Triangle"));
    gtk_combo_box_append_text(GTK_COMBO_BOX(distributionComboBox), _("Bernoulli"));
    gtk_combo_box_append_text(GTK_COMBO_BOX(distributionComboBox), _("Cauchy"));
    gtk_combo_box_append_text(GTK_COMBO_BOX(distributionComboBox), _("Exponential"));
    gtk_combo_box_append_text(GTK_COMBO_BOX(distributionComboBox), _("Geometric"));
    gtk_combo_box_append_text(GTK_COMBO_BOX(distributionComboBox), _("Normal"));
    gtk_combo_box_append_text(GTK_COMBO_BOX(distributionComboBox), _("Lognormal"));
    gtk_combo_box_append_text(GTK_COMBO_BOX(distributionComboBox), _("Uniform on Sphere"));
    gtk_combo_box_set_active(GTK_COMBO_BOX(algorithmComboBox), 0);
    gtk_box_pack_start_defaults(GTK_BOX(hbox),distributionComboBox);
   gtk_box_pack_start_defaults(GTK_BOX(rootVbox), hbox);
  ///Init file widgets
  hbox = gtk_hbox_new(false, 5);
  label = gtk_label_new(_("File:"));
    gtk_box_pack_start_defaults(GTK_BOX(labelVbox), label);
  fileEntry = gtk_entry_new();
    gtk_entry_set_text(GTK_ENTRY(fileEntry), "random.txt");
    gtk_box_pack_start_defaults(GTK_BOX(hbox), fileEntry);
   gtk_box_pack_start_defaults(GTK_BOX(rootVbox), hbox);
  ///Init Seed widgets
  hbox = gtk_hbox_new(false, 5);
  label = gtk_label_new(_("Seed:"));
    gtk_box_pack_start_defaults(GTK_BOX(labelVbox), label);
  seedEntry = gtk_entry_new();
    gtk_entry_set_text(GTK_ENTRY(seedEntry), "1234567890");
    gtk_box_pack_start_defaults(GTK_BOX(hbox), seedEntry);
  #ifdef __linux__
    hqSeedCheckButton = gtk_check_button_new_with_label(_("HQ Entropy"));
     gtk_box_pack_start_defaults(GTK_BOX(hbox), hqSeedCheckButton);
  #endif
  generateSeedButton = gtk_button_new_with_label(_("Generate"));
   g_signal_connect (generateSeedButton, "clicked", G_CALLBACK(generateSeed), NULL);
   gtk_box_pack_start_defaults(GTK_BOX(rootVbox), hbox);
  ///Init limit widgets
  hbox = gtk_hbox_new(false, 5);
  label = gtk_label_new(_("Lower limit:"));
    gtk_box_pack_start_defaults(GTK_BOX(labelVbox), label);
  lowerLimitEntry = gtk_entry_new();
    gtk_entry_set_text(GTK_ENTRY(lowerLimitEntry), "0");
    gtk_box_pack_start_defaults(GTK_BOX(hbox), lowerLimitEntry);
  label = gtk_label_new(_("Upper limit:"));
    gtk_box_pack_start_defaults(GTK_BOX(labelVbox), label);
  upperLimitEntry = gtk_entry_new();
    gtk_entry_set_text(GTK_ENTRY(lowerLimitEntry), "1000");
    gtk_box_pack_start_defaults(GTK_BOX(hbox), upperLimitEntry);
   gtk_box_pack_start_defaults(GTK_BOX(rootVbox), hbox);

   ///Add subordinary boxes to rootBox
   gtk_box_pack_start_defaults(GTK_BOX(rootHbox), labelVbox);
   gtk_box_pack_start_defaults(GTK_BOX(rootHbox), rootVbox);

  gtk_container_add (GTK_CONTAINER (win), rootHbox);

  /* Enter the main loop */
  gtk_widget_show_all(win);
  gtk_main ();
  return 0;
}


// This file is a part of the OpenSurgSim project.
// Copyright 2013, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <Eigen/Core>
using Eigen::MatrixXd;
using Eigen::VectorXd;

#include "SurgSim/Physics/BuildMlcp.h"
#include "SurgSim/Physics/Constraint.h"
#include "SurgSim/Physics/ConstraintImplementation.h"
#include "SurgSim/Physics/Localization.h"
#include "SurgSim/Physics/MlcpPhysicsProblem.h"
#include "SurgSim/Physics/PhysicsManagerState.h"
#include "SurgSim/Physics/Representation.h"

namespace SurgSim
{
namespace Physics
{

BuildMlcp::BuildMlcp(bool doCopyState) : Computation(doCopyState)
{}

BuildMlcp::~BuildMlcp()
{}

std::shared_ptr<PhysicsManagerState>
BuildMlcp::doUpdate(const double& dt, const std::shared_ptr<PhysicsManagerState>& state)
{
	// Copy state to new state
	std::shared_ptr<PhysicsManagerState> result = state;
	MlcpMapping<Representation> representationsMapping;
	MlcpMapping<Constraint> constraintsMapping;

	size_t numAtomicConstraint = 0;
	size_t numDof = 0;

	// Calculate numAtomicConstraint
	auto const activeConstraints = result->getActiveConstraints();
	for (auto it = activeConstraints.cbegin(); it != activeConstraints.cend(); it++)
	{
		constraintsMapping.setValue((*it).get(), static_cast<ptrdiff_t>(numAtomicConstraint));
		numAtomicConstraint += (*it)->getNumDof();
	}
	result->setConstraintsMapping(constraintsMapping);

	// Calculate numDof size
	const auto& activeRepresentations = result->getActiveRepresentations();
	for (auto it = activeRepresentations.cbegin(); it != activeRepresentations.cend(); it++)
	{
		representationsMapping.setValue((*it).get(), numDof);
		numDof += (*it)->getNumDof();
	}
	result->setRepresentationsMapping(representationsMapping);

	// Resize the Mlcp problem
	result->getMlcpProblem().A.setZero(numAtomicConstraint, numAtomicConstraint);
	result->getMlcpProblem().b.setZero(numAtomicConstraint);
	result->getMlcpProblem().H.resize(numAtomicConstraint, numDof);
	result->getMlcpProblem().CHt.setZero(numDof, numAtomicConstraint);
	result->getMlcpProblem().mu.setZero(numAtomicConstraint);
	result->getMlcpProblem().constraintTypes.clear();

	// Resize the Mlcp solution
	result->getMlcpSolution().dofCorrection.setZero(numDof);
	result->getMlcpSolution().x.setZero(numAtomicConstraint);

	// Fill up the Mlcp problem
	for (auto it = activeConstraints.begin(); it != activeConstraints.end(); it++)
	{
		ptrdiff_t indexConstraint = result->getConstraintsMapping().getValue((*it).get());
		SURGSIM_ASSERT(indexConstraint >= 0) << "Index for constraint is invalid: " << indexConstraint << std::endl;

		std::shared_ptr<ConstraintImplementation> side0 = (*it)->getImplementations().first;
		std::shared_ptr<ConstraintImplementation> side1 = (*it)->getImplementations().second;
		SURGSIM_ASSERT(side0) << "Constraint does not have a side0" << std::endl;
		SURGSIM_ASSERT(side1) << "Constraint does not have a side1" << std::endl;
		std::shared_ptr<Localization> localization0 = (*it)->getLocalizations().first;
		std::shared_ptr<Localization> localization1 = (*it)->getLocalizations().second;
		SURGSIM_ASSERT(localization0) << "ConstraintImplementation does not have a localization on side0";
		SURGSIM_ASSERT(localization1) << "ConstraintImplementation does not have a localization on side1";
		const MlcpMapping<Representation>& mapping = result->getRepresentationsMapping();
		ptrdiff_t indexRepresentation0 = mapping.getValue(localization0->getRepresentation().get());
		ptrdiff_t indexRepresentation1 = mapping.getValue(localization1->getRepresentation().get());
		SURGSIM_ASSERT(indexRepresentation0 >= 0) << "Index for representation 0 is invalid: " <<
			indexRepresentation0;
		SURGSIM_ASSERT(indexRepresentation1 >= 0) << "Index for representation 1 is invalid: " <<
			indexRepresentation1;

		(*it)->build(dt, &result->getMlcpProblem(), indexRepresentation0, indexRepresentation1, indexConstraint);
	}

	return result;
}

}; // namespace Physics
}; // namespace SurgSim


//
//  average.hpp
//
//  Created by Anton Leuski on 5/12/13.
//  Copyright (c) 2015 Anton Leuski & ICT/USC. All rights reserved.
//
//  This file is part of Jerome.
//
//  Licensed under the Apache License, Version 2.0 (the "License");
//  you may not use this file except in compliance with the License.
//  You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
//

#ifndef __jerome_ir_evaluation_accumulators_statistics_average_hpp__
#define __jerome_ir_evaluation_accumulators_statistics_average_hpp__

#include <jerome/ir/evaluation/accumulators/statistics/total.hpp>
#include <jerome/ir/evaluation/accumulators/statistics/sample_count.hpp>
#include <boost/accumulators/numeric/functional.hpp>

// =============================================================================
// average

namespace jerome {
  namespace ir {
    namespace evaluation {
      namespace accumulators {

        namespace impl {

					template <typename T>
					using fdiv = boost::numeric::functional::fdiv<
							T, std::size_t>;

          template <typename Sample, typename Feature>
          struct average_impl
            : subtractable_accumulator_base
          {
          private:
            typedef accumulator_set<Sample, features<Feature>>
              feature_accumulator_type;

            typedef typename boost::mpl::apply<
              feature_accumulator_type, Feature>::type
              feature_type;

            typedef typename feature_type::result_type
              feature_result_type;

          public:

						typedef typename fdiv<feature_result_type>::result_type result_type;

            average_impl(dont_care)
            {
            }

            template <typename Args>
            result_type result(Args const& args) const
            {
              std::size_t denom = sample_count(args);
              return denom == 0 ? 0 : boost::numeric::average(
                total_of<Feature>(args), denom);
            }

          };

          template <typename Sample>
          struct average_impl1
            : subtractable_accumulator_base
          {

						typedef typename fdiv<Sample>::result_type result_type;

            average_impl1(dont_care)
            {
            }

            template <typename Args>
            result_type result(Args const& args) const
            {
              std::size_t denom = sample_count(args);
              return denom ==
                     0 ? 0 : boost::numeric::average(total(args), denom);
            }

          };

        }
        namespace tag {
          template <typename Feature>
          struct average_of
            : depends_on<total_of<Feature>, sample_count>
          {
            typedef accumulators::impl::average_impl<boost::mpl::_1,
              Feature> impl;
          };

          struct average
            : depends_on<total, sample_count>
          {
            typedef accumulators::impl::average_impl1<boost::mpl::_1> impl;
          };
        }

        namespace extract {
          BOOST_ACCUMULATORS_DEFINE_EXTRACTOR(tag, average_of, (typename))
          extractor<tag::average> const average = {};
          BOOST_ACCUMULATORS_IGNORE_GLOBAL(average)
        }

        using extract::average;
        using extract::average_of;
      }
    }
  }
}

#endif // __jerome_ir_evaluation_accumulators_statistics_average_hpp__