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

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.

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

// Blueberry
#include <berryISelectionService.h>
#include <berryIWorkbenchWindow.h>

// Qmitk
#include "QmitkFiberGenerationView.h"

// MITK
#include <mitkImage.h>
#include <mitkImageToItk.h>
#include <mitkImageCast.h>
#include <mitkProperties.h>
#include <mitkPlanarFigureInteractor.h>
#include <mitkDataStorage.h>
#include <itkFibersFromPlanarFiguresFilter.h>
#include <itkTractsToDWIImageFilter.h>
#include <mitkTensorImage.h>
#include <mitkILinkedRenderWindowPart.h>
#include <mitkImageToItk.h>
#include <mitkImageCast.h>
#include <mitkImageGenerator.h>
#include <mitkNodePredicateDataType.h>
#include <itkScalableAffineTransform.h>
#include <mitkLevelWindowProperty.h>
#include <mitkNodePredicateOr.h>
#include <mitkNodePredicateAnd.h>
#include <mitkNodePredicateNot.h>
#include <mitkNodePredicateProperty.h>
#include <mitkNodePredicateIsDWI.h>
#include <boost/property_tree/ptree.hpp>
#define RAPIDXML_NO_EXCEPTIONS
#include <boost/property_tree/xml_parser.hpp>
#include <boost/foreach.hpp>
#include <QFileDialog>
#include <QMessageBox>
#include "usModuleRegistry.h"
#include <mitkChiSquareNoiseModel.h>
#include <itksys/SystemTools.hxx>
#include <mitkIOUtil.h>
#include <QScrollBar>
#include <itkInvertIntensityImageFilter.h>
#include <QDialogButtonBox>
#include <itkAdcImageFilter.h>
#include <itkShiftScaleImageFilter.h>
#include <mitkITKImageImport.h>

#include "mitkNodePredicateDataType.h"
#include <mitkNodePredicateProperty.h>
#include <mitkNodePredicateAnd.h>
#include <mitkNodePredicateNot.h>
#include <mitkNodePredicateOr.h>

#define _USE_MATH_DEFINES
#include <math.h>

const std::string QmitkFiberGenerationView::VIEW_ID = "org.mitk.views.fibergenerationview";

QmitkFiberGenerationView::QmitkFiberGenerationView()
  : QmitkAbstractView()
  , m_Controls( 0 )
  , m_SelectedImageNode( nullptr )
{

}

// Destructor
QmitkFiberGenerationView::~QmitkFiberGenerationView()
{
}

void QmitkFiberGenerationView::CreateQtPartControl( QWidget *parent )
{
  // build up qt view, unless already done
  if ( !m_Controls )
  {
    // create GUI widgets from the Qt Designer's .ui file
    m_Controls = new Ui::QmitkFiberGenerationViewControls;
    m_Controls->setupUi( parent );

    m_ParameterFile = QDir::currentPath()+"/param.ffp";
    connect((QObject*) m_Controls->m_GenerateFibersButton, SIGNAL(clicked()), (QObject*) this, SLOT(GenerateFibers()));
    connect((QObject*) m_Controls->m_CircleButton, SIGNAL(clicked()), (QObject*) this, SLOT(OnDrawROI()));
    connect((QObject*) m_Controls->m_FlipButton, SIGNAL(clicked()), (QObject*) this, SLOT(OnFlipButton()));
    connect((QObject*) m_Controls->m_JoinBundlesButton, SIGNAL(clicked()), (QObject*) this, SLOT(JoinBundles()));
    connect((QObject*) m_Controls->m_VarianceBox, SIGNAL(valueChanged(double)), (QObject*) this, SLOT(OnVarianceChanged(double)));
    connect((QObject*) m_Controls->m_DistributionBox, SIGNAL(currentIndexChanged(int)), (QObject*) this, SLOT(OnDistributionChanged(int)));
    connect((QObject*) m_Controls->m_FiberDensityBox, SIGNAL(valueChanged(int)), (QObject*) this, SLOT(OnFiberDensityChanged(int)));
    connect((QObject*) m_Controls->m_FiberSamplingBox, SIGNAL(valueChanged(double)), (QObject*) this, SLOT(OnFiberSamplingChanged(double)));
    connect((QObject*) m_Controls->m_TensionBox, SIGNAL(valueChanged(double)), (QObject*) this, SLOT(OnTensionChanged(double)));
    connect((QObject*) m_Controls->m_ContinuityBox, SIGNAL(valueChanged(double)), (QObject*) this, SLOT(OnContinuityChanged(double)));
    connect((QObject*) m_Controls->m_BiasBox, SIGNAL(valueChanged(double)), (QObject*) this, SLOT(OnBiasChanged(double)));

    connect((QObject*) m_Controls->m_ConstantRadiusBox, SIGNAL(stateChanged(int)), (QObject*) this, SLOT(OnConstantRadius(int)));
    connect((QObject*) m_Controls->m_CopyBundlesButton, SIGNAL(clicked()), (QObject*) this, SLOT(CopyBundles()));
    connect((QObject*) m_Controls->m_TransformBundlesButton, SIGNAL(clicked()), (QObject*) this, SLOT(ApplyTransform()));
    connect((QObject*) m_Controls->m_AlignOnGrid, SIGNAL(clicked()), (QObject*) this, SLOT(AlignOnGrid()));

    connect((QObject*) m_Controls->m_AdvancedOptionsBox, SIGNAL( stateChanged(int)), (QObject*) this, SLOT(ShowAdvancedOptions(int)));

    connect((QObject*) m_Controls->m_SaveParametersButton, SIGNAL(clicked()), (QObject*) this, SLOT(SaveParameters()));
    connect((QObject*) m_Controls->m_LoadParametersButton, SIGNAL(clicked()), (QObject*) this, SLOT(LoadParameters()));

  }
  UpdateGui();
}

void QmitkFiberGenerationView::UpdateParametersFromGui()
{
  m_Parameters.ClearFiberParameters();
  m_Parameters.m_Misc.m_CheckAdvancedFiberOptionsBox = m_Controls->m_AdvancedOptionsBox->isChecked();

  switch(m_Controls->m_DistributionBox->currentIndex())
  {
  case 0:
    m_Parameters.m_FiberGen.m_Distribution = FiberGenerationParameters::DISTRIBUTE_UNIFORM;
    break;
  case 1:
    m_Parameters.m_FiberGen.m_Distribution = FiberGenerationParameters::DISTRIBUTE_GAUSSIAN;
    break;
  default:
    m_Parameters.m_FiberGen.m_Distribution = FiberGenerationParameters::DISTRIBUTE_UNIFORM;
  }
  m_Parameters.m_FiberGen.m_Variance = m_Controls->m_VarianceBox->value();
  m_Parameters.m_FiberGen.m_Density = m_Controls->m_FiberDensityBox->value();
  m_Parameters.m_FiberGen.m_Sampling = m_Controls->m_FiberSamplingBox->value();
  m_Parameters.m_FiberGen.m_Tension = m_Controls->m_TensionBox->value();
  m_Parameters.m_FiberGen.m_Continuity = m_Controls->m_ContinuityBox->value();
  m_Parameters.m_FiberGen.m_Bias = m_Controls->m_BiasBox->value();
  m_Parameters.m_FiberGen.m_Rotation[0] = m_Controls->m_XrotBox->value();
  m_Parameters.m_FiberGen.m_Rotation[1] = m_Controls->m_YrotBox->value();
  m_Parameters.m_FiberGen.m_Rotation[2] = m_Controls->m_ZrotBox->value();
  m_Parameters.m_FiberGen.m_Translation[0] = m_Controls->m_XtransBox->value();
  m_Parameters.m_FiberGen.m_Translation[1] = m_Controls->m_YtransBox->value();
  m_Parameters.m_FiberGen.m_Translation[2] = m_Controls->m_ZtransBox->value();
  m_Parameters.m_FiberGen.m_Scale[0] = m_Controls->m_XscaleBox->value();
  m_Parameters.m_FiberGen.m_Scale[1] = m_Controls->m_YscaleBox->value();
  m_Parameters.m_FiberGen.m_Scale[2] = m_Controls->m_ZscaleBox->value();

  m_Parameters.m_Misc.m_CheckRealTimeFibersBox = m_Controls->m_RealTimeFibers->isChecked();
  m_Parameters.m_Misc.m_CheckAdvancedFiberOptionsBox = m_Controls->m_AdvancedOptionsBox->isChecked();
  m_Parameters.m_Misc.m_CheckIncludeFiducialsBox = m_Controls->m_IncludeFiducials->isChecked();
  m_Parameters.m_Misc.m_CheckConstantRadiusBox = m_Controls->m_ConstantRadiusBox->isChecked();
}

void QmitkFiberGenerationView::SaveParameters(QString filename)
{
  UpdateParametersFromGui();
  m_Parameters.SaveParameters(filename.toStdString());
  m_ParameterFile = filename;
}

void QmitkFiberGenerationView::SaveParameters()
{
  QString filename = QFileDialog::getSaveFileName(
        0,
        tr("Save Parameters"),
        m_ParameterFile,
        tr("Fiberfox Parameters (*.ffp)") );

  SaveParameters(filename);
}

void QmitkFiberGenerationView::LoadParameters()
{
  QString filename = QFileDialog::getOpenFileName(0, tr("Load Parameters"), QString(itksys::SystemTools::GetFilenamePath(m_ParameterFile.toStdString()).c_str()), tr("Fiberfox Parameters (*.ffp)") );
  if(filename.isEmpty() || filename.isNull())
    return;

  m_ParameterFile = filename;
  m_Parameters.LoadParameters(filename.toStdString());

  if (m_Parameters.m_MissingTags.size()>0)
  {
    QString missing("Parameter file might be corrupted. The following parameters could not be read: ");
    missing += QString(m_Parameters.m_MissingTags.c_str());
    missing += "\nDefault values have been assigned to the missing parameters.";
    QMessageBox::information( nullptr, "Warning!", missing);
  }

  m_Controls->m_RealTimeFibers->setChecked(m_Parameters.m_Misc.m_CheckRealTimeFibersBox);
  m_Controls->m_AdvancedOptionsBox->setChecked(m_Parameters.m_Misc.m_CheckAdvancedFiberOptionsBox);
  m_Controls->m_IncludeFiducials->setChecked(m_Parameters.m_Misc.m_CheckIncludeFiducialsBox);
  m_Controls->m_ConstantRadiusBox->setChecked(m_Parameters.m_Misc.m_CheckConstantRadiusBox);

  m_Controls->m_DistributionBox->setCurrentIndex(m_Parameters.m_FiberGen.m_Distribution);
  m_Controls->m_VarianceBox->setValue(m_Parameters.m_FiberGen.m_Variance);
  m_Controls->m_FiberDensityBox->setValue(m_Parameters.m_FiberGen.m_Density);
  m_Controls->m_FiberSamplingBox->setValue(m_Parameters.m_FiberGen.m_Sampling);
  m_Controls->m_TensionBox->setValue(m_Parameters.m_FiberGen.m_Tension);
  m_Controls->m_ContinuityBox->setValue(m_Parameters.m_FiberGen.m_Continuity);
  m_Controls->m_BiasBox->setValue(m_Parameters.m_FiberGen.m_Bias);
  m_Controls->m_XrotBox->setValue(m_Parameters.m_FiberGen.m_Rotation[0]);
  m_Controls->m_YrotBox->setValue(m_Parameters.m_FiberGen.m_Rotation[1]);
  m_Controls->m_ZrotBox->setValue(m_Parameters.m_FiberGen.m_Rotation[2]);
  m_Controls->m_XtransBox->setValue(m_Parameters.m_FiberGen.m_Translation[0]);
  m_Controls->m_YtransBox->setValue(m_Parameters.m_FiberGen.m_Translation[1]);
  m_Controls->m_ZtransBox->setValue(m_Parameters.m_FiberGen.m_Translation[2]);
  m_Controls->m_XscaleBox->setValue(m_Parameters.m_FiberGen.m_Scale[0]);
  m_Controls->m_YscaleBox->setValue(m_Parameters.m_FiberGen.m_Scale[1]);
  m_Controls->m_ZscaleBox->setValue(m_Parameters.m_FiberGen.m_Scale[2]);
}

void QmitkFiberGenerationView::ShowAdvancedOptions(int state)
{
  if (state)
  {
    m_Controls->m_AdvancedFiberOptionsFrame->setVisible(true);
    m_Controls->m_AdvancedOptionsBox->setChecked(true);
  }
  else
  {
    m_Controls->m_AdvancedFiberOptionsFrame->setVisible(false);
    m_Controls->m_AdvancedOptionsBox->setChecked(false);
  }
}

void QmitkFiberGenerationView::OnConstantRadius(int value)
{
  if (value>0 && m_Controls->m_RealTimeFibers->isChecked())
    GenerateFibers();
}

void QmitkFiberGenerationView::OnDistributionChanged(int value)
{
  if (value==1)
    m_Controls->m_VarianceBox->setVisible(true);
  else
    m_Controls->m_VarianceBox->setVisible(false);

  if (m_Controls->m_RealTimeFibers->isChecked())
    GenerateFibers();
}

void QmitkFiberGenerationView::OnVarianceChanged(double)
{
  if (m_Controls->m_RealTimeFibers->isChecked())
    GenerateFibers();
}

void QmitkFiberGenerationView::OnFiberDensityChanged(int)
{
  if (m_Controls->m_RealTimeFibers->isChecked())
    GenerateFibers();
}

void QmitkFiberGenerationView::OnFiberSamplingChanged(double)
{
  if (m_Controls->m_RealTimeFibers->isChecked())
    GenerateFibers();
}

void QmitkFiberGenerationView::OnTensionChanged(double)
{
  if (m_Controls->m_RealTimeFibers->isChecked())
    GenerateFibers();
}

void QmitkFiberGenerationView::OnContinuityChanged(double)
{
  if (m_Controls->m_RealTimeFibers->isChecked())
    GenerateFibers();
}

void QmitkFiberGenerationView::OnBiasChanged(double)
{
  if (m_Controls->m_RealTimeFibers->isChecked())
    GenerateFibers();
}

void QmitkFiberGenerationView::AlignOnGrid()
{
  for (unsigned int i=0; i<m_SelectedFiducials.size(); i++)
  {
    mitk::PlanarEllipse::Pointer pe = dynamic_cast<mitk::PlanarEllipse*>(m_SelectedFiducials.at(i)->GetData());
    mitk::Point3D wc0 = pe->GetWorldControlPoint(0);

    mitk::DataStorage::SetOfObjects::ConstPointer parentFibs = GetDataStorage()->GetSources(m_SelectedFiducials.at(i));
    for( mitk::DataStorage::SetOfObjects::const_iterator it = parentFibs->begin(); it != parentFibs->end(); ++it )
    {
      mitk::DataNode::Pointer pFibNode = *it;
      if ( pFibNode.IsNotNull() && dynamic_cast<mitk::FiberBundle*>(pFibNode->GetData()) )
      {
        mitk::DataStorage::SetOfObjects::ConstPointer parentImgs = GetDataStorage()->GetSources(pFibNode);
        for( mitk::DataStorage::SetOfObjects::const_iterator it2 = parentImgs->begin(); it2 != parentImgs->end(); ++it2 )
        {
          mitk::DataNode::Pointer pImgNode = *it2;
          if ( pImgNode.IsNotNull() && dynamic_cast<mitk::Image*>(pImgNode->GetData()) )
          {
            mitk::Image::Pointer img = dynamic_cast<mitk::Image*>(pImgNode->GetData());
            mitk::BaseGeometry::Pointer geom = img->GetGeometry();
            itk::Index<3> idx;
            geom->WorldToIndex(wc0, idx);

            mitk::Point3D cIdx; cIdx[0]=idx[0]; cIdx[1]=idx[1]; cIdx[2]=idx[2];
            mitk::Point3D world;
            geom->IndexToWorld(cIdx,world);

            mitk::Vector3D trans = world - wc0;
            pe->GetGeometry()->Translate(trans);

            break;
          }
        }
        break;
      }
    }
  }

  for(unsigned int i=0; i<m_SelectedBundles2.size(); i++ )
  {
    mitk::DataNode::Pointer fibNode = m_SelectedBundles2.at(i);

    mitk::DataStorage::SetOfObjects::ConstPointer sources = GetDataStorage()->GetSources(fibNode);
    for( mitk::DataStorage::SetOfObjects::const_iterator it = sources->begin(); it != sources->end(); ++it )
    {
      mitk::DataNode::Pointer imgNode = *it;
      if ( imgNode.IsNotNull() && dynamic_cast<mitk::Image*>(imgNode->GetData()) )
      {
        mitk::DataStorage::SetOfObjects::ConstPointer derivations = GetDataStorage()->GetDerivations(fibNode);
        for( mitk::DataStorage::SetOfObjects::const_iterator it2 = derivations->begin(); it2 != derivations->end(); ++it2 )
        {
          mitk::DataNode::Pointer fiducialNode = *it2;
          if ( fiducialNode.IsNotNull() && dynamic_cast<mitk::PlanarEllipse*>(fiducialNode->GetData()) )
          {
            mitk::PlanarEllipse::Pointer pe = dynamic_cast<mitk::PlanarEllipse*>(fiducialNode->GetData());
            mitk::Point3D wc0 = pe->GetWorldControlPoint(0);

            mitk::Image::Pointer img = dynamic_cast<mitk::Image*>(imgNode->GetData());
            mitk::BaseGeometry::Pointer geom = img->GetGeometry();
            itk::Index<3> idx;
            geom->WorldToIndex(wc0, idx);
            mitk::Point3D cIdx; cIdx[0]=idx[0]; cIdx[1]=idx[1]; cIdx[2]=idx[2];
            mitk::Point3D world;
            geom->IndexToWorld(cIdx,world);

            mitk::Vector3D trans = world - wc0;
            pe->GetGeometry()->Translate(trans);
          }
        }

        break;
      }
    }
  }

  for(unsigned int i=0; i<m_SelectedImages.size(); i++ )
  {
    mitk::Image::Pointer img = dynamic_cast<mitk::Image*>(m_SelectedImages.at(i)->GetData());

    mitk::DataStorage::SetOfObjects::ConstPointer derivations = GetDataStorage()->GetDerivations(m_SelectedImages.at(i));
    for( mitk::DataStorage::SetOfObjects::const_iterator it = derivations->begin(); it != derivations->end(); ++it )
    {
      mitk::DataNode::Pointer fibNode = *it;
      if ( fibNode.IsNotNull() && dynamic_cast<mitk::FiberBundle*>(fibNode->GetData()) )
      {
        mitk::DataStorage::SetOfObjects::ConstPointer derivations2 = GetDataStorage()->GetDerivations(fibNode);
        for( mitk::DataStorage::SetOfObjects::const_iterator it2 = derivations2->begin(); it2 != derivations2->end(); ++it2 )
        {
          mitk::DataNode::Pointer fiducialNode = *it2;
          if ( fiducialNode.IsNotNull() && dynamic_cast<mitk::PlanarEllipse*>(fiducialNode->GetData()) )
          {
            mitk::PlanarEllipse::Pointer pe = dynamic_cast<mitk::PlanarEllipse*>(fiducialNode->GetData());
            mitk::Point3D wc0 = pe->GetWorldControlPoint(0);

            mitk::BaseGeometry::Pointer geom = img->GetGeometry();
            itk::Index<3> idx;
            geom->WorldToIndex(wc0, idx);
            mitk::Point3D cIdx; cIdx[0]=idx[0]; cIdx[1]=idx[1]; cIdx[2]=idx[2];
            mitk::Point3D world;
            geom->IndexToWorld(cIdx,world);

            mitk::Vector3D trans = world - wc0;
            pe->GetGeometry()->Translate(trans);
          }
        }
      }
    }
  }

  mitk::RenderingManager::GetInstance()->RequestUpdateAll();
  if (m_Controls->m_RealTimeFibers->isChecked())
    GenerateFibers();
}

void QmitkFiberGenerationView::OnFlipButton()
{
  if (m_SelectedFiducial.IsNull())
    return;

  std::map<mitk::DataNode*, QmitkPlanarFigureData>::iterator it = m_DataNodeToPlanarFigureData.find(m_SelectedFiducial.GetPointer());
  if( it != m_DataNodeToPlanarFigureData.end() )
  {
    QmitkPlanarFigureData& data = it->second;
    data.m_Flipped += 1;
    data.m_Flipped %= 2;
  }

  if (m_Controls->m_RealTimeFibers->isChecked())
    GenerateFibers();
}

void QmitkFiberGenerationView::OnAddBundle()
{
  if (m_SelectedImageNode.IsNull())
    return;

  mitk::DataStorage::SetOfObjects::ConstPointer children = GetDataStorage()->GetDerivations(m_SelectedImageNode);

  mitk::FiberBundle::Pointer bundle = mitk::FiberBundle::New();
  mitk::DataNode::Pointer node = mitk::DataNode::New();
  node->SetData( bundle );
  QString name = QString("Bundle_%1").arg(children->size());
  node->SetName(name.toStdString());
  m_SelectedBundles.push_back(node);
  UpdateGui();

  GetDataStorage()->Add(node, m_SelectedImageNode);
}

void QmitkFiberGenerationView::OnDrawROI()
{
  if (m_SelectedBundles.empty())
    OnAddBundle();
  if (m_SelectedBundles.empty())
    return;

  mitk::DataStorage::SetOfObjects::ConstPointer children = GetDataStorage()->GetDerivations(m_SelectedBundles.at(0));

  mitk::PlanarEllipse::Pointer figure = mitk::PlanarEllipse::New();

  mitk::DataNode::Pointer node = mitk::DataNode::New();
  node->SetData( figure );
  node->SetBoolProperty("planarfigure.3drendering", true);
  node->SetBoolProperty("planarfigure.3drendering.fill", true);

  QList<mitk::DataNode::Pointer> nodes = this->GetDataManagerSelection();
  for( int i=0; i<nodes.size(); i++)
    nodes.at(i)->SetSelected(false);

  m_SelectedFiducial = node;

  QString name = QString("Fiducial_%1").arg(children->size());
  node->SetName(name.toStdString());
  node->SetSelected(true);

  this->DisableCrosshairNavigation();

  mitk::PlanarFigureInteractor::Pointer figureInteractor = dynamic_cast<mitk::PlanarFigureInteractor*>(node->GetDataInteractor().GetPointer());
  if(figureInteractor.IsNull())
  {
    figureInteractor = mitk::PlanarFigureInteractor::New();
    us::Module* planarFigureModule = us::ModuleRegistry::GetModule( "MitkPlanarFigure" );
    figureInteractor->LoadStateMachine("PlanarFigureInteraction.xml", planarFigureModule );
    figureInteractor->SetEventConfig( "PlanarFigureConfig.xml", planarFigureModule );
    figureInteractor->SetDataNode( node );
  }

  UpdateGui();
  GetDataStorage()->Add(node, m_SelectedBundles.at(0));
}

bool QmitkFiberGenerationView::CompareLayer(mitk::DataNode::Pointer i,mitk::DataNode::Pointer j)
{
  int li = -1;
  i->GetPropertyValue("layer", li);
  int lj = -1;
  j->GetPropertyValue("layer", lj);
  return li<lj;
}

void QmitkFiberGenerationView::GenerateFibers()
{
  if (m_SelectedBundles.empty())
  {
    if (m_SelectedFiducial.IsNull())
      return;

    mitk::DataStorage::SetOfObjects::ConstPointer parents = GetDataStorage()->GetSources(m_SelectedFiducial);
    for( mitk::DataStorage::SetOfObjects::const_iterator it = parents->begin(); it != parents->end(); ++it )
      if(dynamic_cast<mitk::FiberBundle*>((*it)->GetData()))
        m_SelectedBundles.push_back(*it);

    if (m_SelectedBundles.empty())
      return;
  }

  UpdateParametersFromGui();

  for (unsigned int i=0; i<m_SelectedBundles.size(); i++)
  {
    mitk::DataStorage::SetOfObjects::ConstPointer children = GetDataStorage()->GetDerivations(m_SelectedBundles.at(i));
    std::vector< mitk::DataNode::Pointer > childVector;
    for( mitk::DataStorage::SetOfObjects::const_iterator it = children->begin(); it != children->end(); ++it )
      childVector.push_back(*it);
    std::sort(childVector.begin(), childVector.end(), CompareLayer);

    std::vector< mitk::PlanarEllipse::Pointer > fib;
    std::vector< unsigned int > flip;
    float radius = 1;
    int count = 0;
    for( std::vector< mitk::DataNode::Pointer >::const_iterator it = childVector.begin(); it != childVector.end(); ++it )
    {
      mitk::DataNode::Pointer node = *it;

      if ( node.IsNotNull() && dynamic_cast<mitk::PlanarEllipse*>(node->GetData()) )
      {
        mitk::PlanarEllipse* ellipse = dynamic_cast<mitk::PlanarEllipse*>(node->GetData());
        if (m_Controls->m_ConstantRadiusBox->isChecked())
        {
          ellipse->SetTreatAsCircle(true);
          mitk::Point2D c = ellipse->GetControlPoint(0);
          mitk::Point2D p = ellipse->GetControlPoint(1);
          mitk::Vector2D v = p-c;
          if (count==0)
          {
            radius = v.GetVnlVector().magnitude();
            ellipse->SetControlPoint(1, p);
            ellipse->Modified();
          }
          else
          {
            v.Normalize();
            v *= radius;
            ellipse->SetControlPoint(1, c+v);
            ellipse->Modified();
          }
        }
        fib.push_back(ellipse);

        std::map<mitk::DataNode*, QmitkPlanarFigureData>::iterator it = m_DataNodeToPlanarFigureData.find(node.GetPointer());
        if( it != m_DataNodeToPlanarFigureData.end() )
        {
          QmitkPlanarFigureData& data = it->second;
          flip.push_back(data.m_Flipped);
        }
        else
          flip.push_back(0);
      }
      count++;
    }
    if (fib.size()>1)
    {
      m_Parameters.m_FiberGen.m_Fiducials.push_back(fib);
      m_Parameters.m_FiberGen.m_FlipList.push_back(flip);
    }
    else if (fib.size()>0)
      m_SelectedBundles.at(i)->SetData( mitk::FiberBundle::New() );

    mitk::RenderingManager::GetInstance()->RequestUpdateAll();
  }

  itk::FibersFromPlanarFiguresFilter::Pointer filter = itk::FibersFromPlanarFiguresFilter::New();
  filter->SetParameters(m_Parameters.m_FiberGen);
  filter->Update();
  std::vector< mitk::FiberBundle::Pointer > fiberBundles = filter->GetFiberBundles();

  for (unsigned int i=0; i<fiberBundles.size(); i++)
  {
    m_SelectedBundles.at(i)->SetData( fiberBundles.at(i) );
    if (fiberBundles.at(i)->GetNumFibers()>50000)
      m_SelectedBundles.at(i)->SetVisibility(false);
  }

  mitk::RenderingManager::GetInstance()->RequestUpdateAll();
}

void QmitkFiberGenerationView::ApplyTransform()
{
  std::vector< mitk::DataNode::Pointer > selectedBundles;
  for(unsigned int i=0; i<m_SelectedImages.size(); i++ )
  {
    mitk::DataStorage::SetOfObjects::ConstPointer derivations = GetDataStorage()->GetDerivations(m_SelectedImages.at(i));
    for( mitk::DataStorage::SetOfObjects::const_iterator it = derivations->begin(); it != derivations->end(); ++it )
    {
      mitk::DataNode::Pointer fibNode = *it;
      if ( fibNode.IsNotNull() && dynamic_cast<mitk::FiberBundle*>(fibNode->GetData()) )
        selectedBundles.push_back(fibNode);
    }
  }
  if (selectedBundles.empty())
    selectedBundles = m_SelectedBundles2;

  if (!selectedBundles.empty())
  {
    for (std::vector<mitk::DataNode::Pointer>::const_iterator it = selectedBundles.begin(); it!=selectedBundles.end(); ++it)
    {
      mitk::FiberBundle::Pointer fib = dynamic_cast<mitk::FiberBundle*>((*it)->GetData());
      fib->RotateAroundAxis(m_Controls->m_XrotBox->value(), m_Controls->m_YrotBox->value(), m_Controls->m_ZrotBox->value());
      fib->TranslateFibers(m_Controls->m_XtransBox->value(), m_Controls->m_YtransBox->value(), m_Controls->m_ZtransBox->value());
      fib->ScaleFibers(m_Controls->m_XscaleBox->value(), m_Controls->m_YscaleBox->value(), m_Controls->m_ZscaleBox->value());

      // handle child fiducials
      if (m_Controls->m_IncludeFiducials->isChecked())
      {
        mitk::DataStorage::SetOfObjects::ConstPointer derivations = GetDataStorage()->GetDerivations(*it);
        for( mitk::DataStorage::SetOfObjects::const_iterator it2 = derivations->begin(); it2 != derivations->end(); ++it2 )
        {
          mitk::DataNode::Pointer fiducialNode = *it2;
          if ( fiducialNode.IsNotNull() && dynamic_cast<mitk::PlanarEllipse*>(fiducialNode->GetData()) )
          {
            mitk::PlanarEllipse* pe = dynamic_cast<mitk::PlanarEllipse*>(fiducialNode->GetData());
            mitk::BaseGeometry* geom = pe->GetGeometry();

            // translate
            mitk::Vector3D world;
            world[0] = m_Controls->m_XtransBox->value();
            world[1] = m_Controls->m_YtransBox->value();
            world[2] = m_Controls->m_ZtransBox->value();
            geom->Translate(world);

            // calculate rotation matrix
            double x = m_Controls->m_XrotBox->value()*itk::Math::pi/180;
            double y = m_Controls->m_YrotBox->value()*itk::Math::pi/180;
            double z = m_Controls->m_ZrotBox->value()*itk::Math::pi/180;

            itk::Matrix< double, 3, 3 > rotX; rotX.SetIdentity();
            rotX[1][1] = cos(x);
            rotX[2][2] = rotX[1][1];
            rotX[1][2] = -sin(x);
            rotX[2][1] = -rotX[1][2];

            itk::Matrix< double, 3, 3 > rotY; rotY.SetIdentity();
            rotY[0][0] = cos(y);
            rotY[2][2] = rotY[0][0];
            rotY[0][2] = sin(y);
            rotY[2][0] = -rotY[0][2];

            itk::Matrix< double, 3, 3 > rotZ; rotZ.SetIdentity();
            rotZ[0][0] = cos(z);
            rotZ[1][1] = rotZ[0][0];
            rotZ[0][1] = -sin(z);
            rotZ[1][0] = -rotZ[0][1];

            itk::Matrix< double, 3, 3 > rot = rotZ*rotY*rotX;

            // transform control point coordinate into geometry translation
            geom->SetOrigin(pe->GetWorldControlPoint(0));
            mitk::Point2D cp; cp.Fill(0.0);
            pe->SetControlPoint(0, cp);

            // rotate fiducial
            geom->GetIndexToWorldTransform()->SetMatrix(rot*geom->GetIndexToWorldTransform()->GetMatrix());

            // implicit translation
            mitk::Vector3D trans;
            trans[0] = geom->GetOrigin()[0]-fib->GetGeometry()->GetCenter()[0];
            trans[1] = geom->GetOrigin()[1]-fib->GetGeometry()->GetCenter()[1];
            trans[2] = geom->GetOrigin()[2]-fib->GetGeometry()->GetCenter()[2];
            mitk::Vector3D newWc = rot*trans;
            newWc = newWc-trans;
            geom->Translate(newWc);

            pe->Modified();
          }
        }
      }
    }
  }
  else
  {
    for (unsigned int i=0; i<m_SelectedFiducials.size(); i++)
    {
      mitk::PlanarEllipse* pe = dynamic_cast<mitk::PlanarEllipse*>(m_SelectedFiducials.at(i)->GetData());
      mitk::BaseGeometry* geom = pe->GetGeometry();

      // translate
      mitk::Vector3D world;
      world[0] = m_Controls->m_XtransBox->value();
      world[1] = m_Controls->m_YtransBox->value();
      world[2] = m_Controls->m_ZtransBox->value();
      geom->Translate(world);

      // calculate rotation matrix
      double x = m_Controls->m_XrotBox->value()*itk::Math::pi/180;
      double y = m_Controls->m_YrotBox->value()*itk::Math::pi/180;
      double z = m_Controls->m_ZrotBox->value()*itk::Math::pi/180;
      itk::Matrix< double, 3, 3 > rotX; rotX.SetIdentity();
      rotX[1][1] = cos(x);
      rotX[2][2] = rotX[1][1];
      rotX[1][2] = -sin(x);
      rotX[2][1] = -rotX[1][2];
      itk::Matrix< double, 3, 3 > rotY; rotY.SetIdentity();
      rotY[0][0] = cos(y);
      rotY[2][2] = rotY[0][0];
      rotY[0][2] = sin(y);
      rotY[2][0] = -rotY[0][2];
      itk::Matrix< double, 3, 3 > rotZ; rotZ.SetIdentity();
      rotZ[0][0] = cos(z);
      rotZ[1][1] = rotZ[0][0];
      rotZ[0][1] = -sin(z);
      rotZ[1][0] = -rotZ[0][1];
      itk::Matrix< double, 3, 3 > rot = rotZ*rotY*rotX;

      // transform control point coordinate into geometry translation
      geom->SetOrigin(pe->GetWorldControlPoint(0));
      mitk::Point2D cp; cp.Fill(0.0);
      pe->SetControlPoint(0, cp);

      // rotate fiducial
      geom->GetIndexToWorldTransform()->SetMatrix(rot*geom->GetIndexToWorldTransform()->GetMatrix());
      pe->Modified();
    }
    if (m_Controls->m_RealTimeFibers->isChecked())
      GenerateFibers();
  }
  mitk::RenderingManager::GetInstance()->RequestUpdateAll();
}

void QmitkFiberGenerationView::CopyBundles()
{
  if ( m_SelectedBundles.size()<1 ){
    QMessageBox::information( nullptr, "Warning", "Select at least one fiber bundle!");
    MITK_WARN("QmitkFiberGenerationView") << "Select at least one fiber bundle!";
    return;
  }

  for (std::vector<mitk::DataNode::Pointer>::const_iterator it = m_SelectedBundles.begin(); it!=m_SelectedBundles.end(); ++it)
  {
    // find parent image
    mitk::DataNode::Pointer parentNode;
    mitk::DataStorage::SetOfObjects::ConstPointer parentImgs = GetDataStorage()->GetSources(*it);
    for( mitk::DataStorage::SetOfObjects::const_iterator it2 = parentImgs->begin(); it2 != parentImgs->end(); ++it2 )
    {
      mitk::DataNode::Pointer pImgNode = *it2;
      if ( pImgNode.IsNotNull() && dynamic_cast<mitk::Image*>(pImgNode->GetData()) )
      {
        parentNode = pImgNode;
        break;
      }
    }

    mitk::FiberBundle::Pointer fib = dynamic_cast<mitk::FiberBundle*>((*it)->GetData());
    mitk::FiberBundle::Pointer newBundle = fib->GetDeepCopy();
    QString name((*it)->GetName().c_str());
    name += "_copy";

    mitk::DataNode::Pointer fbNode = mitk::DataNode::New();
    fbNode->SetData(newBundle);
    fbNode->SetName(name.toStdString());
    fbNode->SetVisibility(true);
    if (parentNode.IsNotNull())
      GetDataStorage()->Add(fbNode, parentNode);
    else
      GetDataStorage()->Add(fbNode);

    // copy child fiducials
    if (m_Controls->m_IncludeFiducials->isChecked())
    {
      mitk::DataStorage::SetOfObjects::ConstPointer derivations = GetDataStorage()->GetDerivations(*it);
      for( mitk::DataStorage::SetOfObjects::const_iterator it2 = derivations->begin(); it2 != derivations->end(); ++it2 )
      {
        mitk::DataNode::Pointer fiducialNode = *it2;
        if ( fiducialNode.IsNotNull() && dynamic_cast<mitk::PlanarEllipse*>(fiducialNode->GetData()) )
        {
          mitk::PlanarEllipse::Pointer pe = dynamic_cast<mitk::PlanarEllipse*>(fiducialNode->GetData())->Clone();
          mitk::DataNode::Pointer newNode = mitk::DataNode::New();
          newNode->SetData(pe);
          newNode->SetName(fiducialNode->GetName());
          newNode->SetBoolProperty("planarfigure.3drendering", true);
          GetDataStorage()->Add(newNode, fbNode);

        }
      }
    }
  }
  mitk::RenderingManager::GetInstance()->RequestUpdateAll();
}

void QmitkFiberGenerationView::JoinBundles()
{
  if ( m_SelectedBundles.size()<2 ){
    QMessageBox::information( nullptr, "Warning", "Select at least two fiber bundles!");
    MITK_WARN("QmitkFiberGenerationView") << "Select at least two fiber bundles!";
    return;
  }

  std::vector<mitk::DataNode::Pointer>::const_iterator it = m_SelectedBundles.begin();
  mitk::FiberBundle::Pointer newBundle = dynamic_cast<mitk::FiberBundle*>((*it)->GetData());
  QString name("");
  name += QString((*it)->GetName().c_str());
  ++it;
  for (; it!=m_SelectedBundles.end(); ++it)
  {
    newBundle = newBundle->AddBundle(dynamic_cast<mitk::FiberBundle*>((*it)->GetData()));
    name += "+"+QString((*it)->GetName().c_str());
  }

  mitk::DataNode::Pointer fbNode = mitk::DataNode::New();
  fbNode->SetData(newBundle);
  fbNode->SetName(name.toStdString());
  fbNode->SetVisibility(true);
  GetDataStorage()->Add(fbNode);
  mitk::RenderingManager::GetInstance()->RequestUpdateAll();
}

void QmitkFiberGenerationView::UpdateGui()
{
  m_Controls->m_FiberGenMessage->setVisible(true);
  m_Controls->m_TransformBundlesButton->setEnabled(false);
  m_Controls->m_CopyBundlesButton->setEnabled(false);
  m_Controls->m_GenerateFibersButton->setEnabled(false);
  m_Controls->m_FlipButton->setEnabled(false);
  m_Controls->m_CircleButton->setEnabled(false);
  m_Controls->m_JoinBundlesButton->setEnabled(false);
  m_Controls->m_AlignOnGrid->setEnabled(false);

  // Fiber generation gui
  if (m_SelectedFiducial.IsNotNull())
  {
    m_Controls->m_TransformBundlesButton->setEnabled(true);
    m_Controls->m_FlipButton->setEnabled(true);
    m_Controls->m_AlignOnGrid->setEnabled(true);
  }

  if (m_SelectedImageNode.IsNotNull() || !m_SelectedBundles.empty())
  {
    m_Controls->m_CircleButton->setEnabled(true);
    m_Controls->m_FiberGenMessage->setVisible(false);
  }
  if (m_SelectedImageNode.IsNotNull() && !m_SelectedBundles.empty())
    m_Controls->m_AlignOnGrid->setEnabled(true);

  if (!m_SelectedBundles.empty())
  {
    m_Controls->m_TransformBundlesButton->setEnabled(true);
    m_Controls->m_CopyBundlesButton->setEnabled(true);
    m_Controls->m_GenerateFibersButton->setEnabled(true);

    if (m_SelectedBundles.size()>1)
      m_Controls->m_JoinBundlesButton->setEnabled(true);
  }
}

void QmitkFiberGenerationView::OnSelectionChanged(berry::IWorkbenchPart::Pointer, const QList<mitk::DataNode::Pointer>& nodes)
{
  m_SelectedBundles2.clear();
  m_SelectedImages.clear();
  m_SelectedFiducials.clear();
  m_SelectedFiducial = nullptr;
  m_SelectedBundles.clear();
  m_SelectedImageNode = nullptr;

  // iterate all selected objects, adjust warning visibility
  for( int i=0; i<nodes.size(); i++)
  {
    mitk::DataNode::Pointer node = nodes.at(i);

    if( node.IsNotNull() && dynamic_cast<mitk::Image*>(node->GetData()) )
    {
      m_SelectedImages.push_back(node);
      m_SelectedImageNode = node;
    }
    else if ( node.IsNotNull() && dynamic_cast<mitk::FiberBundle*>(node->GetData()) )
    {
      m_SelectedBundles2.push_back(node);
      if (m_Controls->m_RealTimeFibers->isChecked())
      {
        m_SelectedBundles.push_back(node);
        mitk::FiberBundle::Pointer newFib = dynamic_cast<mitk::FiberBundle*>(node->GetData());
        if (newFib->GetNumFibers()!=m_Controls->m_FiberDensityBox->value())
          GenerateFibers();
      }
      else
        m_SelectedBundles.push_back(node);
    }
    else if ( node.IsNotNull() && dynamic_cast<mitk::PlanarEllipse*>(node->GetData()) )
    {
      m_SelectedFiducials.push_back(node);
      m_SelectedFiducial = node;
      m_SelectedBundles.clear();
      mitk::DataStorage::SetOfObjects::ConstPointer parents = GetDataStorage()->GetSources(node);
      for( mitk::DataStorage::SetOfObjects::const_iterator it = parents->begin(); it != parents->end(); ++it )
      {
        mitk::DataNode::Pointer pNode = *it;
        if ( pNode.IsNotNull() && dynamic_cast<mitk::FiberBundle*>(pNode->GetData()) )
          m_SelectedBundles.push_back(pNode);
      }
    }
  }
  UpdateGui();
}

void QmitkFiberGenerationView::EnableCrosshairNavigation()
{
  if (m_Controls->m_RealTimeFibers->isChecked())
    GenerateFibers();
}

void QmitkFiberGenerationView::DisableCrosshairNavigation()
{
}

void QmitkFiberGenerationView::NodeRemoved(const mitk::DataNode* node)
{
  mitk::DataNode* nonConstNode = const_cast<mitk::DataNode*>(node);
  std::map<mitk::DataNode*, QmitkPlanarFigureData>::iterator it = m_DataNodeToPlanarFigureData.find(nonConstNode);

  if (dynamic_cast<FiberBundle*>(node->GetData()))
  {
    m_SelectedBundles.clear();
    m_SelectedBundles2.clear();
  }
  else if (dynamic_cast<Image*>(node->GetData()))
    m_SelectedImages.clear();

  if( it != m_DataNodeToPlanarFigureData.end() )
  {
    QmitkPlanarFigureData& data = it->second;

    // remove observers
    data.m_Figure->RemoveObserver( data.m_EndPlacementObserverTag );
    data.m_Figure->RemoveObserver( data.m_SelectObserverTag );
    data.m_Figure->RemoveObserver( data.m_StartInteractionObserverTag );
    data.m_Figure->RemoveObserver( data.m_EndInteractionObserverTag );

    m_DataNodeToPlanarFigureData.erase( it );
  }
}

void QmitkFiberGenerationView::NodeAdded( const mitk::DataNode* node )
{
  // add observer for selection in renderwindow
  mitk::PlanarFigure* figure = dynamic_cast<mitk::PlanarFigure*>(node->GetData());
  bool isPositionMarker (false);
  node->GetBoolProperty("isContourMarker", isPositionMarker);
  if( figure && !isPositionMarker )
  {
    MITK_DEBUG << "figure added. will add interactor if needed.";
    mitk::PlanarFigureInteractor::Pointer figureInteractor
        = dynamic_cast<mitk::PlanarFigureInteractor*>(node->GetDataInteractor().GetPointer());

    mitk::DataNode* nonConstNode = const_cast<mitk::DataNode*>( node );
    if(figureInteractor.IsNull())
    {
      figureInteractor = mitk::PlanarFigureInteractor::New();
      us::Module* planarFigureModule = us::ModuleRegistry::GetModule( "MitkPlanarFigure" );
      figureInteractor->LoadStateMachine("PlanarFigureInteraction.xml", planarFigureModule );
      figureInteractor->SetEventConfig( "PlanarFigureConfig.xml", planarFigureModule );
      figureInteractor->SetDataNode( nonConstNode );
    }

    MITK_DEBUG << "will now add observers for planarfigure";
    QmitkPlanarFigureData data;
    data.m_Figure = figure;

    //        // add observer for event when figure has been placed
    typedef itk::SimpleMemberCommand< QmitkFiberGenerationView > SimpleCommandType;
    //        SimpleCommandType::Pointer initializationCommand = SimpleCommandType::New();
    //        initializationCommand->SetCallbackFunction( this, &QmitkFiberGenerationView::PlanarFigureInitialized );
    //        data.m_EndPlacementObserverTag = figure->AddObserver( mitk::EndPlacementPlanarFigureEvent(), initializationCommand );

    // add observer for event when figure is picked (selected)
    typedef itk::MemberCommand< QmitkFiberGenerationView > MemberCommandType;
    MemberCommandType::Pointer selectCommand = MemberCommandType::New();
    selectCommand->SetCallbackFunction( this, &QmitkFiberGenerationView::PlanarFigureSelected );
    data.m_SelectObserverTag = figure->AddObserver( mitk::SelectPlanarFigureEvent(), selectCommand );

    // add observer for event when interaction with figure starts
    SimpleCommandType::Pointer startInteractionCommand = SimpleCommandType::New();
    startInteractionCommand->SetCallbackFunction( this, &QmitkFiberGenerationView::DisableCrosshairNavigation);
    data.m_StartInteractionObserverTag = figure->AddObserver( mitk::StartInteractionPlanarFigureEvent(), startInteractionCommand );

    // add observer for event when interaction with figure starts
    SimpleCommandType::Pointer endInteractionCommand = SimpleCommandType::New();
    endInteractionCommand->SetCallbackFunction( this, &QmitkFiberGenerationView::EnableCrosshairNavigation);
    data.m_EndInteractionObserverTag = figure->AddObserver( mitk::EndInteractionPlanarFigureEvent(), endInteractionCommand );

    m_DataNodeToPlanarFigureData[nonConstNode] = data;
  }
}

void QmitkFiberGenerationView::PlanarFigureSelected( itk::Object* object, const itk::EventObject& )
{
  mitk::TNodePredicateDataType<mitk::PlanarFigure>::Pointer isPf = mitk::TNodePredicateDataType<mitk::PlanarFigure>::New();

  mitk::DataStorage::SetOfObjects::ConstPointer allPfs = this->GetDataStorage()->GetSubset( isPf );
  for ( mitk::DataStorage::SetOfObjects::const_iterator it = allPfs->begin(); it!=allPfs->end(); ++it)
  {
    mitk::DataNode* node = *it;

    if( node->GetData() == object )
    {
      node->SetSelected(true);
      m_SelectedFiducial = node;
    }
    else
      node->SetSelected(false);
  }
  UpdateGui();
  this->RequestRenderWindowUpdate();
}

void QmitkFiberGenerationView::SetFocus()
{
  m_Controls->m_CircleButton->setFocus();
}


#include "network/network.hpp"
#include "util/debug.hpp"
#include "util/util.hpp"
#include "algorithm/geom_algorithm.hpp"

#include <ogrsf_frmts.h> // C++ API for GDAL
#include <math.h> // Calulating probability
#include <algorithm> // Partial sort copy
 // Partial sort copy

// Data structures for Rtree
#include <boost/geometry/index/rtree.hpp>
#include <boost/function_output_iterator.hpp>

using namespace FMM;
using namespace FMM::CORE;
using namespace FMM::MM;
using namespace FMM::NETWORK;

bool Network::candidate_compare(const Candidate &a, const Candidate &b)
{
  if (a.dist!=b.dist)
  {
    return a.dist<b.dist;
  }
  else
  {
    return a.edge->index<b.edge->index;
  }
}

Network::Network(const std::string &filename,
                 const std::string &id_name,
                 const std::string &source_name,
                 const std::string &target_name)
{
  SPDLOG_INFO("Read network from file {}",filename);
  OGRRegisterAll();
  GDALDataset *poDS = (GDALDataset*) GDALOpenEx(
    filename.c_str(), GDAL_OF_VECTOR, NULL, NULL, NULL );
  if( poDS == NULL ) {
    SPDLOG_CRITICAL("Open dataset failed.");
    exit( 1 );
  }
  OGRLayer  *ogrlayer = poDS->GetLayer(0);
  int NUM_FEATURES = ogrlayer->GetFeatureCount();
  // edges= std::vector<Edge>(NUM_FEATURES);
  // Initialize network edges
  OGRFeatureDefn *ogrFDefn = ogrlayer->GetLayerDefn();
  OGRFeature *ogrFeature;

  // Fetch the field index given field name.
  int id_idx=ogrFDefn->GetFieldIndex(id_name.c_str());
  int source_idx=ogrFDefn->GetFieldIndex(source_name.c_str());
  int target_idx=ogrFDefn->GetFieldIndex(target_name.c_str());
  if (source_idx<0||target_idx<0||id_idx<0) {
    SPDLOG_CRITICAL("Id, source or target column not found");
    GDALClose( poDS );
    std::exit(EXIT_FAILURE);
  }

  if (wkbFlatten(ogrFDefn->GetGeomType()) != wkbLineString)
  {
    SPDLOG_CRITICAL("Geometry type of network is {}, should be linestring",
                    OGRGeometryTypeToName(ogrFDefn->GetGeomType()));
    GDALClose( poDS );
    std::exit(EXIT_FAILURE);
  } else {
    SPDLOG_DEBUG("Geometry type of network is {}",
                OGRGeometryTypeToName(ogrFDefn->GetGeomType()));
  }
  OGRSpatialReference *ogrsr = ogrFDefn->GetGeomFieldDefn(0)->GetSpatialRef();
  if (ogrsr != nullptr) {
    srid = ogrsr->GetEPSGGeogCS();
    if (srid==-1)
    {
      srid= 4326;
      SPDLOG_WARN("SRID is not found, set to 4326 by default");
    } else {
      SPDLOG_DEBUG("SRID is {}",srid);
    }
  } else {
    srid= 4326;
    SPDLOG_WARN("SRID is not found, set to 4326 by default");
  }
  // Read data from shapefile
  EdgeIndex index = 0;
  while( (ogrFeature = ogrlayer->GetNextFeature()) != NULL){
    EdgeID id = ogrFeature->GetFieldAsInteger(id_idx);
    NodeID source = ogrFeature->GetFieldAsInteger(source_idx);
    NodeID target = ogrFeature->GetFieldAsInteger(target_idx);
    OGRGeometry *rawgeometry = ogrFeature->GetGeometryRef();
    LineString geom;
    if (rawgeometry->getGeometryType()==wkbLineString) {
      geom = ogr2linestring((OGRLineString*) rawgeometry);
    } else if (rawgeometry->getGeometryType()==wkbMultiLineString) {
      SPDLOG_TRACE("Feature id {} s {} t {} is multilinestring",
                   id, source, target);
      SPDLOG_TRACE("Read only the first linestring");
      geom = ogr2linestring((OGRMultiLineString*) rawgeometry);
    } else {
      SPDLOG_CRITICAL("Unknown geometry type for feature id {} s {} t {}",
                      id, source, target);
    }
    NodeIndex s_idx,t_idx;
    if (node_map.find(source)==node_map.end()) {
      s_idx = node_id_vec.size();
      node_id_vec.push_back(source);
      node_map.insert({source,s_idx});
      vertex_points.push_back(geom.get_point(0));
    } else {
      s_idx = node_map[source];
    }
    if (node_map.find(target)==node_map.end()) {
      t_idx = node_id_vec.size();
      node_id_vec.push_back(target);
      node_map.insert({target,t_idx});
      int npoints = geom.get_num_points();
      vertex_points.push_back(geom.get_point(npoints-1));
    } else {
      t_idx = node_map[target];
    }
    edges.push_back({index,id,s_idx,t_idx,geom.get_length(),geom});
    edge_map.insert({id,index});
    ++index;
    OGRFeature::DestroyFeature(ogrFeature);
  }
  GDALClose( poDS );
  num_vertices = node_id_vec.size();
  SPDLOG_INFO("Number of edges {} nodes {}",edges.size(),num_vertices);
  SPDLOG_INFO("Field index: id {} source {} target {}",
      id_idx,source_idx,target_idx);
  build_rtree_index();
  SPDLOG_INFO("Read network done.");
}    // Network constructor

int Network::get_node_count() const {
  return node_id_vec.size();
}

int Network::get_edge_count() const {
  return edges.size();
}

// Get the edge vector
const std::vector<Edge> &Network::get_edges() const
{
  return edges;
}

// Get the ID attribute of an edge according to its index
EdgeID Network::get_edge_id(EdgeIndex index) const
{
  return index<edges.size() ? edges[index].id:-1;
}

EdgeIndex Network::get_edge_index(EdgeID id) const {
  return edge_map.at(id);
}

NodeID Network::get_node_id(NodeIndex index) const {
  return index<num_vertices ? node_id_vec[index] : -1;
}

NodeIndex Network::get_node_index(NodeID id) const {
  return node_map.at(id);
}

Point Network::get_node_geom_from_idx(NodeIndex index) const {
  return vertex_points[index];
}

// Construct a Rtree using the vector of edges
void Network::build_rtree_index() {
  // Build an rtree for candidate search
  SPDLOG_DEBUG("Create boost rtree");
  // create some Items
  for (std::size_t i = 0; i < edges.size(); ++i) {
    // create a boost_box
    Edge *edge = &edges[i];
    double x1,y1,x2,y2;
    ALGORITHM::boundingbox_geometry(edge->geom,&x1,&y1,&x2,&y2);
    boost_box b(Point(x1,y1), Point(x2,y2));
    rtree.insert(std::make_pair(b,edge));
  }
  SPDLOG_DEBUG("Create boost rtree done");
}

Traj_Candidates Network::search_tr_cs_knn(Trajectory &trajectory, std::size_t k,
                                          double radius) const {
  return search_tr_cs_knn(trajectory.geom,k,radius);
}

Traj_Candidates Network::search_tr_cs_knn(const LineString &geom, std::size_t k,
                                          double radius) const
{
  int NumberPoints = geom.get_num_points();
  Traj_Candidates tr_cs(NumberPoints);
  unsigned int current_candidate_index = num_vertices;
  for (int i=0; i<NumberPoints; ++i) {
    // SPDLOG_DEBUG("Search candidates for point index {}",i);
    // Construct a bounding boost_box
    double px = geom.get_x(i);
    double py = geom.get_y(i);
    Point_Candidates pcs;
    boost_box b(Point(geom.get_x(i)-radius,geom.get_y(i)-radius),
                Point(geom.get_x(i)+radius,geom.get_y(i)+radius));
    std::vector<Item> temp;
    // Rtree can only detect intersect with a the bounding box of
    // the geometry stored.
    rtree.query(boost::geometry::index::intersects(b),
                std::back_inserter(temp));
    int Nitems = temp.size();
    for (unsigned int j=0; j<Nitems; ++j) {
      // Check for detailed intersection
      // The two edges are all in OGR_linestring
      Edge *edge = temp[j].second;
      double offset;
      double dist;
      double closest_x,closest_y;
      ALGORITHM::linear_referencing(px,py,edge->geom,
                                    &dist,&offset,&closest_x,&closest_y);
      if (dist<=radius) {
        // index, offset, dist, edge, pseudo id, point
        Candidate c = {0,
                       offset,
                       dist,
                       edge,
                       Point(closest_x,closest_y)};
        pcs.push_back(c);
      }
    }
    if (pcs.empty()) {
      return Traj_Candidates();
    }
    // KNN part
    if (pcs.size()<=k) {
      tr_cs[i]=pcs;
    } else {
      tr_cs[i]=Point_Candidates(k);
      std::partial_sort_copy(
        pcs.begin(),pcs.end(),
        tr_cs[i].begin(),tr_cs[i].end(),
        candidate_compare);
    }
    for (int m=0; m<tr_cs[i].size(); ++m) {
      tr_cs[i][m].index = current_candidate_index+m;
    }
    current_candidate_index+=tr_cs[i].size();
    // SPDLOG_TRACE("current_candidate_index {}",current_candidate_index);
  }
  return tr_cs;
}

const LineString &Network::get_edge_geom(int edge_id) const {
  return edges[get_edge_index(edge_id)].geom;
}

LineString Network::complete_path_to_geometry(
  const LineString &traj, const C_Path &complete_path) const
{
  // if (complete_path->empty()) return nullptr;
  LineString line;
  if (complete_path.empty()) return line;
  int Npts = traj.get_num_points();
  int NCsegs = complete_path.size();
  if (NCsegs ==1) {
    double dist;
    double firstoffset;
    double lastoffset;
    const LineString &firstseg = get_edge_geom(complete_path[0]);
    ALGORITHM::linear_referencing(traj.get_x(0),traj.get_y(0),firstseg,
                                  &dist,&firstoffset);
    ALGORITHM::linear_referencing(traj.get_x(Npts-1),traj.get_y(Npts-1),
                                  firstseg,&dist,&lastoffset);
    LineString firstlineseg= ALGORITHM::cutoffseg_unique(firstseg, firstoffset,
                                                         lastoffset);
    append_segs_to_line(&line,firstlineseg,0);
  } else {
    const LineString &firstseg = get_edge_geom(complete_path[0]);
    const LineString &lastseg = get_edge_geom(complete_path[NCsegs-1]);
    double dist;
    double firstoffset;
    double lastoffset;
    ALGORITHM::linear_referencing(traj.get_x(0),traj.get_y(0),firstseg,
                                  &dist,&firstoffset);
    ALGORITHM::linear_referencing(traj.get_x(Npts-1),traj.get_y(Npts-1),
                                  lastseg,&dist,&lastoffset);
    LineString firstlineseg= ALGORITHM::cutoffseg(firstseg, firstoffset, 0);
    LineString lastlineseg= ALGORITHM::cutoffseg(lastseg, lastoffset, 1);
    append_segs_to_line(&line,firstlineseg,0);
    if (NCsegs>2) {
      for(int i=1; i<NCsegs-1; ++i) {
        const LineString &middleseg =  get_edge_geom(complete_path[i]);
        append_segs_to_line(&line,middleseg,1);
      }
    }
    append_segs_to_line(&line,lastlineseg,1);
  }
  return line;
}

const std::vector<Point> &Network::get_vertex_points() const {
  return vertex_points;
}

const Point &Network::get_vertex_point(NodeIndex u) const {
  return vertex_points[u];
}

LineString Network::route2geometry(const std::vector<EdgeID> &path) const
{
  LineString line;
  if (path.empty()) return line;
  // if (complete_path->empty()) return nullptr;
  int NCsegs = path.size();
  for(int i=0; i<NCsegs; ++i) {
    EdgeIndex e = get_edge_index(path[i]);
    const LineString &seg = edges[e].geom;
    if (i==0) {
      append_segs_to_line(&line,seg,0);
    } else {
      append_segs_to_line(&line,seg,1);
    }
  }
  //SPDLOG_DEBUG("Path geometry is {}",line.exportToWkt());
  return line;
}

LineString Network::route2geometry(const std::vector<EdgeIndex> &path) const
{
  LineString line;
  if (path.empty()) return line;
  // if (complete_path->empty()) return nullptr;
  int NCsegs = path.size();
  for(int i=0; i<NCsegs; ++i)
  {
    const LineString &seg = edges[path[i]].geom;
    if (i==0) {
      append_segs_to_line(&line,seg,0);
    } else {
      append_segs_to_line(&line,seg,1);
    }
  }
  //SPDLOG_DEBUG("Path geometry is {}",line.exportToWkt());
  return line;
}

void Network::append_segs_to_line(LineString *line,
                                  const LineString &segs, int offset)
{
  int Npoints = segs.get_num_points();
  for(int i=0; i<Npoints; ++i) {
    if (i>=offset) {
      line->add_point(segs.get_x(i),segs.get_y(i));
    }
  }
}


#pragma once

#include <Eigen/Dense>
#include <rrt/2dplane/ObstacleGrid.hpp>
#include <rrt/2dplane/PlaneStateSpace.hpp>

namespace RRT {

/**
 * @brief A 2d plane with continuous states and discretized obstacles.
 * @details The state space is broken up into a grid with the given discrete
 * height and widths.
 */
class GridStateSpace : public PlaneStateSpace<Eigen::Vector2f> {
public:
    GridStateSpace(float width, float height, int discretizedWidth,
                   int discretizedHeight);

    /**
     * Returns a boolean indicating whether the given point is within bounds and
     * obstacle-free.
     */
    bool stateValid(const Eigen::Vector2f& pt) const;
    bool transitionValid(const Eigen::Vector2f& from,
                         const Eigen::Vector2f& to) const;

    Eigen::Vector2f intermediateState(const Eigen::Vector2f& source,
                                      const Eigen::Vector2f& target,
                                      float minStepSize,
                                      float maxStepSize) const;

    const ObstacleGrid& obstacleGrid() const;
    ObstacleGrid& obstacleGrid();

private:
    ObstacleGrid _obstacleGrid;
};

}  // namespace RRT


//
// file grid_volumes.cc
// David Cosgrove
// AstraZeneca
// 23rd June 2006
//
// Takes a set of OEMolBases and calculates a load of volume stats for them.
// Uses a grid-based method because it's easy. Gaussian methods don't work so
// well for multiple overlaid molecules.

#include <cmath>
#include <iostream>
#include <limits>
#include <vector>

#include <oechem.h>

#include <boost/shared_ptr.hpp>
#include "gtpl_defs.H"
#include "stddefs.H"
#include "AtomSphere.H"
#include "VolumeGrid.H"

using namespace boost;
using namespace std;
using namespace OEChem;
using namespace OESystem;

namespace DACLIB {

float atomic_num_to_rad( int atomic_num );

float VolumeGrid::grid_spacing_ = 0.25F;

// ***************************************************************************
void find_molecule_extremes( OEMolBase *mol , float *blf , float *trb ,
                             vector<float> &atom_rads ) {

  blf[0] = blf[1] = blf[2] = numeric_limits<float>::max();
  trb[0] = trb[1] = trb[2] = -numeric_limits<float>::max();

  float cds[3];
  OEIter<OEAtomBase> atom;

  float shell_b2 = AtomSphere::surface_thickness() / 2.0F;
  for( atom = mol->GetAtoms() ; atom ; ++atom ) {
    float rad = DACLIB::atomic_num_to_rad( atom->GetAtomicNum() );
    atom_rads.push_back( rad );
    rad += shell_b2;
    if( 1 == atom->GetAtomicNum() )
      continue;
    mol->GetCoords( atom , cds );
    if( cds[0] - rad < blf[0] ) blf[0] = cds[0] - rad;
    if( cds[1] - rad < blf[1] ) blf[1] = cds[1] - rad;
    if( cds[2] - rad < blf[2] ) blf[2] = cds[2] - rad;
    if( cds[0] + rad > trb[0] ) trb[0] = cds[0] + rad;
    if( cds[1] + rad > trb[1] ) trb[1] = cds[1] + rad;
    if( cds[2] + rad > trb[2] ) trb[2] = cds[2] + rad;
  }

}

// ***************************************************************************
void find_molecule_extremes( vector<OEMolBase *> &mols , float *blf ,
                             float *trb , vector<vector<float> > &atom_rads ) {

  blf[0 ] = blf[1] = blf[2] = numeric_limits<float>::max();
  trb[0 ] = trb[1] = trb[2] = -numeric_limits<float>::max();

  float tblf[3] , ttrb[3];
  OEIter<OEAtomBase> atom;

  for( int i = 0 , is = mols.size() ; i < is ; ++i ) {
    atom_rads.push_back( vector<float>() );
    find_molecule_extremes( mols[i] , tblf , ttrb , atom_rads.back() );
    if( tblf[0] < blf[0] ) blf[0] = tblf[0];
    if( tblf[1] < blf[1] ) blf[1] = tblf[1];
    if( tblf[2] < blf[2] ) blf[2] = tblf[2];
    if( ttrb[0] > trb[0] ) trb[0] = ttrb[0];
    if( ttrb[1] > trb[1] ) trb[1] = ttrb[1];
    if( ttrb[2] > trb[2] ) trb[2] = ttrb[2];

  }

}

// ********************************************************************
// make a VolumeGrid from the given conformation of the given molecule
VolumeGrid *prepare_mol_grid( OEMolBase *mol ) {

  vector<float> atom_rads;
  float blf[3] , trb[3];

  find_molecule_extremes( mol , blf , trb , atom_rads );

  VolumeGrid *grid = 0;
  grid = new VolumeGrid( blf , trb );

  grid->drop_molecule_in( mol , atom_rads );

  return grid;

}

// ********************************************************************
// make a VolumeGrid from the given conformation of the given molecule
VolumeGrid *prepare_mol_grid( OEMol *mol , int conf_num ) {

  OEIter<OEConfBase> conf = mol->GetConfs();
  for( int i = 0 ; i < conf_num ; ++i ) {
    ++conf;
  }

  return prepare_mol_grid( conf );

}

// *********************************************************************
// prepare a counts grid containing multiple molecules that will be used for
// more than one shape tanimoto calculation.
void prepare_multi_mol_grids( vector<OEMolBase *> &mols ,
                              boost::shared_ptr<VolumeGrid> &solid_grid ) {

  vector<VolumeGrid *> grids;
  for( int i = 0 , is = mols.size() ; i < is ; ++i )
    grids.push_back( prepare_mol_grid( mols[i] ) );

  solid_grid.reset( new VolumeGrid( grids , 1 ) );

  for( int i = 0 , is = mols.size() ; i < is ; ++i ) {
    delete grids[i];
  }

  solid_grid->counts_to_flag( 1 , GtplDefs::CORE );

}

} // end of namespace


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

#ifndef COMPARE_H
#define COMPARE_H

#include <Eigen/Core>

namespace flexiblesusy {

template<class Real, int N>
struct Compare {
    Compare(const Eigen::Array<Real, N, 1>& s_) : s(s_) {}
    bool operator() (int i, int j) { return s[i] < s[j]; }
    const Eigen::Array<Real, N, 1>& s;
};

template<class Real, int N>
struct CompareAbs {
    CompareAbs(const Eigen::Array<Real, N, 1>& w_) : w(w_) {}
    bool operator() (int i, int j) { return std::abs(w[i]) < std::abs(w[j]); }
    const Eigen::Array<Real, N, 1>& w;
};

template <class T>
struct Is_not_finite {
   bool operator()(T x) { return !std::isfinite(x); }
};

} // namespace flexiblesusy

#endif


/***********************************************************************************************************************
*  OpenStudio(R), Copyright (c) 2008-2020, Alliance for Sustainable Energy, LLC, and other contributors. All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
*  following conditions are met:
*
*  (1) Redistributions of source code must retain the above copyright notice, this list of conditions and the following
*  disclaimer.
*
*  (2) Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
*  disclaimer in the documentation and/or other materials provided with the distribution.
*
*  (3) Neither the name of the copyright holder nor the names of any contributors may be used to endorse or promote products
*  derived from this software without specific prior written permission from the respective party.
*
*  (4) Other than as required in clauses (1) and (2), distributions in any form of modifications or other derivative works
*  may not use the "OpenStudio" trademark, "OS", "os", or any other confusingly similar designation without specific prior
*  written permission from Alliance for Sustainable Energy, LLC.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDER(S) AND ANY CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
*  INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER(S), ANY CONTRIBUTORS, THE UNITED STATES GOVERNMENT, OR THE UNITED
*  STATES DEPARTMENT OF ENERGY, NOR ANY OF THEIR EMPLOYEES, BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
*  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
*  USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
*  STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
*  ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
***********************************************************************************************************************/

#ifndef UTILITIES_DATA_TIMESERIES_HPP
#define UTILITIES_DATA_TIMESERIES_HPP

#include "../UtilitiesAPI.hpp"

#include "Vector.hpp"
#include "../time/Date.hpp"
#include "../time/Time.hpp"
#include "../time/DateTime.hpp"

#include <boost/optional.hpp>
#include <boost/function.hpp>

#include <vector>

namespace openstudio{

namespace detail{

class UTILITIES_API TimeSeries_Impl
{
public:

  TimeSeries_Impl();

  TimeSeries_Impl(const Date& startDate, const Time& intervalLength, const Vector& values, const std::string& units);

  TimeSeries_Impl(const DateTime& firstReportDateTime, const Time& intervalLength, const Vector& values, const std::string& units);

  TimeSeries_Impl(const DateTime& firstReportDateTime, const Vector& timeInDays, const Vector& values, const std::string& units);

  TimeSeries_Impl(const DateTime& firstReportDateTime, const std::vector<double>& timeInDays, const std::vector<double>& values, const std::string& units);

  TimeSeries_Impl(const DateTimeVector& dateTimes, const Vector& values, const std::string& units);

  TimeSeries_Impl(const DateTime& firstReportDateTime, const std::vector<long>& timeInSeconds, const Vector& values, const std::string& units);

  ~TimeSeries_Impl() {}

  openstudio::OptionalTime intervalLength() const;

  openstudio::DateTime firstReportDateTime() const;

  DateTimeVector dateTimes() const;

  openstudio::Vector daysFromFirstReport() const;

  double daysFromFirstReport(const unsigned& i) const;

  std::vector<long> secondsFromFirstReport() const;

  long secondsFromFirstReport(const unsigned& i) const;

  openstudio::Vector values() const;

  double values(const unsigned& i) const;

  const std::string units() const;

  double valueAtSecondsFromFirstReport(long secondsFromFirstReport) const;

  double value(double daysFromFirstReport) const;

  double value(const Time& timeFromFirstReport) const;

  double value(const DateTime& dateTime) const;

  Vector values(const DateTime& startDateTime, const DateTime& endDateTime) const;

  double outOfRangeValue() const;

  void setOutOfRangeValue(double value);

  std::shared_ptr<TimeSeries_Impl> operator+(const TimeSeries_Impl& other) const;

  std::shared_ptr<TimeSeries_Impl> operator-(const TimeSeries_Impl& other) const;

  std::shared_ptr<TimeSeries_Impl> operator*(double d) const;

  double integrate() const;

  double averageValue() const;

private:

  REGISTER_LOGGER("utilities.TimeSeries_Impl");
  // fully qualified first report date
  DateTime m_firstReportDateTime;

  // start date and time of time series
  DateTime m_startDateTime;

  // integer seconds from first report date time, used for quick interpolation
  std::vector<long> m_secondsFromFirstReport;
  Vector m_secondsFromFirstReportAsVector; // same as m_secondsFromFirstReport but stored as Vector
  std::vector<long> m_secondsFromStart;

  // values reported at m_dateTimes
  Vector m_values;

  // units of the values
  std::string m_units;

  // length of the reporting interval if known, can be used to speed up interpolation
  OptionalTime m_intervalLength;

  // value used for out of range data
  double m_outOfRangeValue;

  // true if the time series should support wrap around dates, e.g. 4/11-4/10 without specific year
  bool m_wrapAround;
};
} // detail

/** TimeSeries is a series of values each reported at a single time.  We follow the EnergyPlus
 *   convention that the time reported for each value is at the end of the reporting interval.  For example, if a value
 *   is measured over the interval from hour 1 (non-inclusive) to hour 2 (inclusive), that is 1 < t <= 2, and the reported
 *   value (either the average, median, min, max, etc) is determined to be 7, then the value 7 is reported at hour 2.  One
 *   of the primary tasks of the TimeSeries class is to get the value of the series at an arbitrary time.  For the example above,
 *   any time in the interval 1 < t <= 2 will return 7.  We refer to the beginning of the first reporting interval as the
 *   startDateTime.  The end of the first interval is referred to as the firstReportDateTime.
 *
 *   Todo: add method to mark TimeSeries that represent point in time measurements rather than interval measurements
 *
 *   Todo: add calendar to deal with Daylight Savings
 **/
class UTILITIES_API TimeSeries
{
public:
  /** @name Constructors */
  //@{

  /// Default constructor with no data.
  TimeSeries();

  /** Constructor from start date, interval length, values, and units.
   *  First reporting interval starts at 0 hours on startDate and ends at startDate + intervalLength. */
  TimeSeries(const Date& startDate, const Time& intervalLength, const Vector& values, const std::string& units);

  /** Constructor from first report date and time, interval length, values, and units.
   *  First reporting interval starts at firstReportDateTime - intervalLength and ends at firstReportDateTime. */
  TimeSeries(const DateTime& firstReportDateTime, const Time& intervalLength, const Vector& values, const std::string& units);

  /** Constructor from first report date and time, time in days, values, and units.
   *  The treatment of the time vector depends upon the value of the first element:
   *    - If timeInDays[0] == 0 then the first reporting interval starts at 0 hours on the firstReportDateTime's date and ends at firstReportDateTime.
   *    - If timeInDays[0] > 0 then the first reporting interval starts at firstReportDateTime - timeInDays[0] and ends at firstReportDateTime.
   *
   *  An exception is thrown if:
   *    - timeInDays.size != values.size
   *    - timeInDays is not monotonically increasing
   *    - the start date and time of first reporting interval cannot be determined */
  TimeSeries(const DateTime& firstReportDateTime, const Vector& timeInDays, const Vector& values, const std::string& units);

  /** Constructor from first report date and time, time in days, values, and units.
   *  The treatment of the time vector depends upon the value of the first element:
   *    - If timeInDays[0] == 0 then the first reporting interval starts at 0 hours on the firstReportDateTime's date and ends at firstReportDateTime.
   *    - If timeInDays[0] > 0 then the first reporting interval starts at firstReportDateTime - timeInDays[0] and ends at firstReportDateTime.
   *
   *  An exception is thrown if:
   *    - timeInDays.size != values.size
   *    - timeInDays is not monotonically increasing
   *    - the start date and time of first reporting interval cannot be determined */
  TimeSeries(const DateTime& firstReportDateTime, const std::vector<double>& timeInDays, const std::vector<double>& values, const std::string& units);

  /** Constructor from date times, values, and units.
   *  The size of the dateTimes vector determines how it is handled:
   *    - If dateTimes.size == values.size then the first reporting interval starts at 0 hours on dateTimes[0]'s date and ends at dateTimes[0].
   *    - If dateTimes.size == values.size + 1 then the first reporting interval starts at dateTimes[0] and ends at dateTimes[1].
   *
   * An exception is thrown if:
   *   - (timeInDays.size != values.size) and (timeInDays.size != values.size + 1)
   *   - the start date and time of first reporting interval cannot be determined */
  // DLM: this is the constructor that would be used for point in time measurements
  TimeSeries(const DateTimeVector& dateTimes, const Vector& values, const std::string& units);

  /** Constructor from first report date and time, time in seconds, values, and units.
   *  The treatment of the time vector depends upon the value of the first element:
   *    - If timeInSeconds[0] == 0 then the first reporting interval starts at 0 hours on the firstReportDateTime's date and ends at firstReportDateTime.
   *    - If timeInSeconds[0] > 0 then the first reporting interval starts at firstReportDateTime - timeInSeconds[0] and ends at firstReportDateTime.
   *
   * An exception is thrown if:
   *   - timeInSeconds.size != values.size
   *   - timeInSeconds is not monotonically increasing
   *   - start date and time of first reporting interval cannot be determined */
  TimeSeries(const DateTime& firstReportDateTime, const std::vector<long>& timeInSeconds, const Vector& values, const std::string& units);

  /// Virtual destructor
  ~TimeSeries() {}

  //@}
  /** @name Getters */
  //@{

  /// Returns the interval length if any
  openstudio::OptionalTime intervalLength() const;

  /// Returns the date and times at which values are reported, these are the end of each reporting interval
  openstudio::DateTimeVector dateTimes() const;

  /// Returns the date and time of first report value
  openstudio::DateTime firstReportDateTime() const;

  /// Returns the vector of time in days from end of the first reporting interval
  openstudio::Vector daysFromFirstReport() const;

  /// Returns the time in days from end of the first reporting interval at index i to prevent implicit vector copy for single value
  double daysFromFirstReport(const unsigned& i) const;

  /// Returns the time in seconds from end of the first reporting interval
  std::vector<long> secondsFromFirstReport() const;

  /// Return the time in seconds from end of the first reporting interval at index i to prevent implicit vector copy for single value
  long secondsFromFirstReport(const unsigned& i) const;

  /// Returns the values vector
  openstudio::Vector values() const;

  /// Returns the value at index i to prevent implicit vector copy for single value
  double values(const unsigned& i) const;

  /// Returns the series units as a standard string
  const std::string units() const;

  /// Get value at number of days from first report date and time
  double value(double daysFromFirstReport) const;

  /// Get value at time from first report date and time
  double value(const Time& timeFromFirstReport) const;

  /// Get value at date and time
  double value(const DateTime& dateTime) const;

  /// Get values between start and end date times
  Vector values(const DateTime& startDateTime, const DateTime& endDateTime) const;

  /// Get the value used for out of range data
  double outOfRangeValue() const;

  //@}
  /** @name Setters */
  //@{

  /// Set the value used for out of range data, defaults to 0
  void setOutOfRangeValue(double value);

  //@}
  /** @name Operators */
  //@{

  /// Add timeseries
  TimeSeries operator+(const TimeSeries& other) const;

  /// Subtract timeseries
  TimeSeries operator-(const TimeSeries& other) const;

  /** TimeSeries * double */
  TimeSeries operator*(double d) const;

  /** TimeSeries / double */
  TimeSeries operator/(double d) const;

  //@}
  /** @name Analysis Functions */
  //@{

  /** Integrate the time series */
  double integrate() const;

  /** Compute the time series average value */
  double averageValue() const;

  //@}
private:

  REGISTER_LOGGER("utilities.TimeSeries");
  // constructor from impl
  TimeSeries(std::shared_ptr<detail::TimeSeries_Impl> impl);

  // pointer to impl
  std::shared_ptr<detail::TimeSeries_Impl> m_impl;
};

// optional TimeSeries
typedef boost::optional<TimeSeries> OptionalTimeSeries;

// vector of TimeSeries
typedef std::vector<TimeSeries> TimeSeriesVector;

/** double * TimeSeries */
UTILITIES_API TimeSeries operator*(double d, const TimeSeries& series);

// ETH@20100910 No implementation of double/TimeSeries yet because that would change the units.
// We should be able to tackle double/TimeSeries after adding get/setQuantity to
// IdfObject.

// Helper function to add up all the TimeSeries in timeSeriesVector.
UTILITIES_API TimeSeries sum(const std::vector<TimeSeries>& timeSeriesVector);

/** Returns std::function pointer to sum(const std::vector<TimeSeries>&). */
UTILITIES_API boost::function1<TimeSeries, const std::vector<TimeSeries>&> sumTimeSeriesFunctor();

/** Evaluates functor(timeSeriesVector). For use in SWIG bindings. */
UTILITIES_API TimeSeries evaluateTimeSeriesFromTimeSeriesVectorFunctor(
  const boost::function1<TimeSeries, const std::vector<TimeSeries>&>& functor,
  const std::vector<TimeSeries>& timeSeriesVector);

} // openstudio

#endif // UTILITIES_DATA_TIMESERIES_HPP


#include <freespace/freespace.h>
#include <freespace/freespace_util.h>
#include <geometry_msgs/Vector3.h>
#include <ros/publisher.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Float64.h>
#include <tf/transform_datatypes.h>
#include <Eigen/Dense>
#include <vector>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "imu");

  ros::NodeHandle nh;
  ros::NodeHandle pNh("~");

  double yaw_offset;
  pNh.param("yaw_offset", yaw_offset, 0.0);

  ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("/imu", 1000);

  ros::Publisher raw_mag_pub = nh.advertise<geometry_msgs::Vector3>("/mag_raw", 1000);
  ros::Publisher raw_yaw_pub = nh.advertise<std_msgs::Float64>("/raw_yaw", 1);
  ros::Publisher raw_imu_pub = nh.advertise<sensor_msgs::Imu>("/imu_raw_accel", 1000);

  int ret = freespace_init();
  if (ret != FREESPACE_SUCCESS)
  {
    ROS_ERROR_STREAM("Failed to init IMU");
    return 0;
  }

  FreespaceDeviceId ids[5];
  int num_found = 0;
  ret = freespace_getDeviceList(ids, 5, &num_found);
  if (num_found <= 0)
  {
    ROS_ERROR_STREAM("failed to connect to IMU found no devices");
    return 0;
  }
  ROS_INFO_STREAM("number of devices = " << num_found);

  ret = freespace_openDevice(ids[0]);
  if (ret != FREESPACE_SUCCESS)
  {
    ROS_ERROR_STREAM("failure to connect to device " << ret);
  }

  int seq = 0;  // sequence of published messgaes - should be monatomicly increasing

  freespace_message request;
  request.messageType = FREESPACE_MESSAGE_DATAMODECONTROLV2REQUEST;
  request.dataModeControlV2Request.operatingStatus = 0;  // 0 to change 1 to read
  request.dataModeControlV2Request.outputStatus = 0;     // 0 to change 1 to read
  request.dataModeControlV2Request.mode = 4;             // Full Motion On
  request.dataModeControlV2Request.packetSelect = 8;     // MotionEngine Output
  request.dataModeControlV2Request.formatSelect = 0;     // Format 0
  request.dataModeControlV2Request.ff0 = 0;              // Enable Pointer
  request.dataModeControlV2Request.ff1 = 1;              // Enable Linear Acceleration
  request.dataModeControlV2Request.ff2 = 1;              // Enable Linear Acceleration, No Gravity
  request.dataModeControlV2Request.ff3 = 1;              // Enable Angular Velocity
  request.dataModeControlV2Request.ff4 = 1;              // Enable Magnetometer
  request.dataModeControlV2Request.ff5 = 0;              // Enable Temperature
  request.dataModeControlV2Request.ff6 = 1;              // Enable Angular Position (WXYZ)
  request.dataModeControlV2Request.ff7 = 0;              // Nothing
  ret = freespace_sendMessage(ids[0], &request);
  if (ret != FREESPACE_SUCCESS)
  {
    ROS_ERROR_STREAM("failed to send message");
  }

  freespace_message response;
  ret = freespace_readMessage(ids[0], &response, 1000);
  if (ret != FREESPACE_SUCCESS)
  {
    if (ret == FREESPACE_ERROR_TIMEOUT)
    {
      ROS_ERROR_STREAM("failed to read IMU TIMEOUT");
    }
    else
    {
      ROS_ERROR_STREAM("failed to read IMU" << ret);
    }
  }
  if (response.messageType == FREESPACE_MESSAGE_DATAMODECONTROLV2RESPONSE)
  {
    ROS_INFO_STREAM("got response" << response.dataModeControlV2Response.modeActual);
  }
  else
  {
    ROS_ERROR_STREAM("got invalid type " << response.messageType);
  }

  while (ros::ok())
  {
    ros::spinOnce();

    ret = freespace_readMessage(ids[0], &response, 1000);
    if (ret != FREESPACE_SUCCESS)
    {
      if (ret == FREESPACE_ERROR_TIMEOUT)
      {
        ROS_ERROR_STREAM("failed to read IMU TIMEOUT");
      }
      else
      {
        ROS_ERROR_STREAM("failed to read IMU" << ret);
      }
    }

    if (response.messageType != FREESPACE_MESSAGE_MOTIONENGINEOUTPUT)
    {
      ROS_ERROR_STREAM("error got unknown message type " << response.messageType);
    }

    MultiAxisSensor accel_msg;
    MultiAxisSensor accel_raw_msg;

    ret = freespace_util_getAccNoGravity(&response.motionEngineOutput, &accel_msg);
    ret = freespace_util_getAcceleration(&response.motionEngineOutput, &accel_raw_msg);
    if (ret != FREESPACE_SUCCESS)
    {
      ROS_ERROR_STREAM("failed to read acceleration " << ret);
    }
    Eigen::Vector3d accel_vec(-accel_msg.x, accel_msg.y, accel_msg.z);
    Eigen::Vector3d accel_raw_vec(-accel_raw_msg.x, accel_raw_msg.y, accel_raw_msg.z);
    Eigen::Matrix3d T;
    T << cos(-M_PI / 2), -sin(-M_PI / 2), 0, sin(-M_PI / 2), cos(-M_PI / 2), 0, 0, 0, 1;
    Eigen::Vector3d rotated_accel = T * accel_vec;
    Eigen::Vector3d rotated_raw_accel = T * accel_raw_vec;

    MultiAxisSensor orientation_msg;
    ret = freespace_util_getAngPos(&response.motionEngineOutput, &orientation_msg);
    if (ret != FREESPACE_SUCCESS)
    {
      ROS_ERROR_STREAM("failed to read angular position " << ret);
    }

    MultiAxisSensor angular_vel_msg;
    ret = freespace_util_getAngularVelocity(&response.motionEngineOutput, &angular_vel_msg);
    if (ret != FREESPACE_SUCCESS)
    {
      ROS_ERROR_STREAM("failed to read angular velocity " << ret);
    }
    Eigen::Matrix3d Tz;
    Eigen::Matrix3d Tx;
    Eigen::Matrix3d Ty;
    Eigen::Matrix3d Tt;
    double rollT, pitchT, yawT;
    rollT = 0;
    pitchT = 0;
    yawT = 0;
    Tz << cos(rollT), -sin(rollT), 0, sin(rollT), cos(rollT), 0, 0, 0, 1;
    Ty << cos(pitchT), 0, sin(pitchT), 0, 1, 0, -sin(pitchT), 0, cos(pitchT);
    Tx << 1, 0, 0, 0, cos(yawT), -sin(yawT), 0, sin(yawT), cos(yawT);
    Tt = Ty;
    Eigen::Vector3d angular_vel_vec(angular_vel_msg.x, angular_vel_msg.y, -angular_vel_msg.z);
    Eigen::Vector3d rotated_angular_vel = angular_vel_vec;

    MultiAxisSensor magnetometer_msg;
    ret = freespace_util_getMagnetometer(&response.motionEngineOutput, &magnetometer_msg);
    if (ret != FREESPACE_SUCCESS)
    {
      ROS_ERROR_STREAM("failed to read raw magnetometer velocity " << ret);
    }

    // declare the message to be published
    sensor_msgs::Imu msg;
    msg.header.frame_id = "imu";
    msg.header.stamp = ros::Time::now();
    msg.header.seq = seq++;

    msg.linear_acceleration.x = rotated_accel[0];
    msg.linear_acceleration.y = rotated_accel[1];
    msg.linear_acceleration.z = rotated_accel[2];
    msg.linear_acceleration_covariance = { 0.01, 1e-6, 1e-6, 1e-6, 0.01, 1e-6, 1e-6, 1e-6, 0.01 };

    msg.angular_velocity.x = rotated_angular_vel[0];
    msg.angular_velocity.y = rotated_angular_vel[1];
    msg.angular_velocity.z = rotated_angular_vel[2];
    msg.angular_velocity_covariance = { 0.02, 1e-6, 1e-6, 1e-6, 0.02, 1e-6, 1e-6, 1e-6, 0.02 };

    tf::Quaternion pre_rotated(orientation_msg.x, orientation_msg.y, orientation_msg.z, orientation_msg.w);

    double roll, pitch, yaw;
    tf::Matrix3x3(pre_rotated).getRPY(roll, pitch, yaw);
    ROS_INFO_STREAM("prerotated " << roll << " " << pitch << " " << yaw);
    tf::Quaternion rotated = tf::createQuaternionFromRPY(3.14159 / 2, 0, 0) * pre_rotated;
    rotated = rotated.normalize();

    tf::Matrix3x3(rotated).getRPY(roll, pitch, yaw);
    ROS_INFO_STREAM("rotated = " << roll << " " << pitch << " " << yaw);
    tf::Matrix3x3(pre_rotated).getRPY(roll, pitch, yaw);
    yaw = -yaw;
    geometry_msgs::Quaternion orientation;
    orientation.x = rotated.x();
    orientation.y = rotated.y();
    orientation.z = rotated.z();
    orientation.w = rotated.w();

    msg.orientation = orientation;
    msg.orientation_covariance = { 0.0025, 1E-6, 1E-6, 1E-6, 0.0025, 1E-6, 1E-6, 1E-6, 0.0025 };

    imu_pub.publish(msg);

    sensor_msgs::Imu msg_raw;
    msg_raw.header.frame_id = "imu";
    msg_raw.header.stamp = msg.header.stamp;
    msg_raw.header.seq = seq;

    msg_raw.linear_acceleration.x = rotated_raw_accel[0];
    msg_raw.linear_acceleration.y = rotated_raw_accel[1];
    msg_raw.linear_acceleration.z = rotated_raw_accel[2];
    msg_raw.linear_acceleration_covariance = { 0.005, 1e-6, 1e-6, 1e-6, 0.005, 1e-6, 1e-6, 1e-6, 0.005 };

    msg_raw.angular_velocity.x = rotated_angular_vel[0];
    msg_raw.angular_velocity.y = rotated_angular_vel[1];
    msg_raw.angular_velocity.z = rotated_angular_vel[2];
    msg_raw.angular_velocity_covariance = { 0.02, 1e-6, 1e-6, 1e-6, 0.02, 1e-6, 1e-6, 1e-6, 0.02 };

    tf::Quaternion pre_rotated_raw(orientation_msg.x, orientation_msg.y, orientation_msg.z, orientation_msg.w);
    tf::Quaternion rotation_raw = tf::createQuaternionFromRPY(0, 0, 0);

    tf::Matrix3x3(pre_rotated).getRPY(roll, pitch, yaw);

    tf::Quaternion rotated_raw = rotation_raw * pre_rotated;

    geometry_msgs::Quaternion orientation_raw;
    orientation_raw.x = rotated_raw.x();
    orientation_raw.y = rotated_raw.y();
    orientation_raw.z = rotated_raw.z();
    orientation_raw.w = rotated_raw.w();

    msg_raw.orientation = orientation;
    msg_raw.orientation_covariance = { 0.0025, 1e-6, 1e-6, 1e-6, 0.0025, 1e-6, 1e-6, 1e-6, 0.0025 };

    raw_imu_pub.publish(msg_raw);
  }
  return 0;
}